moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
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.hpp>
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
47namespace robot_interaction
48{
49visualization_msgs::msg::InteractiveMarker
50makeEmptyInteractiveMarker(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
60void 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
116void 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)
146static const double SQRT2INV = 0.707106781;
147
148void 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
176void 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
182void 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
210void 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
238void 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 {
245 control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::MOVE_ROTATE_3D;
246 }
247 else if (orientation)
248 {
249 control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::ROTATE_3D;
250 }
251 else
252 {
253 control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::MOVE_3D;
254 }
255 control.independent_marker_orientation = true;
256 control.name = "move";
257
258 visualization_msgs::msg::Marker marker;
259
260 marker.type = visualization_msgs::msg::Marker::SPHERE;
261 marker.scale.x = radius * 2.0;
262 marker.scale.y = radius * 2.0;
263 marker.scale.z = radius * 2.0;
264 marker.color = color;
265
266 control.markers.push_back(marker);
267 control.always_visible = false;
268
269 int_marker.controls.push_back(control);
270}
271
272visualization_msgs::msg::InteractiveMarker makePlanarXYMarker(const std::string& name,
273 const geometry_msgs::msg::PoseStamped& stamped,
274 double scale, bool orientation_fixed)
275{
276 visualization_msgs::msg::InteractiveMarker int_marker = makeEmptyInteractiveMarker(name, stamped, scale);
277 addPlanarXYControl(int_marker, orientation_fixed);
278 return int_marker;
279}
280
281visualization_msgs::msg::InteractiveMarker make6DOFMarker(const std::string& name,
282 const geometry_msgs::msg::PoseStamped& stamped, double scale,
283 bool orientation_fixed)
284{
285 visualization_msgs::msg::InteractiveMarker int_marker = makeEmptyInteractiveMarker(name, stamped, scale);
286 add6DOFControl(int_marker, orientation_fixed);
287 return int_marker;
288}
289} // namespace robot_interaction
interactive_marker_helpers.hpp
robot_interaction
Definition interaction.hpp: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:281
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:272
robot_interaction::addPositionControl
void addPositionControl(visualization_msgs::msg::InteractiveMarker &int_marker, bool orientation_fixed=false)
Definition interactive_marker_helpers.cpp:210
Generated by doxygen 1.9.8