From 8235bce9b65ee9723a4f0a0e03d7258737cf07ff Mon Sep 17 00:00:00 2001
From: Toni Oliver <toliver.shadow@gmail.com>
Date: Tue, 1 Sep 2015 20:11:37 +0100
Subject: [PATCH] Check URDF to distinguish fixed joints from floating joints.
 Floating joint are ignored by the publisher.

---
 include/robot_state_publisher/joint_state_listener.h |  2 +-
 .../robot_state_publisher/robot_state_publisher.h    |  4 +++-
 src/joint_state_listener.cpp                         |  6 +++---
 src/robot_state_publisher.cpp                        | 12 +++++++++---
 4 files changed, 16 insertions(+), 8 deletions(-)

diff --git a/include/robot_state_publisher/joint_state_listener.h b/include/robot_state_publisher/joint_state_listener.h
index cb8945a..f3c3f15 100644
--- a/include/robot_state_publisher/joint_state_listener.h
+++ b/include/robot_state_publisher/joint_state_listener.h
@@ -57,7 +57,7 @@ class JointStateListener{
   /** Constructor
    * \param tree The kinematic model of a robot, represented by a KDL Tree
    */
-  JointStateListener(const KDL::Tree& tree, const MimicMap& m);
+  JointStateListener(const KDL::Tree& tree, const MimicMap& m, const urdf::Model& model = urdf::Model());
 
   /// Destructor
   ~JointStateListener();
diff --git a/include/robot_state_publisher/robot_state_publisher.h b/include/robot_state_publisher/robot_state_publisher.h
index df44286..de853c4 100644
--- a/include/robot_state_publisher/robot_state_publisher.h
+++ b/include/robot_state_publisher/robot_state_publisher.h
@@ -41,6 +41,7 @@
 #include <boost/scoped_ptr.hpp>
 #include <tf/tf.h>
 #include <tf/transform_broadcaster.h>
+#include <urdf/model.h>
 #include <kdl/frames.hpp>
 #include <kdl/segment.hpp>
 #include <kdl/tree.hpp>
@@ -64,7 +65,7 @@ class RobotStatePublisher
   /** Constructor
    * \param tree The kinematic model of a robot, represented by a KDL Tree 
    */
-  RobotStatePublisher(const KDL::Tree& tree);
+  RobotStatePublisher(const KDL::Tree& tree, const urdf::Model& model = urdf::Model());
 
   /// Destructor
   ~RobotStatePublisher(){};
@@ -82,6 +83,7 @@ class RobotStatePublisher
 
   std::map<std::string, SegmentPair> segments_, segments_fixed_;
   tf::TransformBroadcaster tf_broadcaster_;
+  const urdf::Model& model_;
 };
 
 
diff --git a/src/joint_state_listener.cpp b/src/joint_state_listener.cpp
index 6b9d877..5f50d10 100644
--- a/src/joint_state_listener.cpp
+++ b/src/joint_state_listener.cpp
@@ -47,8 +47,8 @@ using namespace ros;
 using namespace KDL;
 using namespace robot_state_publisher;
 
-JointStateListener::JointStateListener(const KDL::Tree& tree, const MimicMap& m)
-  : state_publisher_(tree), mimic_(m)
+JointStateListener::JointStateListener(const KDL::Tree& tree, const MimicMap& m, const urdf::Model& model)
+  : state_publisher_(tree, model), mimic_(m)
 {
   ros::NodeHandle n_tilde("~");
   ros::NodeHandle n;
@@ -164,7 +164,7 @@ int main(int argc, char** argv)
     }
   }
 
-  JointStateListener state_publisher(tree, mimic);
+  JointStateListener state_publisher(tree, mimic, model);
   ros::spin();
 
   return 0;
diff --git a/src/robot_state_publisher.cpp b/src/robot_state_publisher.cpp
index df26d44..7990aca 100644
--- a/src/robot_state_publisher.cpp
+++ b/src/robot_state_publisher.cpp
@@ -45,7 +45,8 @@ using namespace ros;
 
 namespace robot_state_publisher{
 
-  RobotStatePublisher::RobotStatePublisher(const KDL::Tree& tree)
+  RobotStatePublisher::RobotStatePublisher(const KDL::Tree& tree, const urdf::Model& model)
+    : model_(model)
   {
     // walk the tree and add segments to segments_
     addChildren(tree.getRootSegment());
@@ -62,8 +63,13 @@ namespace robot_state_publisher{
       const KDL::Segment& child = GetTreeElementSegment(children[i]->second);
       SegmentPair s(GetTreeElementSegment(children[i]->second), root, child.getName());
       if (child.getJoint().getType() == KDL::Joint::None){
-        segments_fixed_.insert(make_pair(child.getJoint().getName(), s));
-        ROS_DEBUG("Adding fixed segment from %s to %s", root.c_str(), child.getName().c_str());
+        if (model_.getJoint(child.getJoint().getName()) && model_.getJoint(child.getJoint().getName())->type == urdf::Joint::FLOATING){
+          ROS_INFO("Floating joint. Not adding segment from %s to %s. This TF can not be published based on joint_states info", root.c_str(), child.getName().c_str());
+        }
+        else{
+          segments_fixed_.insert(make_pair(child.getJoint().getName(), s));
+          ROS_DEBUG("Adding fixed segment from %s to %s", root.c_str(), child.getName().c_str());
+        }
       }
       else{
         segments_.insert(make_pair(child.getJoint().getName(), s));