triangle_foot.cpp
Go to the documentation of this file.
00001 #include <iostream>
00002 #include <interactive_markers/tools.h>
00003 #include <jsk_interactive_marker/triangle_foot.h>
00004 #include <jsk_interactive_marker/interactive_marker_utils.h>
00005 
00006 using namespace std;
00007 
00008 visualization_msgs::Marker TriangleFoot::makeTriangleMarker(){
00009   visualization_msgs::Marker marker;
00010   marker.type = visualization_msgs::Marker::LINE_STRIP;
00011   marker.scale.x = 0.03;//line width;
00012 
00013   marker.color.r = 1.0;
00014   marker.color.g = 1.0;
00015   marker.color.b = 0.0;
00016   marker.color.a = 1.0;
00017 
00018   
00019   geometry_msgs::Point point1;
00020 
00021   geometry_msgs::Point point2;
00022   point2.z = 0.3048; //12in
00023 
00024   geometry_msgs::Point point3;
00025 
00026   point3.y = -0.6096; //24in
00027   if(reverse){
00028     point3.y = - point3.y;
00029   }
00030   point3.z = 0.0;
00031 
00032   marker.points.push_back(point1);
00033   marker.points.push_back(point2);
00034   marker.points.push_back(point3);
00035   marker.points.push_back(point1);
00036   return marker;
00037 }
00038 
00039 visualization_msgs::Marker TriangleFoot::makeRFootMarker(){
00040   geometry_msgs::Pose pose;
00041   if(reverse){
00042     pose.position.x = -0.65;
00043     pose.position.y = 0.65;
00044     pose.position.z = -0.910;
00045   }else{
00046     pose.position.x = -0.8;
00047     pose.position.y = 0.0;
00048     pose.position.z = -0.910;
00049   }
00050   pose.orientation.w = 1.0;
00051   return makeFootMarker(pose);
00052 }
00053 visualization_msgs::Marker TriangleFoot::makeLFootMarker(){
00054   geometry_msgs::Pose pose;
00055   if(reverse){
00056     pose.position.x = -0.65;
00057     pose.position.y = 0.35;
00058     pose.position.z = -0.910;
00059   }else{
00060     pose.position.x = -0.8;
00061     pose.position.y = -0.3;
00062     pose.position.z = -0.910;
00063   }
00064   pose.orientation.w = 1.0;
00065   return makeFootMarker(pose);
00066 
00067 }
00068 visualization_msgs::Marker TriangleFoot::makeFootMarker(geometry_msgs::Pose pose){
00069   visualization_msgs::Marker marker;
00070   marker.type = visualization_msgs::Marker::CUBE;
00071   double PADDING_PARAM = 0.01;
00072   marker.scale.x = 0.27 + PADDING_PARAM;
00073   marker.scale.y = 0.14 + PADDING_PARAM;
00074   marker.scale.z = 0.05;
00075 
00076   marker.pose = pose;
00077 
00078   marker.color.r = 1.0;
00079   marker.color.g = 1.0;
00080   marker.color.b = 0.0;
00081   marker.color.a = 0.8;
00082 
00083   return marker;
00084 
00085 }
00086 
00087 visualization_msgs::InteractiveMarker TriangleFoot::makeInteractiveMarker(){
00088   visualization_msgs::InteractiveMarker mk;
00089   mk.header.frame_id = "/map";
00090   mk.header.stamp = ros::Time(0);
00091   mk.name = marker_name;
00092 
00093   mk.scale = 0.4;
00094 
00095   mk.pose.position.z = 0.910;
00096 
00097   visualization_msgs::InteractiveMarkerControl triangleMarker;
00098   triangleMarker.always_visible = true;
00099   triangleMarker.markers.push_back( makeTriangleMarker());
00100   triangleMarker.markers.push_back( makeRFootMarker());
00101   triangleMarker.markers.push_back( makeLFootMarker());
00102   mk.controls.push_back( triangleMarker );
00103   
00104   im_helpers::add6DofControl(mk, true);
00105   return mk;
00106 }
00107 
00108 void TriangleFoot::moveBoxCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
00109 {
00110   //  std::cout << "moved" << std::endl;
00111 }
00112 
00113 void TriangleFoot::reverseTriangleCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback){
00114   reverse ^= true;
00115   updateBoxInteractiveMarker();
00116 }
00117 
00118 
00119 interactive_markers::MenuHandler TriangleFoot::makeMenuHandler(){
00120   interactive_markers::MenuHandler mh;
00121   mh.insert("Reverse Triangle", boost::bind( &TriangleFoot::reverseTriangleCb, this, _1));
00122   return mh;
00123 }
00124 
00125 
00126 void TriangleFoot::updateBoxInteractiveMarker(){
00127   visualization_msgs::InteractiveMarker boxIM = makeInteractiveMarker();
00128 
00129   server_->insert(boxIM,
00130                   boost::bind( &TriangleFoot::moveBoxCb, this, _1 ));
00131   menu_handler.apply(*server_, marker_name);
00132   server_->applyChanges();
00133 }
00134 
00135 TriangleFoot::TriangleFoot () : nh_(), pnh_("~") {
00136   pnh_.param("server_name", server_name, std::string ("") );
00137   pnh_.param("size", size_, 1.0 );
00138   pnh_.param("marker_name", marker_name, std::string ("triangle_marker") );
00139   pnh_.param("reverse", reverse, false);
00140 
00141   if ( server_name == "" ) {
00142     server_name = ros::this_node::getName();
00143   }
00144   server_.reset( new interactive_markers::InteractiveMarkerServer(server_name));
00145 
00146   menu_handler = makeMenuHandler();
00147   updateBoxInteractiveMarker();
00148   return;
00149 }
00150 
00151 
00152 int main(int argc, char** argv)
00153 {
00154   ros::init(argc, argv, "triangle_foot_marker");
00155   TriangleFoot triFoot;
00156   ros::spin();
00157   return 0;
00158 }
00159 
00160 


jsk_interactive_marker
Author(s): furuta
autogenerated on Sun Sep 13 2015 22:29:27