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>
39 #include <tf2/LinearMath/Quaternion.h>
40 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
41 
42 namespace robot_interaction
43 {
44 visualization_msgs::msg::InteractiveMarker
45 makeEmptyInteractiveMarker(const std::string& name, const geometry_msgs::msg::PoseStamped& stamped, double scale)
46 {
47  visualization_msgs::msg::InteractiveMarker int_marker;
48  int_marker.header = stamped.header;
49  int_marker.name = name;
50  int_marker.scale = scale;
51  int_marker.pose = stamped.pose;
52  return int_marker;
53 }
54 
55 void addTArrowMarker(visualization_msgs::msg::InteractiveMarker& im)
56 {
57  // create an arrow marker
58  visualization_msgs::msg::Marker m;
59  m.type = visualization_msgs::msg::Marker::ARROW;
60  m.scale.x = 0.6 * im.scale;
61  m.scale.y = 0.12 * im.scale;
62  m.scale.z = 0.12 * im.scale;
63  m.ns = "goal_pose_arrow_marker";
64  m.id = 1;
65  m.action = visualization_msgs::msg::Marker::ADD;
66  m.header = im.header;
67  m.pose = im.pose;
68  // Arrow points along Z
69  tf2::Quaternion imq, tmq;
70  tf2::fromMsg(m.pose.orientation, imq);
71  tmq.setRPY(0, -M_PI / 2.0, 0);
72  imq = imq * tmq;
73  m.pose.orientation = tf2::toMsg(imq);
74  m.color.r = 0.0f;
75  m.color.g = 1.0f;
76  m.color.b = 0.0f;
77  m.color.a = 1.0f;
78 
79  visualization_msgs::msg::Marker mc;
80  mc.type = visualization_msgs::msg::Marker::CYLINDER;
81  mc.scale.x = 0.05 * im.scale;
82  mc.scale.y = 0.05 * im.scale;
83  mc.scale.z = 0.15 * im.scale;
84  mc.ns = "goal_pose_arrow_marker";
85  mc.id = 2;
86  mc.action = visualization_msgs::msg::Marker::ADD;
87  mc.header = im.header;
88  mc.pose = im.pose;
89  // Cylinder points along Y
90  tf2::fromMsg(mc.pose.orientation, imq);
91  tmq.setRPY(M_PI / 2.0, 0, 0);
92  imq = imq * tmq;
93  mc.pose.orientation = tf2::toMsg(imq);
94  mc.pose.position.x -= 0.04;
95  mc.pose.position.z += 0.01;
96  mc.color.r = 0.0f;
97  mc.color.g = 1.0f;
98  mc.color.b = 0.0f;
99  mc.color.a = 1.0f;
100 
101  visualization_msgs::msg::InteractiveMarkerControl m_control;
102  m_control.always_visible = true;
103  m_control.interaction_mode = m_control.BUTTON;
104  m_control.markers.push_back(m);
105  m_control.markers.push_back(mc);
106 
107  // add the control to the interactive marker
108  im.controls.push_back(m_control);
109 }
110 
111 void addErrorMarker(visualization_msgs::msg::InteractiveMarker& im)
112 {
113  // create a grey box marker
114  visualization_msgs::msg::Marker err;
115  err.type = visualization_msgs::msg::Marker::MESH_RESOURCE;
116  err.scale.x = 0.002 * im.scale;
117  err.scale.y = 0.002 * im.scale;
118  err.scale.z = 0.002 * im.scale;
119  err.mesh_resource = "package://moveit_ros_planning_interface/resources/access-denied.dae";
120  err.ns = "robot_interaction_error";
121  err.id = 1;
122  err.action = visualization_msgs::msg::Marker::ADD;
123  err.header = im.header;
124  err.pose = im.pose;
125  err.pose.orientation.x = err.pose.orientation.y = 0.7071067811865476;
126  err.pose.orientation.z = err.pose.orientation.w = 0.0;
127  err.color.r = 1.0f;
128  err.color.g = 0.0f;
129  err.color.b = 0.0f;
130  err.color.a = 1.0f;
131 
132  visualization_msgs::msg::InteractiveMarkerControl err_control;
133  err_control.always_visible = false;
134  err_control.markers.push_back(err);
135 
136  // add the control to the interactive marker
137  im.controls.push_back(err_control);
138 }
139 
140 // value for normalized quaternion: 1.0 / std::sqrt(2.0)
141 static const double SQRT2INV = 0.707106781;
142 
143 void addPlanarXYControl(visualization_msgs::msg::InteractiveMarker& int_marker, bool orientation_fixed)
144 {
145  visualization_msgs::msg::InteractiveMarkerControl control;
146 
147  if (orientation_fixed)
149  control.orientation.w = SQRT2INV;
150  control.orientation.x = SQRT2INV;
151  control.orientation.y = 0;
152  control.orientation.z = 0;
153  control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS;
154  int_marker.controls.push_back(control);
155 
156  control.orientation.w = SQRT2INV;
157  control.orientation.x = 0;
158  control.orientation.y = SQRT2INV;
159  control.orientation.z = 0;
160  control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::ROTATE_AXIS;
161  int_marker.controls.push_back(control);
162 
163  control.orientation.w = SQRT2INV;
164  control.orientation.x = 0;
165  control.orientation.y = 0;
166  control.orientation.z = SQRT2INV;
167  control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS;
168  int_marker.controls.push_back(control);
169 }
170 
171 void add6DOFControl(visualization_msgs::msg::InteractiveMarker& int_marker, bool orientation_fixed)
172 {
173  addOrientationControl(int_marker, orientation_fixed);
174  addPositionControl(int_marker, orientation_fixed);
175 }
176 
177 void addOrientationControl(visualization_msgs::msg::InteractiveMarker& int_marker, bool orientation_fixed)
178 {
179  visualization_msgs::msg::InteractiveMarkerControl control;
180 
181  if (orientation_fixed)
183  control.orientation.w = SQRT2INV;
184  control.orientation.x = SQRT2INV;
185  control.orientation.y = 0;
186  control.orientation.z = 0;
187  control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::ROTATE_AXIS;
188  int_marker.controls.push_back(control);
189 
190  control.orientation.w = SQRT2INV;
191  control.orientation.x = 0;
192  control.orientation.y = SQRT2INV;
193  control.orientation.z = 0;
194  control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::ROTATE_AXIS;
195  int_marker.controls.push_back(control);
196 
197  control.orientation.w = SQRT2INV;
198  control.orientation.x = 0;
199  control.orientation.y = 0;
200  control.orientation.z = SQRT2INV;
201  control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::ROTATE_AXIS;
202  int_marker.controls.push_back(control);
203 }
204 
205 void addPositionControl(visualization_msgs::msg::InteractiveMarker& int_marker, bool orientation_fixed)
206 {
207  visualization_msgs::msg::InteractiveMarkerControl control;
208 
209  if (orientation_fixed)
211  control.orientation.w = SQRT2INV;
212  control.orientation.x = SQRT2INV;
213  control.orientation.y = 0;
214  control.orientation.z = 0;
215  control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS;
216  int_marker.controls.push_back(control);
217 
218  control.orientation.w = SQRT2INV;
219  control.orientation.x = 0;
220  control.orientation.y = SQRT2INV;
221  control.orientation.z = 0;
222  control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS;
223  int_marker.controls.push_back(control);
224 
225  control.orientation.w = SQRT2INV;
226  control.orientation.x = 0;
227  control.orientation.y = 0;
228  control.orientation.z = SQRT2INV;
229  control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS;
230  int_marker.controls.push_back(control);
231 }
232 
233 void addViewPlaneControl(visualization_msgs::msg::InteractiveMarker& int_marker, double radius,
234  const std_msgs::msg::ColorRGBA& color, bool position, bool orientation)
235 {
236  visualization_msgs::msg::InteractiveMarkerControl control;
237  control.orientation_mode = visualization_msgs::msg::InteractiveMarkerControl::VIEW_FACING;
238  if (position && orientation)
239  {
240  control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::MOVE_ROTATE_3D;
241  }
242  else if (orientation)
243  {
244  control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::ROTATE_3D;
245  }
246  else
247  {
248  control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::MOVE_3D;
249  }
250  control.independent_marker_orientation = true;
251  control.name = "move";
252 
253  visualization_msgs::msg::Marker marker;
254 
255  marker.type = visualization_msgs::msg::Marker::SPHERE;
256  marker.scale.x = radius * 2.0;
257  marker.scale.y = radius * 2.0;
258  marker.scale.z = radius * 2.0;
259  marker.color = color;
260 
261  control.markers.push_back(marker);
262  control.always_visible = false;
263 
264  int_marker.controls.push_back(control);
265 }
266 
267 visualization_msgs::msg::InteractiveMarker makePlanarXYMarker(const std::string& name,
268  const geometry_msgs::msg::PoseStamped& stamped,
269  double scale, bool orientation_fixed)
270 {
271  visualization_msgs::msg::InteractiveMarker int_marker = makeEmptyInteractiveMarker(name, stamped, scale);
272  addPlanarXYControl(int_marker, orientation_fixed);
273  return int_marker;
274 }
275 
276 visualization_msgs::msg::InteractiveMarker make6DOFMarker(const std::string& name,
277  const geometry_msgs::msg::PoseStamped& stamped, double scale,
278  bool orientation_fixed)
279 {
280  visualization_msgs::msg::InteractiveMarker int_marker = makeEmptyInteractiveMarker(name, stamped, scale);
281  add6DOFControl(int_marker, orientation_fixed);
282  return int_marker;
283 }
284 } // namespace robot_interaction
void addViewPlaneControl(visualization_msgs::msg::InteractiveMarker &int_marker, double radius, const std_msgs::msg::ColorRGBA &color, bool position=true, bool orientation=true)
void addTArrowMarker(visualization_msgs::msg::InteractiveMarker &im)
void add6DOFControl(visualization_msgs::msg::InteractiveMarker &int_marker, bool orientation_fixed=false)
visualization_msgs::msg::InteractiveMarker make6DOFMarker(const std::string &name, const geometry_msgs::msg::PoseStamped &stamped, double scale, bool orientation_fixed=false)
void addOrientationControl(visualization_msgs::msg::InteractiveMarker &int_marker, bool orientation_fixed=false)
visualization_msgs::msg::InteractiveMarker makeEmptyInteractiveMarker(const std::string &name, const geometry_msgs::msg::PoseStamped &stamped, double scale)
void addErrorMarker(visualization_msgs::msg::InteractiveMarker &im)
void addPlanarXYControl(visualization_msgs::msg::InteractiveMarker &int_marker, bool orientation_fixed=false)
visualization_msgs::msg::InteractiveMarker makePlanarXYMarker(const std::string &name, const geometry_msgs::msg::PoseStamped &stamped, double scale, bool orientation_fixed=false)
void addPositionControl(visualization_msgs::msg::InteractiveMarker &int_marker, bool orientation_fixed=false)
name
Definition: setup.py:7