From 71ba181883e5b26f563b36bc4f42e621f7a19814 Mon Sep 17 00:00:00 2001 From: wim Date: Thu, 24 Jun 2010 19:52:13 +0000 Subject: [PATCH] When tree has only fixed joints, publish state to tf without expecting empy joint state messages --- robot_state_publisher/CMakeLists.txt | 4 +- .../src/joint_state_listener.cpp | 47 ++++++++++- .../src/robot_state_publisher_node.cpp | 82 ------------------- .../test/test_two_links_fixed_joint.cpp | 12 +-- 4 files changed, 50 insertions(+), 95 deletions(-) delete mode 100644 robot_state_publisher/src/robot_state_publisher_node.cpp diff --git a/robot_state_publisher/CMakeLists.txt b/robot_state_publisher/CMakeLists.txt index 8b840f8..5c68421 100644 --- a/robot_state_publisher/CMakeLists.txt +++ b/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 ) diff --git a/robot_state_publisher/src/joint_state_listener.cpp b/robot_state_publisher/src/joint_state_listener.cpp index 69df07a..e7b2eee 100644 --- a/robot_state_publisher/src/joint_state_listener.cpp +++ b/robot_state_publisher/src/joint_state_listener.cpp @@ -38,6 +38,8 @@ #include #include "robot_state_publisher/robot_state_publisher.h" #include "robot_state_publisher/joint_state_listener.h" +#include + 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 - joint_state_sub_ = n_.subscribe("joint_states", 1, &JointStateListener::callbackJointState, this);; + if (tree.getNrOfJoints() == 0){ + boost::shared_ptr 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; +} diff --git a/robot_state_publisher/src/robot_state_publisher_node.cpp b/robot_state_publisher/src/robot_state_publisher_node.cpp deleted file mode 100644 index ae54ba9..0000000 --- a/robot_state_publisher/src/robot_state_publisher_node.cpp +++ /dev/null @@ -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 -#include -#include -#include -#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; -} diff --git a/robot_state_publisher/test/test_two_links_fixed_joint.cpp b/robot_state_publisher/test/test_two_links_fixed_joint.cpp index f0355c0..530ffd1 100644 --- a/robot_state_publisher/test/test_two_links_fixed_joint.cpp +++ b/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"); - ros::NodeHandle n; - ros::Publisher js_pub = n.advertise("joint_states", 100); - 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(); - } + // don't need to publish joint state for tree with only fixed joints + // wait for tf data to come in + ros::Duration(10.0).sleep(); ASSERT_TRUE(tf.canTransform("link1", "link2", Time())); ASSERT_FALSE(tf.canTransform("base_link", "wim_link", Time()));