From e4b91b263d26e1198af44cee24b3ff54e050bddf Mon Sep 17 00:00:00 2001 From: Ioan Sucan Date: Wed, 1 Aug 2012 22:33:14 -0700 Subject: [PATCH] removing robot_state_publisher (new location) --- robot_state_publisher/CMakeLists.txt | 55 - robot_state_publisher/Makefile | 1 - robot_state_publisher/doc.dox | 11 - .../joint_state_listener.h | 77 - .../robot_state_publisher.h | 91 - .../treefksolverposfull_recursive.hpp | 53 - robot_state_publisher/manifest.xml | 34 - .../src/joint_state_listener.cpp | 136 - .../src/robot_state_publisher.cpp | 120 - .../src/treefksolverposfull_recursive.cpp | 85 - robot_state_publisher/test/one_link.urdf | 3 - robot_state_publisher/test/pr2.urdf | 3132 ----------------- robot_state_publisher/test/test_one_link.cpp | 111 - .../test/test_one_link.launch | 6 - robot_state_publisher/test/test_publisher.cpp | 122 - .../test/test_publisher.launch | 8 - .../test/test_two_links_fixed_joint.cpp | 114 - .../test/test_two_links_fixed_joint.launch | 6 - .../test/test_two_links_moving_joint.cpp | 122 - .../test/test_two_links_moving_joint.launch | 6 - .../test/two_links_fixed_joint.urdf | 10 - .../test/two_links_moving_joint.urdf | 10 - 22 files changed, 4313 deletions(-) delete mode 100644 robot_state_publisher/CMakeLists.txt delete mode 100644 robot_state_publisher/Makefile delete mode 100644 robot_state_publisher/doc.dox delete mode 100644 robot_state_publisher/include/robot_state_publisher/joint_state_listener.h delete mode 100644 robot_state_publisher/include/robot_state_publisher/robot_state_publisher.h delete mode 100644 robot_state_publisher/include/robot_state_publisher/treefksolverposfull_recursive.hpp delete mode 100644 robot_state_publisher/manifest.xml delete mode 100644 robot_state_publisher/src/joint_state_listener.cpp delete mode 100644 robot_state_publisher/src/robot_state_publisher.cpp delete mode 100644 robot_state_publisher/src/treefksolverposfull_recursive.cpp delete mode 100644 robot_state_publisher/test/one_link.urdf delete mode 100644 robot_state_publisher/test/pr2.urdf delete mode 100644 robot_state_publisher/test/test_one_link.cpp delete mode 100644 robot_state_publisher/test/test_one_link.launch delete mode 100644 robot_state_publisher/test/test_publisher.cpp delete mode 100644 robot_state_publisher/test/test_publisher.launch delete mode 100644 robot_state_publisher/test/test_two_links_fixed_joint.cpp delete mode 100644 robot_state_publisher/test/test_two_links_fixed_joint.launch delete mode 100644 robot_state_publisher/test/test_two_links_moving_joint.cpp delete mode 100644 robot_state_publisher/test/test_two_links_moving_joint.launch delete mode 100644 robot_state_publisher/test/two_links_fixed_joint.urdf delete mode 100644 robot_state_publisher/test/two_links_moving_joint.urdf diff --git a/robot_state_publisher/CMakeLists.txt b/robot_state_publisher/CMakeLists.txt deleted file mode 100644 index 5c68421..0000000 --- a/robot_state_publisher/CMakeLists.txt +++ /dev/null @@ -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) diff --git a/robot_state_publisher/Makefile b/robot_state_publisher/Makefile deleted file mode 100644 index b75b928..0000000 --- a/robot_state_publisher/Makefile +++ /dev/null @@ -1 +0,0 @@ -include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/robot_state_publisher/doc.dox b/robot_state_publisher/doc.dox deleted file mode 100644 index 9267270..0000000 --- a/robot_state_publisher/doc.dox +++ /dev/null @@ -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 - - -**/ diff --git a/robot_state_publisher/include/robot_state_publisher/joint_state_listener.h b/robot_state_publisher/include/robot_state_publisher/joint_state_listener.h deleted file mode 100644 index 4df60d8..0000000 --- a/robot_state_publisher/include/robot_state_publisher/joint_state_listener.h +++ /dev/null @@ -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 -#include -#include -#include "robot_state_publisher/robot_state_publisher.h" - -using namespace std; -using namespace ros; -using namespace KDL; - -typedef boost::shared_ptr 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 last_publish_time_; - -}; -} - - -#endif diff --git a/robot_state_publisher/include/robot_state_publisher/robot_state_publisher.h b/robot_state_publisher/include/robot_state_publisher/robot_state_publisher.h deleted file mode 100644 index 9ea834c..0000000 --- a/robot_state_publisher/include/robot_state_publisher/robot_state_publisher.h +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include - -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& joint_positions, const ros::Time& time); - void publishFixedTransforms(); - -private: - void addChildren(const KDL::SegmentMap::const_iterator segment); - - - std::map segments_, segments_fixed_; - tf::TransformBroadcaster tf_broadcaster_; -}; - - - -} - -#endif diff --git a/robot_state_publisher/include/robot_state_publisher/treefksolverposfull_recursive.hpp b/robot_state_publisher/include/robot_state_publisher/treefksolverposfull_recursive.hpp deleted file mode 100644 index 1d11b52..0000000 --- a/robot_state_publisher/include/robot_state_publisher/treefksolverposfull_recursive.hpp +++ /dev/null @@ -1,53 +0,0 @@ -// Copyright (C) 2009 Willow Garage Inc - -// Version: 1.0 -// Author: Wim Meeussen -// Maintainer: Ruben Smits -// 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 -#include - - -namespace KDL { - -class TreeFkSolverPosFull_recursive - -{ -public: - TreeFkSolverPosFull_recursive(const Tree& _tree); - ~TreeFkSolverPosFull_recursive(); - - int JntToCart(const std::map& q_in, std::map >& p_out, bool flatten_tree=true); - -private: - void addFrameToMap(const std::map& q_in, - std::map >& p_out, - const tf::Stamped& previous_frame, - const SegmentMap::const_iterator this_segment, - bool flatten_tree); - - Tree tree; - -}; -} - -#endif diff --git a/robot_state_publisher/manifest.xml b/robot_state_publisher/manifest.xml deleted file mode 100644 index 69aed7e..0000000 --- a/robot_state_publisher/manifest.xml +++ /dev/null @@ -1,34 +0,0 @@ - - - - This package allows you to publish the state of a robot to - tf. Once the state gets published, it is - available to all components in the system that also use tf. - 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 - - - Wim Meeussen meeussen@willowgarage.com - BSD - - - http://ros.org/wiki/robot_state_publisher - - - - - - - - - - - - - - - - diff --git a/robot_state_publisher/src/joint_state_listener.cpp b/robot_state_publisher/src/joint_state_listener.cpp deleted file mode 100644 index be0438a..0000000 --- a/robot_state_publisher/src/joint_state_listener.cpp +++ /dev/null @@ -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 -#include -#include "robot_state_publisher/robot_state_publisher.h" -#include "robot_state_publisher/joint_state_listener.h" -#include - - -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; iname.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 joint_positions; - for (unsigned int i=0; iname.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; iname.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; -} diff --git a/robot_state_publisher/src/robot_state_publisher.cpp b/robot_state_publisher/src/robot_state_publisher.cpp deleted file mode 100644 index d26927a..0000000 --- a/robot_state_publisher/src/robot_state_publisher.cpp +++ /dev/null @@ -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 -#include - -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& children = segment->second.children; - for (unsigned int i=0; isecond.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& joint_positions, const Time& time) - { - ROS_DEBUG("Publishing transforms for moving joints"); - std::vector tf_transforms; - tf::StampedTransform tf_transform; - tf_transform.stamp_ = time; - - // loop over all joints - for (map::const_iterator jnt=joint_positions.begin(); jnt != joint_positions.end(); jnt++){ - std::map::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_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::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); - } - -} - - - diff --git a/robot_state_publisher/src/treefksolverposfull_recursive.cpp b/robot_state_publisher/src/treefksolverposfull_recursive.cpp deleted file mode 100644 index c4700cd..0000000 --- a/robot_state_publisher/src/treefksolverposfull_recursive.cpp +++ /dev/null @@ -1,85 +0,0 @@ -// Copyright (C) 2009 Willow Garage Inc - -// Version: 1.0 -// Author: Wim Meeussen -// Maintainer: Ruben Smits -// 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 -#include -#include - -using namespace std; - -namespace KDL { - -TreeFkSolverPosFull_recursive::TreeFkSolverPosFull_recursive(const Tree& _tree): - tree(_tree) -{ -} - -TreeFkSolverPosFull_recursive::~TreeFkSolverPosFull_recursive() -{ -} - - -int TreeFkSolverPosFull_recursive::JntToCart(const map& q_in, map >& p_out, bool flatten_tree) -{ - // clear output - p_out.clear(); - - addFrameToMap(q_in, p_out, tf::Stamped(KDL::Frame::Identity(), ros::Time(), tree.getRootSegment()->second.segment.getName()), tree.getRootSegment(), flatten_tree); - - return 0; -} - - - -void TreeFkSolverPosFull_recursive::addFrameToMap(const map& q_in, - map >& p_out, - const tf::Stamped& previous_frame, - const SegmentMap::const_iterator this_segment, - bool flatten_tree) -{ - // get pose of this segment - tf::Stamped this_frame; - double jnt_p = 0; - if (this_segment->second.segment.getJoint().getType() != Joint::None){ - map::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(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(this_frame, ros::Time(), previous_frame.frame_id_))); - - // get poses of child segments - for (vector::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::Identity(), ros::Time(), this_segment->first), *child, flatten_tree); - } -} - -} diff --git a/robot_state_publisher/test/one_link.urdf b/robot_state_publisher/test/one_link.urdf deleted file mode 100644 index be21ee1..0000000 --- a/robot_state_publisher/test/one_link.urdf +++ /dev/null @@ -1,3 +0,0 @@ - - - \ No newline at end of file diff --git a/robot_state_publisher/test/pr2.urdf b/robot_state_publisher/test/pr2.urdf deleted file mode 100644 index ae3ee20..0000000 --- a/robot_state_publisher/test/pr2.urdf +++ /dev/null @@ -1,3132 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - true - 1000.0 - - - - true - 1.0 - 5 - - power_state - 10.0 - 87.78 - -474 - 525 - 15.52 - 16.41 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 640 - 640 - 1 - 0.0 0.0 0.0 - false - -129.998394137 - 129.998394137 - 0.08 - 10.0 - 0.01 - 20 - - 0.005 - true - 20 - base_scan - base_laser_link - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -79.2380952381 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 79.2380952381 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -79.2380952381 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -79.2380952381 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 79.2380952381 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -79.2380952381 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -79.2380952381 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 79.2380952381 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -79.2380952381 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -79.2380952381 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 79.2380952381 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -79.2380952381 - - - - - - false - - base_link_geom - 100.0 - - true - 100.0 - base_bumper - - - - - - - - - - true - 100.0 - base_link - base_pose_ground_truth - 0.01 - map - 25.7 25.7 0 - - 0 0 0 - - - base_footprint - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - torso_lift_link_geom - 100.0 - - true - 100.0 - torso_lift_bumper - - - - - - - - -52143.33 - - - - - - - - - - - - - - - - - true - 100.0 - imu_link - torso_lift_imu/data - 2.89e-08 - 0 0 0 - 0 0 0 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 6.0 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 6.0 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - R8G8B8 - 2448 2050 - 45 - 0.1 - 100 - 20.0 - - true - 20.0 - /prosilica/image_raw - /prosilica/camera_info - /prosilica/request_image - high_def_frame - 1224.5 - 1224.5 - 1025.5 - 2955 - - 0.00000001 - 0.00000001 - 0.00000001 - 0.00000001 - 0.00000001 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 640 480 - BAYER_BGGR8 - 90 - 0.1 - 100 - 25.0 - - true - 25.0 - wide_stereo/left/image_raw - wide_stereo/left/camera_info - wide_stereo_optical_frame - 0 - 320.5 - 320.5 - 240.5 - - - 320 - 0.00000001 - 0.00000001 - 0.00000001 - 0.00000001 - 0.00000001 - - - - true - PR2/Blue - - - - - - - - - - - - - - - - - - - - - - 640 480 - BAYER_BGGR8 - 90 - 0.1 - 100 - 25.0 - - true - 25.0 - wide_stereo/right/image_raw - wide_stereo/right/camera_info - wide_stereo_optical_frame - 0.09 - 320.5 - 320.5 - 240.5 - - - 320 - 0.00000001 - 0.00000001 - 0.00000001 - 0.00000001 - 0.00000001 - - - - true - PR2/Blue - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 640 480 - L8 - 45 - 0.1 - 100 - 25.0 - - true - 25.0 - narrow_stereo/left/image_raw - narrow_stereo/left/camera_info - narrow_stereo_optical_frame - 0 - 320.5 - 320.5 - 240.5 - - - 772.55 - 0.00000001 - 0.00000001 - 0.00000001 - 0.00000001 - 0.00000001 - - - - true - PR2/Blue - - - - - - - - - - - - - - - - - - - - - - 640 480 - L8 - 45 - 0.1 - 100 - 25.0 - - true - 25.0 - narrow_stereo/right/image_raw - narrow_stereo/right/camera_info - narrow_stereo_optical_frame - 0.09 - 320.5 - 320.5 - 240.5 - - - 772.55 - 0.00000001 - 0.00000001 - 0.00000001 - 0.00000001 - 0.00000001 - - - - true - PR2/Blue - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - 15.0 - stereo_projection_pattern_high_res_red.png - projector_wg6802418_child_frame - stereo_projection_pattern_filter.png - projector_wg6802418_controller/image - projector_wg6802418_controller/projector - 0.785398163397 - 0.4 - 10 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 640 - 640 - 1 - 0.0 0.0 0.0 - false - -79.9999999086 - 79.9999999086 - 0.08 - 10.0 - 0.01 - 40 - - 0.005 - true - 40 - tilt_scan - laser_tilt_link - - - - - - - - - - -6.05 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - - - - - - - - - - 32.6525111499 - - - true - - - - - - - true - - - - - - - - - 63.1552452977 - - - - - - 61.8948225713 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - - - - - - - - -90.5142857143 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -1.0 - - - true - - - - - - - - - -36.167452007 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - - - true - - - - - - - true - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - - r_gripper_l_finger_link_geom - 100.0 - - true - 100.0 - r_gripper_l_finger_link - r_gripper_l_finger_bumper - - - - - - - - - - - - - - - - - true - - r_gripper_r_finger_link_geom - 100.0 - - true - r_gripper_r_finger_link - 100.0 - r_gripper_r_finger_bumper - - - - - - - - - - - - - - - - true - false - - r_gripper_l_finger_tip_link_geom - 100.0 - - true - r_gripper_l_finger_tip_link - 100.0 - r_gripper_l_finger_tip_bumper - - - - - - - - - - - - - - - - true - false - - r_gripper_r_finger_tip_link_geom - 100.0 - - true - r_gripper_r_finger_tip_link - 100.0 - r_gripper_r_finger_tip_bumper - - - - - - - - - - - true - 100.0 - r_gripper_l_finger_link - r_gripper_l_finger_pose_ground_truth - 0.0 - base_link - - - - true - 100.0 - r_gripper_l_finger_link - r_gripper_l_finger_force_ground_truth - r_gripper_l_finger_link - - - - - - - - - - - - true - 0.17126 - 7.7562e-05 - 1.49095e-06 - -9.83385e-06 - 0.000197083 - -3.06125e-06 - 0.000181054 - 0.03598 - 0.0173 - -0.00164 - 0.82991 -0.157 0.790675 - 0 -0 0 - true - false - - - true - 0.17389 - 7.73841e-05 - -2.09309e-06 - -8.36228e-06 - 0.000198474 - 2.4611e-06 - 0.00018107 - 0.03576 - -0.01736 - -0.00095 - 0.82991 -0.219 0.790675 - 0 -0 0 - true - false - - - - - r_gripper_r_parallel_link - r_gripper_palm_link - r_gripper_palm_link - 0 0 -1 - 0.2 - 0.05891 -0.031 0 - - - r_gripper_l_parallel_link - r_gripper_palm_link - r_gripper_palm_link - 0 0 1 - 0.2 - 0.05891 0.031 0 - - - r_gripper_r_parallel_link - r_gripper_r_finger_tip_link - r_gripper_r_finger_tip_link - 0 0 1 - -0.018 -0.021 0 - - - r_gripper_l_parallel_link - r_gripper_l_finger_tip_link - r_gripper_l_finger_tip_link - 0 0 1 - -0.018 0.021 0 - - - r_gripper_l_finger_tip_link - r_gripper_r_finger_tip_link - r_gripper_r_finger_tip_link - 0 1 0 - - - - true - - - - true - - - - - - - - - - - - - true - - r_gripper_palm_link_geom - 100.0 - - true - 100.0 - r_gripper_palm_link - r_gripper_palm_bumper - - - - - - - true - 100.0 - r_gripper_palm_link - r_gripper_palm_pose_ground_truth - 0 0 0 - 0 0 0 - 0.0 - map - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - - - - - - - - - - 32.6525111499 - - - true - - - - - - - true - - - - - - - - - 63.1552452977 - - - - - - 61.8948225713 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - - - - - - - - -90.5142857143 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -1.0 - - - true - - - - - - - - - -36.167452007 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - - - true - - - - - - - true - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - - l_gripper_l_finger_link_geom - 100.0 - - true - 100.0 - l_gripper_l_finger_link - l_gripper_l_finger_bumper - - - - - - - - - - - - - - - - - true - - l_gripper_r_finger_link_geom - 100.0 - - true - l_gripper_r_finger_link - 100.0 - l_gripper_r_finger_bumper - - - - - - - - - - - - - - - - true - false - - l_gripper_l_finger_tip_link_geom - 100.0 - - true - l_gripper_l_finger_tip_link - 100.0 - l_gripper_l_finger_tip_bumper - - - - - - - - - - - - - - - - true - false - - l_gripper_r_finger_tip_link_geom - 100.0 - - true - l_gripper_r_finger_tip_link - 100.0 - l_gripper_r_finger_tip_bumper - - - - - - - - - - - true - 100.0 - l_gripper_l_finger_link - l_gripper_l_finger_pose_ground_truth - 0.0 - base_link - - - - true - 100.0 - l_gripper_l_finger_link - l_gripper_l_finger_force_ground_truth - l_gripper_l_finger_link - - - - - - - - - - - - true - 0.17126 - 7.7562e-05 - 1.49095e-06 - -9.83385e-06 - 0.000197083 - -3.06125e-06 - 0.000181054 - 0.03598 - 0.0173 - -0.00164 - 0.82991 0.219 0.790675 - 0 -0 0 - true - false - - - true - 0.17389 - 7.73841e-05 - -2.09309e-06 - -8.36228e-06 - 0.000198474 - 2.4611e-06 - 0.00018107 - 0.03576 - -0.01736 - -0.00095 - 0.82991 0.157 0.790675 - 0 -0 0 - true - false - - - - - l_gripper_r_parallel_link - l_gripper_palm_link - l_gripper_palm_link - 0 0 -1 - 0.2 - 0.05891 -0.031 0 - - - l_gripper_l_parallel_link - l_gripper_palm_link - l_gripper_palm_link - 0 0 1 - 0.2 - 0.05891 0.031 0 - - - l_gripper_r_parallel_link - l_gripper_r_finger_tip_link - l_gripper_r_finger_tip_link - 0 0 1 - -0.018 -0.021 0 - - - l_gripper_l_parallel_link - l_gripper_l_finger_tip_link - l_gripper_l_finger_tip_link - 0 0 1 - -0.018 0.021 0 - - - l_gripper_l_finger_tip_link - l_gripper_r_finger_tip_link - l_gripper_r_finger_tip_link - 0 1 0 - - - - true - - - - true - - - - - - - - - - - - - true - - l_gripper_palm_link_geom - 100.0 - - true - 100.0 - l_gripper_palm_link - l_gripper_palm_bumper - - - - - - - true - 100.0 - l_gripper_palm_link - l_gripper_palm_pose_ground_truth - 0 0 0 - 0 0 0 - 0.0 - map - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 640 480 - L8 - 90 - 0.1 - 100 - 25.0 - - true - 25.0 - l_forearm_cam/image_raw - l_forearm_cam/camera_info - l_forearm_cam_optical_frame - 0 - 320.5 - 320.5 - 240.5 - - - 320 - 0.00000001 - 0.00000001 - 0.00000001 - 0.00000001 - 0.00000001 - - - - true - PR2/Blue - - - - - - - - - - - - - - - - - - - - - - 640 480 - L8 - 90 - 0.1 - 100 - 25.0 - - true - 25.0 - r_forearm_cam/image_raw - r_forearm_cam/camera_info - r_forearm_cam_optical_frame - 0 - 320.5 - 320.5 - 240.5 - - - 320 - 0.00000001 - 0.00000001 - 0.00000001 - 0.00000001 - 0.00000001 - - - - true - PR2/Blue - - - diff --git a/robot_state_publisher/test/test_one_link.cpp b/robot_state_publisher/test/test_one_link.cpp deleted file mode 100644 index 3115042..0000000 --- a/robot_state_publisher/test/test_one_link.cpp +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include -#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("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; -} diff --git a/robot_state_publisher/test/test_one_link.launch b/robot_state_publisher/test/test_one_link.launch deleted file mode 100644 index f46d8ee..0000000 --- a/robot_state_publisher/test/test_one_link.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/robot_state_publisher/test/test_publisher.cpp b/robot_state_publisher/test/test_publisher.cpp deleted file mode 100644 index 530170e..0000000 --- a/robot_state_publisher/test/test_publisher.cpp +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include -#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; -} diff --git a/robot_state_publisher/test/test_publisher.launch b/robot_state_publisher/test/test_publisher.launch deleted file mode 100644 index 2c954f3..0000000 --- a/robot_state_publisher/test/test_publisher.launch +++ /dev/null @@ -1,8 +0,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 deleted file mode 100644 index 530ffd1..0000000 --- a/robot_state_publisher/test/test_two_links_fixed_joint.cpp +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include -#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; -} diff --git a/robot_state_publisher/test/test_two_links_fixed_joint.launch b/robot_state_publisher/test/test_two_links_fixed_joint.launch deleted file mode 100644 index 9ad67b0..0000000 --- a/robot_state_publisher/test/test_two_links_fixed_joint.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/robot_state_publisher/test/test_two_links_moving_joint.cpp b/robot_state_publisher/test/test_two_links_moving_joint.cpp deleted file mode 100644 index 2f05f79..0000000 --- a/robot_state_publisher/test/test_two_links_moving_joint.cpp +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include -#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("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; -} diff --git a/robot_state_publisher/test/test_two_links_moving_joint.launch b/robot_state_publisher/test/test_two_links_moving_joint.launch deleted file mode 100644 index 51a4b55..0000000 --- a/robot_state_publisher/test/test_two_links_moving_joint.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/robot_state_publisher/test/two_links_fixed_joint.urdf b/robot_state_publisher/test/two_links_fixed_joint.urdf deleted file mode 100644 index 8bd84ef..0000000 --- a/robot_state_publisher/test/two_links_fixed_joint.urdf +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - \ No newline at end of file diff --git a/robot_state_publisher/test/two_links_moving_joint.urdf b/robot_state_publisher/test/two_links_moving_joint.urdf deleted file mode 100644 index eb50dd6..0000000 --- a/robot_state_publisher/test/two_links_moving_joint.urdf +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - \ No newline at end of file