urdf_control_marker.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 
00003 #include <interactive_markers/interactive_marker_server.h>
00004 #include <interactive_markers/menu_handler.h>
00005 
00006 #include <jsk_interactive_marker/interactive_marker_utils.h>
00007 #include <std_msgs/Bool.h>
00008 
00009 #include <tf/transform_broadcaster.h>
00010 #include <tf/transform_listener.h>
00011 #include <tf/tf.h>
00012 
00013 #include <dynamic_tf_publisher/SetDynamicTF.h>
00014 
00015 using namespace visualization_msgs;
00016 
00017 class UrdfControlMarker {
00018 public:
00019   UrdfControlMarker();
00020   void processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00021   void makeControlMarker( bool fixed );
00022   void set_pose_cb( const geometry_msgs::PoseStampedConstPtr &msg );
00023   void show_marker_cb ( const std_msgs::BoolConstPtr &msg);
00024   void markerUpdate ( std_msgs::Header header, geometry_msgs::Pose pose);
00025   void publish_pose_cb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00026   void callDynamicTf(const std_msgs::Header& header,
00027                      const std::string& child_frame,
00028                      const geometry_msgs::Transform& pose,
00029                      bool until_success = false);
00030 private:
00031   bool use_dynamic_tf_, move_2d_;
00032   tf::TransformListener tf_listener_;
00033   ros::ServiceClient dynamic_tf_publisher_client_;
00034   ros::Subscriber sub_set_pose_, sub_show_marker_;
00035   std::shared_ptr<interactive_markers::InteractiveMarkerServer> server_;
00036   ros::NodeHandle nh_, pnh_;
00037   ros::Publisher pub_pose_, pub_selected_pose_;
00038   std::string frame_id_, marker_frame_id_, fixed_frame_id_;
00039   std::string center_marker_;
00040   std_msgs::ColorRGBA color_;
00041   bool mesh_use_embedded_materials_;
00042   double marker_scale_, center_marker_scale_;
00043   interactive_markers::MenuHandler marker_menu_;
00044   geometry_msgs::Pose center_marker_pose_;
00045 };
00046 
00047 UrdfControlMarker::UrdfControlMarker() : nh_(), pnh_("~"){
00048   server_.reset( new interactive_markers::InteractiveMarkerServer("urdf_control_marker","",false) );
00049 
00050   pnh_.param("move_2d", move_2d_, false);
00051   pnh_.param("use_dynamic_tf", use_dynamic_tf_, false);
00052   pnh_.param<std::string>("frame_id", frame_id_, "/map");
00053   pnh_.param<std::string>("fixed_frame_id", fixed_frame_id_, "/odom_on_ground");
00054   pnh_.param<std::string>("marker_frame_id", marker_frame_id_, "/urdf_control_marker");
00055   pnh_.param<std::string>("center_marker", center_marker_, "");
00056   pnh_.param("marker_scale", marker_scale_, 1.0);
00057   pnh_.param("center_marker_scale", center_marker_scale_, 1.0);
00058   //set color
00059   if(pnh_.hasParam("center_marker_color")){
00060     mesh_use_embedded_materials_ = false;
00061     std::map<std::string, double> color_map;
00062     pnh_.getParam("color", color_map);
00063     color_.r = color_map["r"];
00064     color_.g = color_map["g"];
00065     color_.b = color_map["b"];
00066     color_.a = color_map["a"];
00067   }else{
00068     mesh_use_embedded_materials_ = true;
00069   }
00070 
00071   //set pose
00072   if(pnh_.hasParam("center_marker_pose")){
00073     XmlRpc::XmlRpcValue pose_v;
00074     pnh_.param("center_marker_pose", pose_v, pose_v);
00075     center_marker_pose_ = im_utils::getPose(pose_v);
00076 
00077   }else{
00078     center_marker_pose_.orientation.w = 1.0;
00079   }
00080 
00081   //dynamic_tf_publisher
00082   if (use_dynamic_tf_) {
00083     dynamic_tf_publisher_client_ = nh_.serviceClient<dynamic_tf_publisher::SetDynamicTF>("set_dynamic_tf", true);
00084     dynamic_tf_publisher_client_.waitForExistence();
00085   }
00086   pub_pose_ = pnh_.advertise<geometry_msgs::PoseStamped>("pose", 1);
00087   pub_selected_pose_ = pnh_.advertise<geometry_msgs::PoseStamped>("selected_pose", 1);
00088   sub_set_pose_ = pnh_.subscribe<geometry_msgs::PoseStamped> ("set_pose", 1, boost::bind( &UrdfControlMarker::set_pose_cb, this, _1));
00089   sub_show_marker_ = pnh_.subscribe<std_msgs::Bool> ("show_marker", 1, boost::bind( &UrdfControlMarker::show_marker_cb, this, _1));
00090 
00091   marker_menu_.insert( "Publish Pose",
00092                        boost::bind( &UrdfControlMarker::publish_pose_cb, this, _1) );
00093 
00094   makeControlMarker( false );
00095 
00096   ros::spin();
00097   server_.reset();
00098 }
00099 
00100 void UrdfControlMarker::publish_pose_cb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ){
00101   geometry_msgs::PoseStamped ps;
00102   ps.header = feedback->header;
00103   ps.pose = feedback->pose;
00104   pub_selected_pose_.publish(ps);
00105 }
00106 
00107 void UrdfControlMarker::set_pose_cb ( const geometry_msgs::PoseStampedConstPtr &msg){
00108   // Convert PoseStamped frame_id to fixed_frame_id_
00109   geometry_msgs::PoseStamped in_pose(*msg);
00110   geometry_msgs::PoseStamped out_pose;
00111   in_pose.header.stamp = ros::Time(0.0);
00112   tf_listener_.transformPose(fixed_frame_id_, in_pose, out_pose);
00113   out_pose.header.stamp = msg->header.stamp;
00114   server_->setPose("urdf_control_marker", out_pose.pose, out_pose.header);
00115   server_->applyChanges();
00116   markerUpdate(out_pose.header, out_pose.pose);
00117 }
00118 
00119 void UrdfControlMarker::show_marker_cb ( const std_msgs::BoolConstPtr &msg){
00120   if(msg->data){
00121     makeControlMarker( false );
00122   }else{
00123     server_->clear();
00124     server_->applyChanges();
00125   }
00126 }
00127 
00128 
00129 void UrdfControlMarker::processFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00130 {
00131   markerUpdate( feedback->header, feedback->pose);
00132 }
00133 
00134 void UrdfControlMarker::callDynamicTf(
00135   const std_msgs::Header& header,
00136   const std::string& child_frame,
00137   const geometry_msgs::Transform& transform,
00138   bool until_success)
00139 {
00140   dynamic_tf_publisher::SetDynamicTF SetTf;
00141   SetTf.request.freq = 20;
00142   SetTf.request.cur_tf.header = header;
00143   SetTf.request.cur_tf.child_frame_id = child_frame;
00144   SetTf.request.cur_tf.transform = transform;
00145   ros::Rate r(1);
00146   while (true) {
00147     if (!dynamic_tf_publisher_client_.call(SetTf)) {
00148       ROS_ERROR("Failed to call dynamic_tf: %s => %s",
00149                 header.frame_id.c_str(),
00150                 child_frame.c_str());
00151       // Re-create connection to service server
00152       dynamic_tf_publisher_client_ = nh_.serviceClient<dynamic_tf_publisher::SetDynamicTF>("set_dynamic_tf", true);
00153       dynamic_tf_publisher_client_.waitForExistence();
00154       if (!until_success) {
00155         break;
00156       }
00157     }
00158     else {
00159       break;
00160     }
00161     r.sleep();
00162   }
00163 }
00164 
00165 void UrdfControlMarker::markerUpdate ( std_msgs::Header header, geometry_msgs::Pose pose){
00166   if (use_dynamic_tf_){
00167     geometry_msgs::Transform transform;
00168     transform.translation.x = pose.position.x;
00169     transform.translation.y = pose.position.y;
00170     transform.translation.z = pose.position.z;
00171     transform.rotation = pose.orientation;
00172     callDynamicTf(header, marker_frame_id_, transform);
00173   }
00174   geometry_msgs::PoseStamped ps;
00175   ps.header = header;
00176   ps.pose = pose;
00177   pub_pose_.publish(ps);
00178 }
00179 
00180 
00181 
00182 
00183 void UrdfControlMarker::makeControlMarker( bool fixed )
00184 {
00185   InteractiveMarker int_marker;
00186   int_marker.header.frame_id = frame_id_;
00187   int_marker.scale = marker_scale_;
00188 
00189   int_marker.name = "urdf_control_marker";
00190 
00191   //add center marker
00192   if(center_marker_ != ""){
00193     InteractiveMarkerControl center_marker_control;
00194     center_marker_control.name = "center_marker";
00195     center_marker_control.always_visible = true;
00196     center_marker_control.orientation.w = 1.0;
00197     center_marker_control.orientation.y = 1.0;
00198 
00199     if(move_2d_){
00200       center_marker_control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
00201     }else{
00202       center_marker_control.interaction_mode = InteractiveMarkerControl::MOVE_3D;
00203     }
00204     Marker center_marker;
00205     center_marker.type = Marker::MESH_RESOURCE;
00206     center_marker.scale.x = center_marker.scale.y = center_marker.scale.z = center_marker_scale_;
00207     center_marker.mesh_use_embedded_materials = mesh_use_embedded_materials_;
00208     center_marker.mesh_resource = center_marker_;
00209     center_marker.pose = center_marker_pose_;
00210     center_marker.color = color_;
00211 
00212     center_marker_control.markers.push_back(center_marker);
00213     int_marker.controls.push_back(center_marker_control);
00214   }
00215 
00216   InteractiveMarkerControl control;
00217 
00218   if ( fixed )
00219     {
00220       int_marker.name += "_fixed";
00221       control.orientation_mode = InteractiveMarkerControl::FIXED;
00222     }
00223   control.always_visible = true;
00224 
00225   control.orientation.w = 1;
00226   control.orientation.x = 1;
00227   control.orientation.y = 0;
00228   control.orientation.z = 0;
00229   control.name = "rotate_x";
00230   control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00231   if(!move_2d_){
00232     int_marker.controls.push_back(control);
00233   }
00234   control.name = "move_x";
00235   control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00236   int_marker.controls.push_back(control);
00237 
00238   control.orientation.w = 1;
00239   control.orientation.x = 0;
00240   control.orientation.y = 1;
00241   control.orientation.z = 0;
00242   control.name = "rotate_z";
00243   control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00244   int_marker.controls.push_back(control);
00245   control.name = "move_z";
00246   control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00247   if(!move_2d_){
00248     int_marker.controls.push_back(control);
00249   }
00250 
00251   control.orientation.w = 1;
00252   control.orientation.x = 0;
00253   control.orientation.y = 0;
00254   control.orientation.z = 1;
00255   control.name = "rotate_y";
00256   control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00257   if(!move_2d_){
00258     int_marker.controls.push_back(control);
00259   }
00260   control.name = "move_y";
00261   control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00262   int_marker.controls.push_back(control);
00263 
00264   server_->insert(int_marker);
00265   server_->setCallback(int_marker.name, boost::bind( &UrdfControlMarker::processFeedback, this, _1));
00266   marker_menu_.apply(*server_, int_marker.name);
00267   server_->applyChanges();
00268   if (use_dynamic_tf_) {
00269     /* First initialize dynamic tf as identity */
00270     std_msgs::Header header;
00271     header.frame_id = fixed_frame_id_;
00272     header.stamp = ros::Time::now();
00273     geometry_msgs::Transform transform;
00274     transform.rotation.w = 1.0;
00275     callDynamicTf(header, marker_frame_id_, transform, true);
00276   }
00277 }
00278 
00279 int main(int argc, char** argv)
00280 {
00281   ros::init(argc, argv, "basic_controls");
00282   UrdfControlMarker ucm;
00283 }


jsk_interactive_marker
Author(s): furuta
autogenerated on Wed May 1 2019 02:40:31