bkinnightskytw
/
kdl_parser
mirror of https://github.com/existedinnettw/kdl_parser.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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

This commit is contained in:
wim 2010-06-24 19:52:13 +00:00
parent 2b84aed3e5
commit 71ba181883
4 changed files with 50 additions and 95 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
robot_state_publisher/CMakeLists.txt
View File

@ -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 )

45
robot_state_publisher/src/joint_state_listener.cpp
View File

@ -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);
if (tree.getNrOfJoints() == 0){
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);;
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;
}

82
robot_state_publisher/src/robot_state_publisher_node.cpp
View File

@ -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;
}

12
robot_state_publisher/test/test_two_links_fixed_joint.cpp
View File

@ -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<sensor_msgs::JointState>("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()));