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 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
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
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
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
00107 im.controls.push_back(m_control);
00108 }
00109
00110 void addErrorMarker(visualization_msgs::InteractiveMarker& im)
00111 {
00112
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
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 }