When tree has only fixed joints, publish state to tf without expecting empy joint state messages

robot_state_publisher/CMakeLists.txt

@@ -22,9 +22,9 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
 #uncomment if you have defined services
 #rosbuild_gensrv()
 
-rosbuild_add_library(${PROJECT_NAME} src/joint_state_listener.cpp src/robot_state_publisher.cpp src/treefksolverposfull_recursive.cpp)
+rosbuild_add_library(${PROJECT_NAME} src/robot_state_publisher.cpp src/treefksolverposfull_recursive.cpp)
 
-rosbuild_add_executable(state_publisher src/robot_state_publisher_node.cpp )
+rosbuild_add_executable(state_publisher src/joint_state_listener.cpp )
 target_link_libraries(state_publisher ${PROJECT_NAME})
 
 rosbuild_add_executable(test_publisher test/test_publisher.cpp )

robot_state_publisher/src/joint_state_listener.cpp

@@ -38,6 +38,8 @@
 #include <ros/ros.h>
 #include "robot_state_publisher/robot_state_publisher.h"
 #include "robot_state_publisher/joint_state_listener.h"
+#include <kdl_parser/kdl_parser.hpp>
+
 
 using namespace std;
 using namespace ros;
@@ -53,8 +55,18 @@ JointStateListener::JointStateListener(const KDL::Tree& tree)
   n_tilde_.param("publish_frequency", publish_freq, 50.0);
   publish_rate_ = Rate(publish_freq);
   
-  // subscribe to mechanism state
+  if (tree.getNrOfJoints() == 0){
-  joint_state_sub_ = n_.subscribe("joint_states", 1, &JointStateListener::callbackJointState, this);;
+    boost::shared_ptr<sensor_msgs::JointState> empty_state(new sensor_msgs::JointState);
+    empty_state->header.stamp = ros::Time::now();
+    while (ros::ok()){
+      this->callbackJointState(empty_state);
+      publish_rate_.sleep();
+    }
+  }
+  else{
+    // subscribe to mechanism state
+    joint_state_sub_ = n_.subscribe("joint_states", 1, &JointStateListener::callbackJointState, this);
+  }
 };
 
 
@@ -77,3 +89,34 @@ void JointStateListener::callbackJointState(const JointStateConstPtr& state)
   publish_rate_.sleep();
 }
 
+
+
+
+
+// ----------------------------------
+// ----- MAIN -----------------------
+// ----------------------------------
+int main(int argc, char** argv)
+{
+  // Initialize ros
+  ros::init(argc, argv, "robot_state_publisher");
+  NodeHandle node;
+
+  // gets the location of the robot description on the parameter server
+  KDL::Tree tree;
+  if (!kdl_parser::treeFromParam("robot_description", tree)){
+    ROS_ERROR("Failed to extract kdl tree from xml robot description");
+    return -1;
+  }
+
+  if (tree.getNrOfSegments() == 0){
+    ROS_WARN("Robot state publisher got an empty tree and cannot publish any state to tf");
+    ros::spin();
+  }
+  else{
+    JointStateListener state_publisher(tree);
+    ros::spin();
+  }
+
+  return 0;
+}

robot_state_publisher/src/robot_state_publisher_node.cpp

@@ -1,82 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-*  Copyright (c) 2008, Willow Garage, Inc.
-*  All rights reserved.
-*
-*  Redistribution and use in source and binary forms, with or without
-*  modification, are permitted provided that the following conditions
-*  are met:
-*
-*   * Redistributions of source code must retain the above copyright
-*     notice, this list of conditions and the following disclaimer.
-*   * Redistributions in binary form must reproduce the above
-*     copyright notice, this list of conditions and the following
-*     disclaimer in the documentation and/or other materials provided
-*     with the distribution.
-*   * Neither the name of the Willow Garage nor the names of its
-*     contributors may be used to endorse or promote products derived
-*     from this software without specific prior written permission.
-*
-*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-*  POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
-
-/* Author: Wim Meeussen */
-
-#include <kdl/tree.hpp>
-#include <ros/ros.h>
-#include <kdl_parser/kdl_parser.hpp>
-#include <urdf/model.h>
-#include "robot_state_publisher/joint_state_listener.h"
-
-using namespace std;
-using namespace ros;
-using namespace KDL;
-using namespace robot_state_publisher;
-
-
-
-// ----------------------------------
-// ----- MAIN -----------------------
-// ----------------------------------
-int main(int argc, char** argv)
-{
-  // Initialize ros
-  ros::init(argc, argv, "robot_state_publisher");
-  NodeHandle node;
-
-  // gets the location of the robot description on the parameter server
-  string full_param_name;
-  node.searchParam("robot_description",full_param_name);
-  string robot_desc;
-
-  // constructs a kdl tree from the robot model
-  node.param(full_param_name, robot_desc, string());
-  Tree tree;
-  if (!kdl_parser::treeFromString(robot_desc, tree)){
-    ROS_ERROR("Failed to extract kdl tree from xml robot description");
-    return -1;
-  }
-
-  if (tree.getNrOfSegments() == 0){
-    ROS_WARN("Robot state publisher got an empty tree and cannot publish any state to tf");
-    ros::spin();
-  }
-  else{
-    JointStateListener state_publisher(tree);
-    ros::spin();
-  }
-
-  return 0;
-}

robot_state_publisher/test/test_two_links_fixed_joint.cpp

@@ -78,15 +78,9 @@ TEST_F(TestPublisher, test)
   ROS_INFO("Creating tf listener");
   TransformListener tf;
 
-  ROS_INFO("Publishing joint state to robot state publisher");
+  // don't need to publish joint state for tree with only fixed joints
-  ros::NodeHandle n;
+  // wait for tf data to come in
-  ros::Publisher js_pub = n.advertise<sensor_msgs::JointState>("joint_states", 100);
+  ros::Duration(10.0).sleep();
-  sensor_msgs::JointState js_msg;
-  for (unsigned int i=0; i<100; i++){
-    js_msg.header.stamp = ros::Time::now();
-    js_pub.publish(js_msg);
-    ros::Duration(0.1).sleep();
-  }
 
   ASSERT_TRUE(tf.canTransform("link1", "link2", Time()));
   ASSERT_FALSE(tf.canTransform("base_link", "wim_link", Time()));