Add TF Prefix to effort plugin (#1213)

ros-visualization · May 10, 2018 · d9afa34 · d9afa34
1 parent 71c66a6
commit d9afa34
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 5 deletions.
diff --git a/src/rviz/default_plugin/effort_display.cpp b/src/rviz/default_plugin/effort_display.cpp
@@ -107,7 +107,11 @@ namespace rviz
             new rviz::StringProperty( "Robot Description", "robot_description",
                                       "Name of the parameter to search for to load the robot description.",
                                       this, SLOT( updateRobotDescription() ) );
-
+
+        tf_prefix_property_ = new StringProperty( "TF Prefix", "",
+                                                "Robot Model normally assumes the link name is the same as the tf frame name. "
+                                                " This option allows you to set a prefix.  Mainly useful for multi-robot situations.",
+                                                this, SLOT( updateTfPrefix() ) );
 
         joints_category_ =
             new rviz::Property("Joints", QVariant(), "", this);
@@ -131,6 +135,12 @@ namespace rviz
         visuals_.clear();
     }
 
+    void EffortDisplay::updateTfPrefix()
+    {
+      clearStatuses();
+      context_->queueRender();
+    }
+
     void EffortDisplay::clear()
     {
         clearStatuses();
@@ -312,21 +322,21 @@ namespace rviz
 	    int joint_type = joint->type;
 	    if ( joint_type == urdf::Joint::REVOLUTE )
 	    {
-		// we expects that parent_link_name equals to frame_id.
-		std::string parent_link_name = joint->child_link_name;
+		std::string tf_prefix = tf_prefix_property_->getStdString();
+		std::string tf_frame_id = (tf_prefix.empty() ? "" : tf_prefix + "/" ) + joint->child_link_name;
 		Ogre::Quaternion orientation;
 		Ogre::Vector3 position;
 
 		// Here we call the rviz::FrameManager to get the transform from the
 		// fixed frame to the frame in the header of this Effort message.  If
 		// it fails, we can't do anything else so we return.
-		if( !context_->getFrameManager()->getTransform( parent_link_name,
+		if( !context_->getFrameManager()->getTransform( tf_frame_id,
                                                                 ros::Time(),
                                                                 //msg->header.stamp, // ???
                                                                 position, orientation ))
 		{
 		    ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
-			       parent_link_name.c_str(), qPrintable( fixed_frame_) );
+			       tf_frame_id.c_str(), qPrintable( fixed_frame_) );
 		    continue;
 		}
 ;

diff --git a/src/rviz/default_plugin/effort_display.h b/src/rviz/default_plugin/effort_display.h
@@ -692,6 +692,7 @@ namespace rviz
 	void updateColorAndAlpha();
         void updateHistoryLength();
         void updateRobotDescription();
+        void updateTfPrefix();
 
         JointInfo* getJointInfo( const std::string& joint);
         JointInfo* createJoint(const std::string &joint);
@@ -726,6 +727,7 @@ namespace rviz
 	rviz::IntProperty *history_length_property_;
 
         rviz::StringProperty *robot_description_property_;
+        rviz::StringProperty *tf_prefix_property_;
         rviz::Property *joints_category_;
         rviz::BoolProperty *all_enabled_property_;
     };