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 #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)
148  control.orientation_mode = visualization_msgs::msg::InteractiveMarkerControl::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)
182  control.orientation_mode = visualization_msgs::msg::InteractiveMarkerControl::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)
210  control.orientation_mode = visualization_msgs::msg::InteractiveMarkerControl::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  control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::MOVE_ROTATE_3D;
240  else if (orientation)
241  control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::ROTATE_3D;
242  else
243  control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::MOVE_3D;
244  control.independent_marker_orientation = true;
245  control.name = "move";
246 
247  visualization_msgs::msg::Marker marker;
248 
249  marker.type = visualization_msgs::msg::Marker::SPHERE;
250  marker.scale.x = radius * 2.0;
251  marker.scale.y = radius * 2.0;
252  marker.scale.z = radius * 2.0;
253  marker.color = color;
254 
255  control.markers.push_back(marker);
256  control.always_visible = false;
257 
258  int_marker.controls.push_back(control);
259 }
260 
261 visualization_msgs::msg::InteractiveMarker makePlanarXYMarker(const std::string& name,
262  const geometry_msgs::msg::PoseStamped& stamped,
263  double scale, bool orientation_fixed)
264 {
265  visualization_msgs::msg::InteractiveMarker int_marker = makeEmptyInteractiveMarker(name, stamped, scale);
266  addPlanarXYControl(int_marker, orientation_fixed);
267  return int_marker;
268 }
269 
270 visualization_msgs::msg::InteractiveMarker make6DOFMarker(const std::string& name,
271  const geometry_msgs::msg::PoseStamped& stamped, double scale,
272  bool orientation_fixed)
273 {
274  visualization_msgs::msg::InteractiveMarker int_marker = makeEmptyInteractiveMarker(name, stamped, scale);
275  add6DOFControl(int_marker, orientation_fixed);
276  return int_marker;
277 }
278 } // 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:233
robot_interaction::addTArrowMarker
void addTArrowMarker(visualization_msgs::msg::InteractiveMarker &im)
Definition: interactive_marker_helpers.cpp:55
robot_interaction::add6DOFControl
void add6DOFControl(visualization_msgs::msg::InteractiveMarker &int_marker, bool orientation_fixed=false)
Definition: interactive_marker_helpers.cpp:171
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:270
robot_interaction::addOrientationControl
void addOrientationControl(visualization_msgs::msg::InteractiveMarker &int_marker, bool orientation_fixed=false)
Definition: interactive_marker_helpers.cpp:177
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:45
robot_interaction::addErrorMarker
void addErrorMarker(visualization_msgs::msg::InteractiveMarker &im)
Definition: interactive_marker_helpers.cpp:111
robot_interaction::addPlanarXYControl
void addPlanarXYControl(visualization_msgs::msg::InteractiveMarker &int_marker, bool orientation_fixed=false)
Definition: interactive_marker_helpers.cpp:143
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:261
robot_interaction::addPositionControl
void addPositionControl(visualization_msgs::msg::InteractiveMarker &int_marker, bool orientation_fixed=false)
Definition: interactive_marker_helpers.cpp:205
setup.name
name
Definition: setup.py:7
Generated by doxygen 1.9.1