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

robot_interaction
Author(s): Ioan Sucan
autogenerated on Sun Oct 18 2020 13:18:32