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

robot_interaction
Author(s): Ioan Sucan
autogenerated on Sat May 3 2025 02:27:06