Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
00045 visualization_msgs::InteractiveMarker makeEmptyInteractiveMarker(const std::string& name,
00046 const geometry_msgs::PoseStamped &stamped,
00047 double scale)
00048 {
00049 visualization_msgs::InteractiveMarker int_marker;
00050 int_marker.header = stamped.header;
00051 int_marker.name = name;
00052 int_marker.scale = scale;
00053 int_marker.pose = stamped.pose;
00054 return int_marker;
00055 }
00056
00057 void addTArrowMarker(visualization_msgs::InteractiveMarker &im)
00058 {
00059
00060 visualization_msgs::Marker m;
00061 m.type = visualization_msgs::Marker::ARROW;
00062 m.scale.x = 0.6 * im.scale;
00063 m.scale.y = 0.12 * im.scale;
00064 m.scale.z = 0.12 * im.scale;
00065 m.ns = "goal_pose_arrow_marker";
00066 m.id = 1;
00067 m.action = visualization_msgs::Marker::ADD;
00068 m.header = im.header;
00069 m.pose = im.pose;
00070
00071 tf::Quaternion imq;
00072 tf::quaternionMsgToTF(m.pose.orientation, imq);
00073 imq=imq*tf::createQuaternionFromRPY(0, -boost::math::constants::pi<double>() / 2.0, 0);
00074 tf::quaternionTFToMsg(imq, m.pose.orientation);
00075 m.color.r = 0.0f;
00076 m.color.g = 1.0f;
00077 m.color.b = 0.0f;
00078 m.color.a = 1.0f;
00079
00080 visualization_msgs::Marker mc;
00081 mc.type = visualization_msgs::Marker::CYLINDER;
00082 mc.scale.x = 0.05 * im.scale;
00083 mc.scale.y = 0.05 * im.scale;
00084 mc.scale.z = 0.15 * im.scale;
00085 mc.ns = "goal_pose_arrow_marker";
00086 mc.id = 2;
00087 mc.action = visualization_msgs::Marker::ADD;
00088 mc.header = im.header;
00089 mc.pose = im.pose;
00090
00091 tf::quaternionMsgToTF(mc.pose.orientation, imq);
00092 imq=imq*tf::createQuaternionFromRPY(boost::math::constants::pi<double>() / 2.0, 0, 0);
00093 tf::quaternionTFToMsg(imq, mc.pose.orientation);
00094 mc.pose.position.x -= 0.04;
00095 mc.pose.position.z += 0.01;
00096 mc.color.r = 0.0f;
00097 mc.color.g = 1.0f;
00098 mc.color.b = 0.0f;
00099 mc.color.a = 1.0f;
00100
00101 visualization_msgs::InteractiveMarkerControl m_control;
00102 m_control.always_visible = true;
00103 m_control.interaction_mode=m_control.BUTTON;
00104 m_control.markers.push_back(m);
00105 m_control.markers.push_back(mc);
00106
00107
00108 im.controls.push_back(m_control);
00109 }
00110
00111 void addErrorMarker(visualization_msgs::InteractiveMarker &im)
00112 {
00113
00114 visualization_msgs::Marker err;
00115 err.type = visualization_msgs::Marker::MESH_RESOURCE;
00116 err.scale.x = 0.002 * im.scale;
00117 err.scale.y = 0.002 * im.scale;
00118 err.scale.z = 0.002 * im.scale;
00119 err.mesh_resource = "package://moveit_ros_planning_interface/resources/access-denied.dae";
00120 err.ns = "robot_interaction_error";
00121 err.id = 1;
00122 err.action = visualization_msgs::Marker::ADD;
00123 err.header = im.header;
00124 err.pose = im.pose;
00125 err.pose.orientation.x = err.pose.orientation.y = 0.7071067811865476;
00126 err.pose.orientation.z = err.pose.orientation.w = 0.0;
00127 err.color.r = 1.0f;
00128 err.color.g = 0.0f;
00129 err.color.b = 0.0f;
00130 err.color.a = 1.0f;
00131
00132 visualization_msgs::InteractiveMarkerControl err_control;
00133 err_control.always_visible = false;
00134 err_control.markers.push_back(err);
00135
00136
00137 im.controls.push_back(err_control);
00138 }
00139
00140 void addPlanarXYControl(visualization_msgs::InteractiveMarker& int_marker, bool orientation_fixed)
00141 {
00142 visualization_msgs::InteractiveMarkerControl control;
00143
00144 if (orientation_fixed)
00145 control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
00146 control.orientation.w = 1;
00147 control.orientation.x = 1;
00148 control.orientation.y = 0;
00149 control.orientation.z = 0;
00150 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00151 int_marker.controls.push_back(control);
00152
00153 control.orientation.w = 1;
00154 control.orientation.x = 0;
00155 control.orientation.y = 1;
00156 control.orientation.z = 0;
00157 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00158 int_marker.controls.push_back(control);
00159
00160 control.orientation.w = 1;
00161 control.orientation.x = 0;
00162 control.orientation.y = 0;
00163 control.orientation.z = 1;
00164 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00165 int_marker.controls.push_back(control);
00166 }
00167
00168 void add6DOFControl(visualization_msgs::InteractiveMarker& int_marker, bool orientation_fixed)
00169 {
00170 addOrientationControl(int_marker, orientation_fixed);
00171 addPositionControl(int_marker, orientation_fixed);
00172 }
00173
00174 void addOrientationControl(visualization_msgs::InteractiveMarker& int_marker, bool orientation_fixed)
00175 {
00176 visualization_msgs::InteractiveMarkerControl control;
00177
00178 if (orientation_fixed)
00179 control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
00180 control.orientation.w = 1;
00181 control.orientation.x = 1;
00182 control.orientation.y = 0;
00183 control.orientation.z = 0;
00184 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00185 int_marker.controls.push_back(control);
00186
00187 control.orientation.w = 1;
00188 control.orientation.x = 0;
00189 control.orientation.y = 1;
00190 control.orientation.z = 0;
00191 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00192 int_marker.controls.push_back(control);
00193
00194 control.orientation.w = 1;
00195 control.orientation.x = 0;
00196 control.orientation.y = 0;
00197 control.orientation.z = 1;
00198 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00199 int_marker.controls.push_back(control);
00200 }
00201
00202 void addPositionControl(visualization_msgs::InteractiveMarker& int_marker, bool orientation_fixed)
00203 {
00204 visualization_msgs::InteractiveMarkerControl control;
00205
00206 if (orientation_fixed)
00207 control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
00208 control.orientation.w = 1;
00209 control.orientation.x = 1;
00210 control.orientation.y = 0;
00211 control.orientation.z = 0;
00212 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00213 int_marker.controls.push_back(control);
00214
00215 control.orientation.w = 1;
00216 control.orientation.x = 0;
00217 control.orientation.y = 1;
00218 control.orientation.z = 0;
00219 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00220 int_marker.controls.push_back(control);
00221
00222 control.orientation.w = 1;
00223 control.orientation.x = 0;
00224 control.orientation.y = 0;
00225 control.orientation.z = 1;
00226 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00227 int_marker.controls.push_back(control);
00228 }
00229
00230 void addViewPlaneControl(visualization_msgs::InteractiveMarker& int_marker, double radius, 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,
00259 double scale,
00260 bool orientation_fixed)
00261 {
00262 visualization_msgs::InteractiveMarker int_marker = makeEmptyInteractiveMarker(name, stamped, scale);
00263 addPlanarXYControl(int_marker, orientation_fixed);
00264 return int_marker;
00265 }
00266
00267 visualization_msgs::InteractiveMarker make6DOFMarker(const std::string& name,
00268 const geometry_msgs::PoseStamped &stamped,
00269 double scale,
00270 bool orientation_fixed)
00271 {
00272 visualization_msgs::InteractiveMarker int_marker = makeEmptyInteractiveMarker(name, stamped, scale);
00273 add6DOFControl(int_marker, orientation_fixed);
00274 return int_marker;
00275 }
00276
00277 }