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

removing robot_state_publisher (new location)

This commit is contained in:
Ioan Sucan 2012-08-01 22:33:14 -07:00
parent f5e750ee37
commit e4b91b263d
22 changed files with 0 additions and 4313 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
robot_state_publisher/CMakeLists.txt
View File

@ -1,55 +0,0 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
# Set the build type. Options are:
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
# Debug : w/ debug symbols, w/o optimization
# Release : w/o debug symbols, w/ optimization
# RelWithDebInfo : w/ debug symbols, w/ optimization
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
#set(ROS_BUILD_TYPE RelWithDebInfo)
rosbuild_init()
rosbuild_add_boost_directories()
#set the default path for built executables to the "bin" directory
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
#set the default path for built libraries to the "lib" directory
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#uncomment if you have defined messages
#rosbuild_genmsg()
#uncomment if you have defined services
#rosbuild_gensrv()
rosbuild_add_library(${PROJECT_NAME} src/robot_state_publisher.cpp src/treefksolverposfull_recursive.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 )
target_link_libraries(test_publisher ${PROJECT_NAME})
rosbuild_add_gtest_build_flags(test_publisher)
rosbuild_declare_test(test_publisher)
rosbuild_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_publisher.launch)
rosbuild_add_executable(test_one_link test/test_one_link.cpp )
target_link_libraries(test_one_link ${PROJECT_NAME})
rosbuild_add_gtest_build_flags(test_one_link)
rosbuild_declare_test(test_one_link)
rosbuild_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_one_link.launch)
rosbuild_add_executable(test_two_links_fixed_joint test/test_two_links_fixed_joint.cpp )
target_link_libraries(test_two_links_fixed_joint ${PROJECT_NAME})
rosbuild_add_gtest_build_flags(test_two_links_fixed_joint)
rosbuild_declare_test(test_two_links_fixed_joint)
rosbuild_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_two_links_fixed_joint.launch)
rosbuild_add_executable(test_two_links_moving_joint test/test_two_links_moving_joint.cpp )
target_link_libraries(test_two_links_moving_joint ${PROJECT_NAME})
rosbuild_add_gtest_build_flags(test_two_links_moving_joint)
rosbuild_declare_test(test_two_links_moving_joint)
rosbuild_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_two_links_moving_joint.launch)
# Download needed data file
rosbuild_download_test_data(http://pr.willowgarage.com/data/robot_state_publisher/joint_states_indexed.bag test/joint_states_indexed.bag 793e0b566ebe4698265a936b92fa2bba)

1
robot_state_publisher/Makefile
View File

@ -1 +0,0 @@
include $(shell rospack find mk)/cmake.mk

11
robot_state_publisher/doc.dox
View File

@ -1,11 +0,0 @@
/**
@mainpage
@htmlinclude manifest.html
This package contains the robot state publisher, which can be used as
a library or as a ROS node. The API documentation for the library can
be found here: robot_state_publisher::RobotStatePublisher
**/

77
robot_state_publisher/include/robot_state_publisher/joint_state_listener.h
View File

@ -1,77 +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 */
#ifndef JOINT_STATE_LISTENER_H
#define JOINT_STATE_LISTENER_H
#include <kdl/tree.hpp>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include "robot_state_publisher/robot_state_publisher.h"
using namespace std;
using namespace ros;
using namespace KDL;
typedef boost::shared_ptr<sensor_msgs::JointState const> JointStateConstPtr;
namespace robot_state_publisher{
class JointStateListener{
public:
/** Constructor
* \param tree The kinematic model of a robot, represented by a KDL Tree
*/
JointStateListener(const KDL::Tree& tree);
/// Destructor
~JointStateListener();
private:
void callbackJointState(const JointStateConstPtr& state);
void callbackFixedJoint(const ros::TimerEvent& e);
Duration publish_interval_;
robot_state_publisher::RobotStatePublisher state_publisher_;
Subscriber joint_state_sub_;
ros::Timer timer_;
std::map<std::string, ros::Time> last_publish_time_;
};
}
#endif

91
robot_state_publisher/include/robot_state_publisher/robot_state_publisher.h
View File

@ -1,91 +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 */
#ifndef ROBOT_STATE_PUBLISHER_H
#define ROBOT_STATE_PUBLISHER_H
#include <ros/ros.h>
#include <boost/scoped_ptr.hpp>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <kdl/frames.hpp>
#include <kdl/segment.hpp>
#include <kdl/tree.hpp>
namespace robot_state_publisher{
class SegmentPair
{
public:
SegmentPair(const KDL::Segment& p_segment, const std::string& p_root, const std::string& p_tip):
segment(p_segment), root(p_root), tip(p_tip){}
KDL::Segment segment;
std::string root, tip;
};
class RobotStatePublisher
{
public:
/** Constructor
* \param tree The kinematic model of a robot, represented by a KDL Tree
*/
RobotStatePublisher(const KDL::Tree& tree);
/// Destructor
~RobotStatePublisher(){};
/** Publish transforms to tf
* \param joint_positions A map of joint names and joint positions.
* \param time The time at which the joint positions were recorded
*/
void publishTransforms(const std::map<std::string, double>& joint_positions, const ros::Time& time);
void publishFixedTransforms();
private:
void addChildren(const KDL::SegmentMap::const_iterator segment);
std::map<std::string, SegmentPair> segments_, segments_fixed_;
tf::TransformBroadcaster tf_broadcaster_;
};
}
#endif

53
robot_state_publisher/include/robot_state_publisher/treefksolverposfull_recursive.hpp
View File

@ -1,53 +0,0 @@
// Copyright (C) 2009 Willow Garage Inc
// Version: 1.0
// Author: Wim Meeussen <meeussen at willowgarage dot com>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDLTREEFKSOLVERPOSFULL_RECURSIVE_HPP
#define KDLTREEFKSOLVERPOSFULL_RECURSIVE_HPP
#include <kdl/tree.hpp>
#include <tf/transform_datatypes.h>
namespace KDL {
class TreeFkSolverPosFull_recursive
{
public:
TreeFkSolverPosFull_recursive(const Tree& _tree);
~TreeFkSolverPosFull_recursive();
int JntToCart(const std::map<std::string, double>& q_in, std::map<std::string, tf::Stamped<Frame> >& p_out, bool flatten_tree=true);
private:
void addFrameToMap(const std::map<std::string, double>& q_in,
std::map<std::string, tf::Stamped<KDL::Frame> >& p_out,
const tf::Stamped<KDL::Frame>& previous_frame,
const SegmentMap::const_iterator this_segment,
bool flatten_tree);
Tree tree;
};
}
#endif

34
robot_state_publisher/manifest.xml
View File

@ -1,34 +0,0 @@
<package>
<description brief="This package allows you to publish the state of a robot to the transform library topic">
This package allows you to publish the state of a robot to
<a href="http://ros.org/wiki/tf">tf</a>. Once the state gets published, it is
available to all components in the system that also use <tt>tf</tt>.
The package takes the joint angles of the robot as input
and publishes the 3D poses of the robot links, using a kinematic
tree model of the robot. The package can both be used as a library
and as a ROS node. This package has been well tested and the code
is stable. No major changes are planned in the near future
</description>
<author>Wim Meeussen meeussen@willowgarage.com </author>
<license>BSD</license>
<review status="Doc reviewed" notes=""/>
<url>http://ros.org/wiki/robot_state_publisher</url>
<depend package="kdl_parser" />
<depend package="sensor_msgs" />
<depend package="roscpp" />
<depend package="tf" />
<depend package="tf_conversions" />
<export>
<cpp cflags="-I${prefix}/include/" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lrobot_state_publisher" />
</export>
<platform os="ubuntu" version="9.04"/>
<platform os="ubuntu" version="9.10"/>
<platform os="ubuntu" version="10.04"/>
</package>

136
robot_state_publisher/src/joint_state_listener.cpp
View File

@ -1,136 +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 "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;
using namespace KDL;
using namespace robot_state_publisher;
JointStateListener::JointStateListener(const KDL::Tree& tree)
: state_publisher_(tree)
{
ros::NodeHandle n_tilde("~");
ros::NodeHandle n;
// set publish frequency
double publish_freq;
n_tilde.param("publish_frequency", publish_freq, 50.0);
publish_interval_ = ros::Duration(1.0/max(publish_freq,1.0));
// subscribe to joint state
joint_state_sub_ = n.subscribe("joint_states", 1, &JointStateListener::callbackJointState, this);
// trigger to publish fixed joints
timer_ = n_tilde.createTimer(publish_interval_, &JointStateListener::callbackFixedJoint, this);
};
JointStateListener::~JointStateListener()
{};
void JointStateListener::callbackFixedJoint(const ros::TimerEvent& e)
{
state_publisher_.publishFixedTransforms();
}
void JointStateListener::callbackJointState(const JointStateConstPtr& state)
{
if (state->name.size() != state->position.size()){
ROS_ERROR("Robot state publisher received an invalid joint state vector");
return;
}
// determine least recently published joint
ros::Time last_published = ros::Time::now();
for (unsigned int i=0; i<state->name.size(); i++)
{
ros::Time t = last_publish_time_[state->name[i]];
last_published = (t < last_published) ? t : last_published;
}
// note: if a joint was seen for the first time,
// then last_published is zero.
// check if we need to publish
if (state->header.stamp >= last_published + publish_interval_){
// get joint positions from state message
map<string, double> joint_positions;
for (unsigned int i=0; i<state->name.size(); i++)
joint_positions.insert(make_pair(state->name[i], state->position[i]));
state_publisher_.publishTransforms(joint_positions, state->header.stamp);
// store publish time in joint map
for (unsigned int i=0; i<state->name.size(); i++)
last_publish_time_[state->name[i]] = state->header.stamp;
}
}
// ----------------------------------
// ----- 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;
}
JointStateListener state_publisher(tree);
ros::spin();
return 0;
}

120
robot_state_publisher/src/robot_state_publisher.cpp
View File

@ -1,120 +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 "robot_state_publisher/robot_state_publisher.h"
#include <kdl/frames_io.hpp>
#include <tf_conversions/tf_kdl.h>
using namespace std;
using namespace ros;
namespace robot_state_publisher{
RobotStatePublisher::RobotStatePublisher(const KDL::Tree& tree)
{
// walk the tree and add segments to segments_
addChildren(tree.getRootSegment());
}
// add children to correct maps
void RobotStatePublisher::addChildren(const KDL::SegmentMap::const_iterator segment)
{
const std::string& root = segment->second.segment.getName();
const std::vector<KDL::SegmentMap::const_iterator>& children = segment->second.children;
for (unsigned int i=0; i<children.size(); i++){
const KDL::Segment& child = children[i]->second.segment;
SegmentPair s(children[i]->second.segment, 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());
}
else{
segments_.insert(make_pair(child.getJoint().getName(), s));
ROS_DEBUG("Adding moving segment from %s to %s", root.c_str(), child.getName().c_str());
}
addChildren(children[i]);
}
}
// publish moving transforms
void RobotStatePublisher::publishTransforms(const map<string, double>& joint_positions, const Time& time)
{
ROS_DEBUG("Publishing transforms for moving joints");
std::vector<tf::StampedTransform> tf_transforms;
tf::StampedTransform tf_transform;
tf_transform.stamp_ = time;
// loop over all joints
for (map<string, double>::const_iterator jnt=joint_positions.begin(); jnt != joint_positions.end(); jnt++){
std::map<std::string, SegmentPair>::const_iterator seg = segments_.find(jnt->first);
if (seg != segments_.end()){
tf::TransformKDLToTF(seg->second.segment.pose(jnt->second), tf_transform);
tf_transform.frame_id_ = seg->second.root;
tf_transform.child_frame_id_ = seg->second.tip;
tf_transforms.push_back(tf_transform);
}
}
tf_broadcaster_.sendTransform(tf_transforms);
}
// publish fixed transforms
void RobotStatePublisher::publishFixedTransforms()
{
ROS_DEBUG("Publishing transforms for moving joints");
std::vector<tf::StampedTransform> tf_transforms;
tf::StampedTransform tf_transform;
tf_transform.stamp_ = ros::Time::now()+ros::Duration(0.5); // future publish by 0.5 seconds
// loop over all fixed segments
for (map<string, SegmentPair>::const_iterator seg=segments_fixed_.begin(); seg != segments_fixed_.end(); seg++){
tf::TransformKDLToTF(seg->second.segment.pose(0), tf_transform);
tf_transform.frame_id_ = seg->second.root;
tf_transform.child_frame_id_ = seg->second.tip;
tf_transforms.push_back(tf_transform);
}
tf_broadcaster_.sendTransform(tf_transforms);
}
}

85
robot_state_publisher/src/treefksolverposfull_recursive.cpp
View File

@ -1,85 +0,0 @@
// Copyright (C) 2009 Willow Garage Inc
// Version: 1.0
// Author: Wim Meeussen <meeussen at willowgarage dot com>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#include "robot_state_publisher/treefksolverposfull_recursive.hpp"
#include <ros/ros.h>
#include <iostream>
#include <cstdio>
using namespace std;
namespace KDL {
TreeFkSolverPosFull_recursive::TreeFkSolverPosFull_recursive(const Tree& _tree):
tree(_tree)
{
}
TreeFkSolverPosFull_recursive::~TreeFkSolverPosFull_recursive()
{
}
int TreeFkSolverPosFull_recursive::JntToCart(const map<string, double>& q_in, map<string, tf::Stamped<Frame> >& p_out, bool flatten_tree)
{
// clear output
p_out.clear();
addFrameToMap(q_in, p_out, tf::Stamped<KDL::Frame>(KDL::Frame::Identity(), ros::Time(), tree.getRootSegment()->second.segment.getName()), tree.getRootSegment(), flatten_tree);
return 0;
}
void TreeFkSolverPosFull_recursive::addFrameToMap(const map<string, double>& q_in,
map<string, tf::Stamped<Frame> >& p_out,
const tf::Stamped<KDL::Frame>& previous_frame,
const SegmentMap::const_iterator this_segment,
bool flatten_tree)
{
// get pose of this segment
tf::Stamped<KDL::Frame> this_frame;
double jnt_p = 0;
if (this_segment->second.segment.getJoint().getType() != Joint::None){
map<string, double>::const_iterator jnt_pos = q_in.find(this_segment->second.segment.getJoint().getName());
if (jnt_pos == q_in.end()){
ROS_DEBUG("Warning: TreeFKSolverPosFull Could not find value for joint '%s'. Skipping this tree branch", this_segment->first.c_str());
return;
}
jnt_p = jnt_pos->second;
}
this_frame = tf::Stamped<KDL::Frame>(previous_frame * this_segment->second.segment.pose(jnt_p), ros::Time(), previous_frame.frame_id_);
if (this_segment->first != tree.getRootSegment()->first)
p_out.insert(make_pair(this_segment->first, tf::Stamped<KDL::Frame>(this_frame, ros::Time(), previous_frame.frame_id_)));
// get poses of child segments
for (vector<SegmentMap::const_iterator>::const_iterator child=this_segment->second.children.begin(); child !=this_segment->second.children.end(); child++){
if (flatten_tree)
addFrameToMap(q_in, p_out, this_frame, *child, flatten_tree);
else
addFrameToMap(q_in, p_out, tf::Stamped<KDL::Frame>(KDL::Frame::Identity(), ros::Time(), this_segment->first), *child, flatten_tree);
}
}
}

3
robot_state_publisher/test/one_link.urdf
View File

@ -1,3 +0,0 @@
<robot name="test_robot">
<link name="link1" />
</robot>

3132
robot_state_publisher/test/pr2.urdf
View File

File diff suppressed because it is too large Load Diff

111
robot_state_publisher/test/test_one_link.cpp
View File

@ -1,111 +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 <string>
#include <gtest/gtest.h>
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <boost/thread/thread.hpp>
#include <urdf/model.h>
#include <kdl_parser/kdl_parser.hpp>
#include "robot_state_publisher/joint_state_listener.h"
using namespace ros;
using namespace tf;
using namespace robot_state_publisher;
int g_argc;
char** g_argv;
#define EPS 0.01
class TestPublisher : public testing::Test
{
public:
JointStateListener* publisher;
protected:
/// constructor
TestPublisher()
{}
/// Destructor
~TestPublisher()
{}
};
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();
}
SUCCEED();
}
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "test_robot_state_publisher");
ros::NodeHandle node;
boost::thread ros_thread(boost::bind(&ros::spin));
g_argc = argc;
g_argv = argv;
int res = RUN_ALL_TESTS();
ros_thread.interrupt();
ros_thread.join();
return res;
}

6
robot_state_publisher/test/test_one_link.launch
View File

@ -1,6 +0,0 @@
<launch>
<node pkg="robot_state_publisher" name="robot_state_publisher" type="state_publisher" />
<param name="robot_description" textfile="$(find robot_state_publisher)/test/one_link.urdf" />
<test test-name="test_publisher" pkg="robot_state_publisher" type="test_one_link" />
</launch>

122
robot_state_publisher/test/test_publisher.cpp
View File

@ -1,122 +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 <string>
#include <gtest/gtest.h>
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <boost/thread/thread.hpp>
#include <urdf/model.h>
#include <kdl_parser/kdl_parser.hpp>
#include "robot_state_publisher/joint_state_listener.h"
using namespace ros;
using namespace tf;
using namespace robot_state_publisher;
int g_argc;
char** g_argv;
#define EPS 0.01
class TestPublisher : public testing::Test
{
public:
JointStateListener* publisher;
protected:
/// constructor
TestPublisher()
{}
/// Destructor
~TestPublisher()
{}
};
TEST_F(TestPublisher, test)
{
ROS_INFO("Creating tf listener");
TransformListener tf;
ROS_INFO("Waiting for bag to complete");
Duration(15.0).sleep();
ASSERT_TRUE(tf.canTransform("base_link", "torso_lift_link", Time()));
ASSERT_TRUE(tf.canTransform("base_link", "r_gripper_palm_link", Time()));
ASSERT_TRUE(tf.canTransform("base_link", "r_gripper_palm_link", Time()));
ASSERT_TRUE(tf.canTransform("l_gripper_palm_link", "r_gripper_palm_link", Time()));
ASSERT_TRUE(tf.canTransform("l_gripper_palm_link", "fl_caster_r_wheel_link", Time()));
ASSERT_FALSE(tf.canTransform("base_link", "wim_link", Time()));
tf::StampedTransform t;
tf.lookupTransform("base_link", "r_gripper_palm_link",Time(), t );
EXPECT_NEAR(t.getOrigin().x(), 0.769198, EPS);
EXPECT_NEAR(t.getOrigin().y(), -0.188800, EPS);
EXPECT_NEAR(t.getOrigin().z(), 0.764914, EPS);
tf.lookupTransform("l_gripper_palm_link", "r_gripper_palm_link",Time(), t );
EXPECT_NEAR(t.getOrigin().x(), 0.000384222, EPS);
EXPECT_NEAR(t.getOrigin().y(), -0.376021, EPS);
EXPECT_NEAR(t.getOrigin().z(), -1.0858e-05, EPS);
SUCCEED();
}
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "test_robot_state_publisher");
ros::NodeHandle node;
boost::thread ros_thread(boost::bind(&ros::spin));
g_argc = argc;
g_argv = argv;
int res = RUN_ALL_TESTS();
ros_thread.interrupt();
ros_thread.join();
return res;
}

8
robot_state_publisher/test/test_publisher.launch
View File

@ -1,8 +0,0 @@
<launch>
<param name="use_sim_time" value="true" />
<node pkg="rosbag" name="rosbag" type="play" args="--clock $(find robot_state_publisher)/test/joint_states_indexed.bag" />
<node pkg="robot_state_publisher" name="robot_state_publisher" type="state_publisher" />
<param name="robot_description" textfile="$(find robot_state_publisher)/test/pr2.urdf" />
<test test-name="test_publisher" pkg="robot_state_publisher" type="test_publisher" />
</launch>

114
robot_state_publisher/test/test_two_links_fixed_joint.cpp
View File

@ -1,114 +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 <string>
#include <gtest/gtest.h>
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <boost/thread/thread.hpp>
#include <urdf/model.h>
#include <kdl_parser/kdl_parser.hpp>
#include "robot_state_publisher/joint_state_listener.h"
using namespace ros;
using namespace tf;
using namespace robot_state_publisher;
int g_argc;
char** g_argv;
#define EPS 0.01
class TestPublisher : public testing::Test
{
public:
JointStateListener* publisher;
protected:
/// constructor
TestPublisher()
{}
/// Destructor
~TestPublisher()
{}
};
TEST_F(TestPublisher, test)
{
ROS_INFO("Creating tf listener");
TransformListener tf;
// 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()));
tf::StampedTransform t;
tf.lookupTransform("link1", "link2",Time(), t );
EXPECT_NEAR(t.getOrigin().x(), 5.0, EPS);
EXPECT_NEAR(t.getOrigin().y(), 0.0, EPS);
EXPECT_NEAR(t.getOrigin().z(), 0.0, EPS);
SUCCEED();
}
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "test_robot_state_publisher");
ros::NodeHandle node;
boost::thread ros_thread(boost::bind(&ros::spin));
g_argc = argc;
g_argv = argv;
int res = RUN_ALL_TESTS();
ros_thread.interrupt();
ros_thread.join();
return res;
}

6
robot_state_publisher/test/test_two_links_fixed_joint.launch
View File

@ -1,6 +0,0 @@
<launch>
<node pkg="robot_state_publisher" name="robot_state_publisher" type="state_publisher" />
<param name="robot_description" textfile="$(find robot_state_publisher)/test/two_links_fixed_joint.urdf" />
<test test-name="test_publisher" pkg="robot_state_publisher" type="test_two_links_fixed_joint" />
</launch>

122
robot_state_publisher/test/test_two_links_moving_joint.cpp
View File

@ -1,122 +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 <string>
#include <gtest/gtest.h>
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <boost/thread/thread.hpp>
#include <urdf/model.h>
#include <kdl_parser/kdl_parser.hpp>
#include "robot_state_publisher/joint_state_listener.h"
using namespace ros;
using namespace tf;
using namespace robot_state_publisher;
int g_argc;
char** g_argv;
#define EPS 0.01
class TestPublisher : public testing::Test
{
public:
JointStateListener* publisher;
protected:
/// constructor
TestPublisher()
{}
/// Destructor
~TestPublisher()
{}
};
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;
js_msg.name.push_back("joint1");
js_msg.position.push_back(M_PI);
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()));
tf::StampedTransform t;
tf.lookupTransform("link1", "link2",Time(), t );
EXPECT_NEAR(t.getOrigin().x(), 5.0, EPS);
EXPECT_NEAR(t.getOrigin().y(), 0.0, EPS);
EXPECT_NEAR(t.getOrigin().z(), 0.0, EPS);
SUCCEED();
}
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "test_robot_state_publisher");
ros::NodeHandle node;
boost::thread ros_thread(boost::bind(&ros::spin));
g_argc = argc;
g_argv = argv;
int res = RUN_ALL_TESTS();
ros_thread.interrupt();
ros_thread.join();
return res;
}

6
robot_state_publisher/test/test_two_links_moving_joint.launch
View File

@ -1,6 +0,0 @@
<launch>
<node pkg="robot_state_publisher" name="robot_state_publisher" type="state_publisher" />
<param name="robot_description" textfile="$(find robot_state_publisher)/test/two_links_moving_joint.urdf" />
<test test-name="test_publisher" pkg="robot_state_publisher" type="test_two_links_moving_joint" />
</launch>

10
robot_state_publisher/test/two_links_fixed_joint.urdf
View File

@ -1,10 +0,0 @@
<robot name="test_robot">
<link name="link1" />
<link name="link2" />
<joint name="joint1" type="fixed">
<parent link="link1"/>
<child link="link2"/>
<origin xyz="5 0 0" rpy="0 0 1.57" />
</joint>
</robot>

10
robot_state_publisher/test/two_links_moving_joint.urdf
View File

@ -1,10 +0,0 @@
<robot name="test_robot">
<link name="link1" />
<link name="link2" />
<joint name="joint1" type="continuous">
<parent link="link1"/>
<child link="link2"/>
<origin xyz="5 0 0" rpy="0 0 1.57" />
</joint>
</robot>