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.h>
39#include <tf2/LinearMath/Quaternion.h>
40#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
41
42namespace robot_interaction
43{
44visualization_msgs::msg::InteractiveMarker
45makeEmptyInteractiveMarker(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
55void 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
111void 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)
141static const double SQRT2INV = 0.707106781;
142
143void 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
171void 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
177void 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
205void 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
233void 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
267visualization_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
276visualization_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
interactive_marker_helpers.h
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:276
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:267
robot_interaction::addPositionControl
void addPositionControl(visualization_msgs::msg::InteractiveMarker &int_marker, bool orientation_fixed=false)
Definition interactive_marker_helpers.cpp:205
Generated by doxygen 1.9.8