00001 #include <remote_manipulation_markers/Common.h>
00002
00003 using namespace std;
00004
00005 visualization_msgs::InteractiveMarker Common::makeGripperMarker(geometry_msgs::PoseStamped pose)
00006 {
00007 visualization_msgs::InteractiveMarker iMarker;
00008 iMarker.header.frame_id = pose.header.frame_id;
00009
00010 iMarker.pose = pose.pose;
00011
00012 iMarker.scale = 0.2;
00013
00014 iMarker.name = "gripper";
00015 iMarker.description = "gripper goal pose";
00016
00017
00018 visualization_msgs::Marker gripperBase = createGripperMeshMarker(
00019 -0.055, 0, 0,
00020 -0.707, 0, 0, 0.707,
00021 "package://remote_manipulation_markers/meshes/visual/robotiq_85_base_link.dae");
00022 visualization_msgs::Marker gripperLeftKnuckle = createGripperMeshMarker(
00023 -0.001, 0, -0.031,
00024 0.707, 0, 0, 0.707,
00025 "package://remote_manipulation_markers/meshes/visual/robotiq_85_knuckle_link.dae");
00026 visualization_msgs::Marker gripperRightKnuckle = createGripperMeshMarker(
00027 -0.001, 0, 0.031,
00028 -0.707, 0, 0, 0.707,
00029 "package://remote_manipulation_markers/meshes/visual/robotiq_85_knuckle_link.dae");
00030 visualization_msgs::Marker gripperLeftFinger = createGripperMeshMarker(
00031 -0.005, 0, -0.062,
00032 0.707, 0, 0, 0.707,
00033 "package://remote_manipulation_markers/meshes/visual/robotiq_85_finger_link.dae");
00034 visualization_msgs::Marker gripperRightFinger = createGripperMeshMarker(
00035 -0.005, 0, 0.062,
00036 -0.707, 0, 0, 0.707,
00037 "package://remote_manipulation_markers/meshes/visual/robotiq_85_finger_link.dae");
00038 visualization_msgs::Marker gripperLeftInnerKnuckle = createGripperMeshMarker(
00039 0.006, 0, -0.013,
00040 0.707, 0, 0, 0.707,
00041 "package://remote_manipulation_markers/meshes/visual/robotiq_85_inner_knuckle_link.dae");
00042 visualization_msgs::Marker gripperRightInnerKnuckle = createGripperMeshMarker(
00043 0.006, 0, 0.013,
00044 -0.707, 0, 0, 0.707,
00045 "package://remote_manipulation_markers/meshes/visual/robotiq_85_inner_knuckle_link.dae");
00046 visualization_msgs::Marker gripperLeftFingerTip = createGripperMeshMarker(
00047 0.049, 0, -0.05,
00048 0.707, 0, 0, 0.707,
00049 "package://remote_manipulation_markers/meshes/visual/robotiq_85_finger_tip_link.dae");
00050 visualization_msgs::Marker gripperRightFingerTip = createGripperMeshMarker(
00051 0.049, 0, 0.05,
00052 -0.707, 0, 0, 0.707,
00053 "package://remote_manipulation_markers/meshes/visual/robotiq_85_finger_tip_link.dae");
00054
00055 visualization_msgs::InteractiveMarkerControl gripperControl;
00056 gripperControl.markers.push_back(gripperBase);
00057 gripperControl.markers.push_back(gripperLeftKnuckle);
00058 gripperControl.markers.push_back(gripperRightKnuckle);
00059 gripperControl.markers.push_back(gripperLeftFinger);
00060 gripperControl.markers.push_back(gripperRightFinger);
00061 gripperControl.markers.push_back(gripperLeftInnerKnuckle);
00062 gripperControl.markers.push_back(gripperRightInnerKnuckle);
00063 gripperControl.markers.push_back(gripperLeftFingerTip);
00064 gripperControl.markers.push_back(gripperRightFingerTip);
00065 gripperControl.interaction_mode = visualization_msgs::InteractiveMarkerControl::NONE;
00066 gripperControl.name = "gripper_control";
00067 gripperControl.always_visible = true;
00068
00069 iMarker.controls.push_back(gripperControl);
00070
00071 return iMarker;
00072 }
00073
00074 visualization_msgs::Marker Common::createGripperMeshMarker(double x, double y, double z, double rx, double ry, double rz, double rw, string meshLocation)
00075 {
00076 visualization_msgs::Marker marker;
00077 marker.pose.position.x = x;
00078 marker.pose.position.y = y;
00079 marker.pose.position.z = z;
00080 marker.pose.orientation.x = rx;
00081 marker.pose.orientation.y = ry;
00082 marker.pose.orientation.z = rz;
00083 marker.pose.orientation.w = rw;
00084 marker.type = visualization_msgs::Marker::MESH_RESOURCE;
00085 marker.mesh_resource = meshLocation;
00086 marker.scale.x = 1.0;
00087 marker.scale.y = 1.0;
00088 marker.scale.z = 1.0;
00089 marker.color.r = 0.65;
00090 marker.color.g = 0.0;
00091 marker.color.b = 0.65;
00092 marker.color.a = 1.0;
00093
00094 return marker;
00095 }