interactive_marker_helpers.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan, Acorn Pooley, Adam Leeper */
00036 
00037 #include <moveit/robot_interaction/interactive_marker_helpers.h>
00038 #include <tf/transform_datatypes.h>
00039 
00040 #include <boost/math/constants/constants.hpp>
00041 
00042 namespace robot_interaction
00043 {
00044 visualization_msgs::InteractiveMarker makeEmptyInteractiveMarker(const std::string& name,
00045                                                                  const geometry_msgs::PoseStamped& stamped,
00046                                                                  double scale)
00047 {
00048   visualization_msgs::InteractiveMarker int_marker;
00049   int_marker.header = stamped.header;
00050   int_marker.name = name;
00051   int_marker.scale = scale;
00052   int_marker.pose = stamped.pose;
00053   return int_marker;
00054 }
00055 
00056 void addTArrowMarker(visualization_msgs::InteractiveMarker& im)
00057 {
00058   // create an arrow marker
00059   visualization_msgs::Marker m;
00060   m.type = visualization_msgs::Marker::ARROW;
00061   m.scale.x = 0.6 * im.scale;
00062   m.scale.y = 0.12 * im.scale;
00063   m.scale.z = 0.12 * im.scale;
00064   m.ns = "goal_pose_arrow_marker";
00065   m.id = 1;
00066   m.action = visualization_msgs::Marker::ADD;
00067   m.header = im.header;
00068   m.pose = im.pose;
00069   // Arrow points along Z
00070   tf::Quaternion imq;
00071   tf::quaternionMsgToTF(m.pose.orientation, imq);
00072   imq = imq * tf::createQuaternionFromRPY(0, -boost::math::constants::pi<double>() / 2.0, 0);
00073   tf::quaternionTFToMsg(imq, m.pose.orientation);
00074   m.color.r = 0.0f;
00075   m.color.g = 1.0f;
00076   m.color.b = 0.0f;
00077   m.color.a = 1.0f;
00078 
00079   visualization_msgs::Marker mc;
00080   mc.type = visualization_msgs::Marker::CYLINDER;
00081   mc.scale.x = 0.05 * im.scale;
00082   mc.scale.y = 0.05 * im.scale;
00083   mc.scale.z = 0.15 * im.scale;
00084   mc.ns = "goal_pose_arrow_marker";
00085   mc.id = 2;
00086   mc.action = visualization_msgs::Marker::ADD;
00087   mc.header = im.header;
00088   mc.pose = im.pose;
00089   // Cylinder points along Y
00090   tf::quaternionMsgToTF(mc.pose.orientation, imq);
00091   imq = imq * tf::createQuaternionFromRPY(boost::math::constants::pi<double>() / 2.0, 0, 0);
00092   tf::quaternionTFToMsg(imq, mc.pose.orientation);
00093   mc.pose.position.x -= 0.04;
00094   mc.pose.position.z += 0.01;
00095   mc.color.r = 0.0f;
00096   mc.color.g = 1.0f;
00097   mc.color.b = 0.0f;
00098   mc.color.a = 1.0f;
00099 
00100   visualization_msgs::InteractiveMarkerControl m_control;
00101   m_control.always_visible = true;
00102   m_control.interaction_mode = m_control.BUTTON;
00103   m_control.markers.push_back(m);
00104   m_control.markers.push_back(mc);
00105 
00106   // add the control to the interactive marker
00107   im.controls.push_back(m_control);
00108 }
00109 
00110 void addErrorMarker(visualization_msgs::InteractiveMarker& im)
00111 {
00112   // create a grey box marker
00113   visualization_msgs::Marker err;
00114   err.type = visualization_msgs::Marker::MESH_RESOURCE;
00115   err.scale.x = 0.002 * im.scale;
00116   err.scale.y = 0.002 * im.scale;
00117   err.scale.z = 0.002 * im.scale;
00118   err.mesh_resource = "package://moveit_ros_planning_interface/resources/access-denied.dae";
00119   err.ns = "robot_interaction_error";
00120   err.id = 1;
00121   err.action = visualization_msgs::Marker::ADD;
00122   err.header = im.header;
00123   err.pose = im.pose;
00124   err.pose.orientation.x = err.pose.orientation.y = 0.7071067811865476;
00125   err.pose.orientation.z = err.pose.orientation.w = 0.0;
00126   err.color.r = 1.0f;
00127   err.color.g = 0.0f;
00128   err.color.b = 0.0f;
00129   err.color.a = 1.0f;
00130 
00131   visualization_msgs::InteractiveMarkerControl err_control;
00132   err_control.always_visible = false;
00133   err_control.markers.push_back(err);
00134 
00135   // add the control to the interactive marker
00136   im.controls.push_back(err_control);
00137 }
00138 
00139 void addPlanarXYControl(visualization_msgs::InteractiveMarker& int_marker, bool orientation_fixed)
00140 {
00141   visualization_msgs::InteractiveMarkerControl control;
00142 
00143   if (orientation_fixed)
00144     control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
00145   control.orientation.w = 1;
00146   control.orientation.x = 1;
00147   control.orientation.y = 0;
00148   control.orientation.z = 0;
00149   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00150   int_marker.controls.push_back(control);
00151 
00152   control.orientation.w = 1;
00153   control.orientation.x = 0;
00154   control.orientation.y = 1;
00155   control.orientation.z = 0;
00156   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00157   int_marker.controls.push_back(control);
00158 
00159   control.orientation.w = 1;
00160   control.orientation.x = 0;
00161   control.orientation.y = 0;
00162   control.orientation.z = 1;
00163   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00164   int_marker.controls.push_back(control);
00165 }
00166 
00167 void add6DOFControl(visualization_msgs::InteractiveMarker& int_marker, bool orientation_fixed)
00168 {
00169   addOrientationControl(int_marker, orientation_fixed);
00170   addPositionControl(int_marker, orientation_fixed);
00171 }
00172 
00173 void addOrientationControl(visualization_msgs::InteractiveMarker& int_marker, bool orientation_fixed)
00174 {
00175   visualization_msgs::InteractiveMarkerControl control;
00176 
00177   if (orientation_fixed)
00178     control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
00179   control.orientation.w = 1;
00180   control.orientation.x = 1;
00181   control.orientation.y = 0;
00182   control.orientation.z = 0;
00183   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00184   int_marker.controls.push_back(control);
00185 
00186   control.orientation.w = 1;
00187   control.orientation.x = 0;
00188   control.orientation.y = 1;
00189   control.orientation.z = 0;
00190   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00191   int_marker.controls.push_back(control);
00192 
00193   control.orientation.w = 1;
00194   control.orientation.x = 0;
00195   control.orientation.y = 0;
00196   control.orientation.z = 1;
00197   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00198   int_marker.controls.push_back(control);
00199 }
00200 
00201 void addPositionControl(visualization_msgs::InteractiveMarker& int_marker, bool orientation_fixed)
00202 {
00203   visualization_msgs::InteractiveMarkerControl control;
00204 
00205   if (orientation_fixed)
00206     control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
00207   control.orientation.w = 1;
00208   control.orientation.x = 1;
00209   control.orientation.y = 0;
00210   control.orientation.z = 0;
00211   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00212   int_marker.controls.push_back(control);
00213 
00214   control.orientation.w = 1;
00215   control.orientation.x = 0;
00216   control.orientation.y = 1;
00217   control.orientation.z = 0;
00218   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00219   int_marker.controls.push_back(control);
00220 
00221   control.orientation.w = 1;
00222   control.orientation.x = 0;
00223   control.orientation.y = 0;
00224   control.orientation.z = 1;
00225   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00226   int_marker.controls.push_back(control);
00227 }
00228 
00229 void addViewPlaneControl(visualization_msgs::InteractiveMarker& int_marker, double radius,
00230                          const std_msgs::ColorRGBA& color, bool position, bool orientation)
00231 {
00232   visualization_msgs::InteractiveMarkerControl control;
00233   control.orientation_mode = visualization_msgs::InteractiveMarkerControl::VIEW_FACING;
00234   if (position && orientation)
00235     control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE_3D;
00236   else if (orientation)
00237     control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_3D;
00238   else
00239     control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_3D;
00240   control.independent_marker_orientation = true;
00241   control.name = "move";
00242 
00243   visualization_msgs::Marker marker;
00244 
00245   marker.type = visualization_msgs::Marker::SPHERE;
00246   marker.scale.x = radius * 2.0;
00247   marker.scale.y = radius * 2.0;
00248   marker.scale.z = radius * 2.0;
00249   marker.color = color;
00250 
00251   control.markers.push_back(marker);
00252   control.always_visible = false;
00253 
00254   int_marker.controls.push_back(control);
00255 }
00256 
00257 visualization_msgs::InteractiveMarker makePlanarXYMarker(const std::string& name,
00258                                                          const geometry_msgs::PoseStamped& stamped, double scale,
00259                                                          bool orientation_fixed)
00260 {
00261   visualization_msgs::InteractiveMarker int_marker = makeEmptyInteractiveMarker(name, stamped, scale);
00262   addPlanarXYControl(int_marker, orientation_fixed);
00263   return int_marker;
00264 }
00265 
00266 visualization_msgs::InteractiveMarker make6DOFMarker(const std::string& name, const geometry_msgs::PoseStamped& stamped,
00267                                                      double scale, bool orientation_fixed)
00268 {
00269   visualization_msgs::InteractiveMarker int_marker = makeEmptyInteractiveMarker(name, stamped, scale);
00270   add6DOFControl(int_marker, orientation_fixed);
00271   return int_marker;
00272 }
00273 }


robot_interaction
Author(s): Ioan Sucan
autogenerated on Mon Apr 23 2018 10:25:44