moveit2
The MoveIt Motion Planning Framework for ROS 2.
interactive_marker_helpers.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan, Acorn Pooley, Adam Leeper */
36 
37 #include <math.h>
38 #include <moveit/robot_interaction/interactive_marker_helpers.h>
39 // TODO: Remove conditional include when released to all active distros.
40 #if __has_include(<tf2/LinearMath/Quaternion.hpp>)
41 #include <tf2/LinearMath/Quaternion.hpp>
42 #else
43 #include <tf2/LinearMath/Quaternion.h>
44 #endif
45 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
46 
47 namespace robot_interaction
48 {
49 visualization_msgs::msg::InteractiveMarker
50 makeEmptyInteractiveMarker(const std::string& name, const geometry_msgs::msg::PoseStamped& stamped, double scale)
51 {
52  visualization_msgs::msg::InteractiveMarker int_marker;
53  int_marker.header = stamped.header;
54  int_marker.name = name;
55  int_marker.scale = scale;
56  int_marker.pose = stamped.pose;
57  return int_marker;
58 }
59 
60 void addTArrowMarker(visualization_msgs::msg::InteractiveMarker& im)
61 {
62  // create an arrow marker
63  visualization_msgs::msg::Marker m;
64  m.type = visualization_msgs::msg::Marker::ARROW;
65  m.scale.x = 0.6 * im.scale;
66  m.scale.y = 0.12 * im.scale;
67  m.scale.z = 0.12 * im.scale;
68  m.ns = "goal_pose_arrow_marker";
69  m.id = 1;
70  m.action = visualization_msgs::msg::Marker::ADD;
71  m.header = im.header;
72  m.pose = im.pose;
73  // Arrow points along Z
74  tf2::Quaternion imq, tmq;
75  tf2::fromMsg(m.pose.orientation, imq);
76  tmq.setRPY(0, -M_PI / 2.0, 0);
77  imq = imq * tmq;
78  m.pose.orientation = tf2::toMsg(imq);
79  m.color.r = 0.0f;
80  m.color.g = 1.0f;
81  m.color.b = 0.0f;
82  m.color.a = 1.0f;
83 
84  visualization_msgs::msg::Marker mc;
85  mc.type = visualization_msgs::msg::Marker::CYLINDER;
86  mc.scale.x = 0.05 * im.scale;
87  mc.scale.y = 0.05 * im.scale;
88  mc.scale.z = 0.15 * im.scale;
89  mc.ns = "goal_pose_arrow_marker";
90  mc.id = 2;
91  mc.action = visualization_msgs::msg::Marker::ADD;
92  mc.header = im.header;
93  mc.pose = im.pose;
94  // Cylinder points along Y
95  tf2::fromMsg(mc.pose.orientation, imq);
96  tmq.setRPY(M_PI / 2.0, 0, 0);
97  imq = imq * tmq;
98  mc.pose.orientation = tf2::toMsg(imq);
99  mc.pose.position.x -= 0.04;
100  mc.pose.position.z += 0.01;
101  mc.color.r = 0.0f;
102  mc.color.g = 1.0f;
103  mc.color.b = 0.0f;
104  mc.color.a = 1.0f;
105 
106  visualization_msgs::msg::InteractiveMarkerControl m_control;
107  m_control.always_visible = true;
108  m_control.interaction_mode = m_control.BUTTON;
109  m_control.markers.push_back(m);
110  m_control.markers.push_back(mc);
111 
112  // add the control to the interactive marker
113  im.controls.push_back(m_control);
114 }
115 
116 void addErrorMarker(visualization_msgs::msg::InteractiveMarker& im)
117 {
118  // create a grey box marker
119  visualization_msgs::msg::Marker err;
120  err.type = visualization_msgs::msg::Marker::MESH_RESOURCE;
121  err.scale.x = 0.002 * im.scale;
122  err.scale.y = 0.002 * im.scale;
123  err.scale.z = 0.002 * im.scale;
124  err.mesh_resource = "package://moveit_ros_planning_interface/resources/access-denied.dae";
125  err.ns = "robot_interaction_error";
126  err.id = 1;
127  err.action = visualization_msgs::msg::Marker::ADD;
128  err.header = im.header;
129  err.pose = im.pose;
130  err.pose.orientation.x = err.pose.orientation.y = 0.7071067811865476;
131  err.pose.orientation.z = err.pose.orientation.w = 0.0;
132  err.color.r = 1.0f;
133  err.color.g = 0.0f;
134  err.color.b = 0.0f;
135  err.color.a = 1.0f;
136 
137  visualization_msgs::msg::InteractiveMarkerControl err_control;
138  err_control.always_visible = false;
139  err_control.markers.push_back(err);
140 
141  // add the control to the interactive marker
142  im.controls.push_back(err_control);
143 }
144 
145 // value for normalized quaternion: 1.0 / std::sqrt(2.0)
146 static const double SQRT2INV = 0.707106781;
147 
148 void addPlanarXYControl(visualization_msgs::msg::InteractiveMarker& int_marker, bool orientation_fixed)
149 {
150  visualization_msgs::msg::InteractiveMarkerControl control;
151 
152  if (orientation_fixed)
153  control.orientation_mode = visualization_msgs::msg::InteractiveMarkerControl::FIXED;
154  control.orientation.w = SQRT2INV;
155  control.orientation.x = SQRT2INV;
156  control.orientation.y = 0;
157  control.orientation.z = 0;
158  control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS;
159  int_marker.controls.push_back(control);
160 
161  control.orientation.w = SQRT2INV;
162  control.orientation.x = 0;
163  control.orientation.y = SQRT2INV;
164  control.orientation.z = 0;
165  control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::ROTATE_AXIS;
166  int_marker.controls.push_back(control);
167 
168  control.orientation.w = SQRT2INV;
169  control.orientation.x = 0;
170  control.orientation.y = 0;
171  control.orientation.z = SQRT2INV;
172  control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS;
173  int_marker.controls.push_back(control);
174 }
175 
176 void add6DOFControl(visualization_msgs::msg::InteractiveMarker& int_marker, bool orientation_fixed)
177 {
178  addOrientationControl(int_marker, orientation_fixed);
179  addPositionControl(int_marker, orientation_fixed);
180 }
181 
182 void addOrientationControl(visualization_msgs::msg::InteractiveMarker& int_marker, bool orientation_fixed)
183 {
184  visualization_msgs::msg::InteractiveMarkerControl control;
185 
186  if (orientation_fixed)
187  control.orientation_mode = visualization_msgs::msg::InteractiveMarkerControl::FIXED;
188  control.orientation.w = SQRT2INV;
189  control.orientation.x = SQRT2INV;
190  control.orientation.y = 0;
191  control.orientation.z = 0;
192  control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::ROTATE_AXIS;
193  int_marker.controls.push_back(control);
194 
195  control.orientation.w = SQRT2INV;
196  control.orientation.x = 0;
197  control.orientation.y = SQRT2INV;
198  control.orientation.z = 0;
199  control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::ROTATE_AXIS;
200  int_marker.controls.push_back(control);
201 
202  control.orientation.w = SQRT2INV;
203  control.orientation.x = 0;
204  control.orientation.y = 0;
205  control.orientation.z = SQRT2INV;
206  control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::ROTATE_AXIS;
207  int_marker.controls.push_back(control);
208 }
209 
210 void addPositionControl(visualization_msgs::msg::InteractiveMarker& int_marker, bool orientation_fixed)
211 {
212  visualization_msgs::msg::InteractiveMarkerControl control;
213 
214  if (orientation_fixed)
215  control.orientation_mode = visualization_msgs::msg::InteractiveMarkerControl::FIXED;
216  control.orientation.w = SQRT2INV;
217  control.orientation.x = SQRT2INV;
218  control.orientation.y = 0;
219  control.orientation.z = 0;
220  control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS;
221  int_marker.controls.push_back(control);
222 
223  control.orientation.w = SQRT2INV;
224  control.orientation.x = 0;
225  control.orientation.y = SQRT2INV;
226  control.orientation.z = 0;
227  control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS;
228  int_marker.controls.push_back(control);
229 
230  control.orientation.w = SQRT2INV;
231  control.orientation.x = 0;
232  control.orientation.y = 0;
233  control.orientation.z = SQRT2INV;
234  control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS;
235  int_marker.controls.push_back(control);
236 }
237 
238 void addViewPlaneControl(visualization_msgs::msg::InteractiveMarker& int_marker, double radius,
239  const std_msgs::msg::ColorRGBA& color, bool position, bool orientation)
240 {
241  visualization_msgs::msg::InteractiveMarkerControl control;
242  control.orientation_mode = visualization_msgs::msg::InteractiveMarkerControl::VIEW_FACING;
243  if (position && orientation)
244  control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::MOVE_ROTATE_3D;
245  else if (orientation)
246  control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::ROTATE_3D;
247  else
248  control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::MOVE_3D;
249  control.independent_marker_orientation = true;
250  control.name = "move";
251 
252  visualization_msgs::msg::Marker marker;
253 
254  marker.type = visualization_msgs::msg::Marker::SPHERE;
255  marker.scale.x = radius * 2.0;
256  marker.scale.y = radius * 2.0;
257  marker.scale.z = radius * 2.0;
258  marker.color = color;
259 
260  control.markers.push_back(marker);
261  control.always_visible = false;
262 
263  int_marker.controls.push_back(control);
264 }
265 
266 visualization_msgs::msg::InteractiveMarker makePlanarXYMarker(const std::string& name,
267  const geometry_msgs::msg::PoseStamped& stamped,
268  double scale, bool orientation_fixed)
269 {
270  visualization_msgs::msg::InteractiveMarker int_marker = makeEmptyInteractiveMarker(name, stamped, scale);
271  addPlanarXYControl(int_marker, orientation_fixed);
272  return int_marker;
273 }
274 
275 visualization_msgs::msg::InteractiveMarker make6DOFMarker(const std::string& name,
276  const geometry_msgs::msg::PoseStamped& stamped, double scale,
277  bool orientation_fixed)
278 {
279  visualization_msgs::msg::InteractiveMarker int_marker = makeEmptyInteractiveMarker(name, stamped, scale);
280  add6DOFControl(int_marker, orientation_fixed);
281  return int_marker;
282 }
283 } // namespace robot_interaction
interactive_marker_helpers.h
robot_interaction::InteractionStyle::FIXED
@ FIXED
Definition: interaction.h:68
robot_interaction
Definition: interaction.h:55
robot_interaction::addViewPlaneControl
void addViewPlaneControl(visualization_msgs::msg::InteractiveMarker &int_marker, double radius, const std_msgs::msg::ColorRGBA &color, bool position=true, bool orientation=true)
Definition: interactive_marker_helpers.cpp:238
robot_interaction::addTArrowMarker
void addTArrowMarker(visualization_msgs::msg::InteractiveMarker &im)
Definition: interactive_marker_helpers.cpp:60
robot_interaction::add6DOFControl
void add6DOFControl(visualization_msgs::msg::InteractiveMarker &int_marker, bool orientation_fixed=false)
Definition: interactive_marker_helpers.cpp:176
robot_interaction::make6DOFMarker
visualization_msgs::msg::InteractiveMarker make6DOFMarker(const std::string &name, const geometry_msgs::msg::PoseStamped &stamped, double scale, bool orientation_fixed=false)
Definition: interactive_marker_helpers.cpp:275
robot_interaction::addOrientationControl
void addOrientationControl(visualization_msgs::msg::InteractiveMarker &int_marker, bool orientation_fixed=false)
Definition: interactive_marker_helpers.cpp:182
robot_interaction::makeEmptyInteractiveMarker
visualization_msgs::msg::InteractiveMarker makeEmptyInteractiveMarker(const std::string &name, const geometry_msgs::msg::PoseStamped &stamped, double scale)
Definition: interactive_marker_helpers.cpp:50
robot_interaction::addErrorMarker
void addErrorMarker(visualization_msgs::msg::InteractiveMarker &im)
Definition: interactive_marker_helpers.cpp:116
robot_interaction::addPlanarXYControl
void addPlanarXYControl(visualization_msgs::msg::InteractiveMarker &int_marker, bool orientation_fixed=false)
Definition: interactive_marker_helpers.cpp:148
robot_interaction::makePlanarXYMarker
visualization_msgs::msg::InteractiveMarker makePlanarXYMarker(const std::string &name, const geometry_msgs::msg::PoseStamped &stamped, double scale, bool orientation_fixed=false)
Definition: interactive_marker_helpers.cpp:266
robot_interaction::addPositionControl
void addPositionControl(visualization_msgs::msg::InteractiveMarker &int_marker, bool orientation_fixed=false)
Definition: interactive_marker_helpers.cpp:210
setup.name
name
Definition: setup.py:7
Generated by doxygen 1.9.1