pax_global_header00006660000000000000000000000064135313046210014510gustar00rootroot0000000000000052 comment=89ca4150e243f9215a82fb8f8bbbe15739771184 robot_state_publisher-1.14.0/000077500000000000000000000000001353130462100161755ustar00rootroot00000000000000robot_state_publisher-1.14.0/CHANGELOG.rst000066400000000000000000000156631353130462100202310ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package robot_state_publisher ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 1.14.0 (2019-08-27) ------------------- * Revert "Remove dependency on tf and tf_prefix support (`#82 `_)" (`#113 `_) * update how compiler flags are added (`#104 `_) * update install destination in CMakeLists.txt (`#103 `_) * Remove treefksolver completely from the repository. (`#100 `_) * changed return code from -1 to 1 since its considered a reserved bash exit code (`#98 `_) * Fixed problem when building static library version (`#92 `_) (`#96 `_) * Add Ian as a maintainer for robot_state_publisher. (`#94 `_) * added warning when joint is found in joint message but not in the urdf (`#83 `_) * added ros_warn if JointStateMessage is older than 30 seconds (`#84 `_) * Add tcp_no_delay to joint_states subscriber (`#80 `_) (`#85 `_) * Remove dependency on tf and tf_prefix support (`#82 `_) * make rostest in CMakeLists optional (`ros/rosdistro#3010 `_) (`#75 `_) * Added c++11 target_compile_options (`#78 `_) * Contributors: Chris Lalancette, James Xu, Lukas Bulwahn, Shane Loretz, betab0t, jgueldenstein 1.13.5 (2017-04-11) ------------------- * Style cleanup throughout the tree. * add Chris and Shane as maintainers (`#70 `_) * Contributors: Chris Lalancette, William Woodall 1.13.4 (2017-01-05) ------------------- * Use ``urdf::*ShredPtr`` instead of ``boost::shared_ptr`` (`#60 `_) * Error log for empty JointState.position was downgraded to a throttled warning (`#64 `_) * Contributors: Jochen Sprickerhof, Sébastien BARTHÉLÉMY 1.13.3 (2016-10-20) ------------------- * Added a new parameter "ignore_timestamp" (`#65 `_) * Fixed joints are not published over tf_static by default (`#56 `_) * Fixed segfault on undefined robot_description (`#61 `_) * Fixed cmake eigen3 warning (`#62 `_) * Contributors: Davide Faconti, Ioan A Sucan, Johannes Meyer, Robert Haschke 1.13.2 (2016-06-10) ------------------- * Add target_link_libraries for joint_state_listener library + install it (`#54 `_) * Contributors: Kartik Mohta 1.13.1 (2016-05-20) ------------------- * Add back future dating for robot_state_publisher (`#49 `_) (`#51 `_) * Fix subclassing test (`#48 `_) * Support for subclassing (`#45 `_) * Add joint_state_listener as a library * Contributors: Jackie Kay 1.13.0 (2016-04-12) ------------------- * fix bad rebase * Contributors: Jackie Kay, Paul Bovbel 1.12.1 (2016-02-22) ------------------- * Merge pull request `#42 `_ from ros/fix_tests_jade Fix tests for Jade * Correct failing tests * Re-enabling rostests * Merge pull request `#39 `_ from scpeters/issue_38 * Fix API break in publishFixedTransforms A bool argument was added to RobotStatePublisher::publishFixedTransforms which broke API. I've added a default value of false, to match the default specified in the JointStateListener constructor. * Contributors: Jackie Kay, Jonathan Bohren, Steven Peters 1.12.0 (2015-10-21) ------------------- * Merge pull request `#37 `_ from clearpathrobotics/static-default Publish fixed joints over tf_static by default * Merge pull request `#34 `_ from ros/tf2-static-jade Port to tf2 and enable using static broadcaster * Merge pull request `#32 `_ from `shadow-robot/fix_issue#19 `_ Check URDF to distinguish fixed joints from floating joints. Floating joint are ignored by the publisher. * Merge pull request `#26 `_ from xqms/remove-debug get rid of argv[0] debug output on startup * Contributors: David Lu!!, Ioan A Sucan, Jackie Kay, Max Schwarz, Paul Bovbel, Toni Oliver 1.11.1 (2016-02-22) ------------------- * Merge pull request `#41 `_ from ros/fix_tests_indigo Re-enable and clean up rostests * Correct failing tests * Re-enabling rostests * Fix API break in publishFixedTransforms A bool argument was added to RobotStatePublisher::publishFixedTransforms which broke API. I've added a default value of false, to match the default specified in the JointStateListener constructor. * Contributors: Jackie Kay, Jonathan Bohren, Steven Peters 1.11.0 (2015-10-21) ------------------- * Merge pull request `#28 `_ from clearpathrobotics/tf2-static 1.10.4 (2014-11-30) ------------------- * Merge pull request `#21 `_ from rcodddow/patch-1 * Fix for joint transforms not being published anymore after a clock reset (e.g. when playing a bagfile and looping) * Contributors: Ioan A Sucan, Robert Codd-Downey, Timm Linder 1.10.3 (2014-07-24) ------------------- * add version depend on orocos_kdl >= 1.3.0 Conflicts: package.xml * Update KDL SegmentMap interface to optionally use shared pointers The KDL Tree API optionally uses shared pointers on platforms where the STL containers don't support incomplete types. * Contributors: Brian Jensen, William Woodall 1.10.0 (2014-03-03) ------------------- * minor style fixes * Add support for mimic tag. * Contributors: Ioan Sucan, Konrad Banachowicz robot_state_publisher-1.14.0/CMakeLists.txt000066400000000000000000000102461353130462100207400ustar00rootroot00000000000000cmake_minimum_required(VERSION 2.8) project(robot_state_publisher) include(CheckCXXCompilerFlag) unset(COMPILER_SUPPORTS_CXX11 CACHE) if(MSVC) # https://docs.microsoft.com/en-us/cpp/build/reference/std-specify-language-standard-version # MSVC will fail the following check since it does not have the c++11 switch # however, c++11 is always enabled (the newer /std:c++14 is enabled by default) check_cxx_compiler_flag(/std:c++11 COMPILER_SUPPORTS_CXX11) if(COMPILER_SUPPORTS_CXX11) add_compile_options(/std:c++11) endif() # MSVC does not support the Wextra flag add_compile_options(/Wall) else() check_cxx_compiler_flag(-std=c++11 COMPILER_SUPPORTS_CXX11) if(COMPILER_SUPPORTS_CXX11) add_compile_options(-std=c++11) endif() add_compile_options(-Wall -Wextra) endif() find_package(orocos_kdl REQUIRED) find_package(catkin REQUIRED COMPONENTS roscpp rosconsole rostime tf tf2_ros tf2_kdl kdl_parser ) find_package(Eigen3 REQUIRED) find_package(urdfdom_headers REQUIRED) catkin_package( LIBRARIES ${PROJECT_NAME}_solver INCLUDE_DIRS include DEPENDS roscpp rosconsole rostime tf2_ros tf2_kdl kdl_parser orocos_kdl urdfdom_headers ) include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS}) include_directories(include ${catkin_INCLUDE_DIRS} ${orocos_kdl_INCLUDE_DIRS} ${urdfdom_headers_INCLUDE_DIRS}) link_directories(${orocos_kdl_LIBRARY_DIRS}) add_library(${PROJECT_NAME}_solver src/robot_state_publisher.cpp ) target_link_libraries(${PROJECT_NAME}_solver ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES}) add_library(joint_state_listener src/joint_state_listener.cpp) target_link_libraries(joint_state_listener ${PROJECT_NAME}_solver ${orocos_kdl_LIBRARIES}) add_executable(${PROJECT_NAME} src/joint_state_listener.cpp) target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_solver ${orocos_kdl_LIBRARIES}) # compile the same executable using the old name as well add_executable(state_publisher src/joint_state_listener.cpp) target_link_libraries(state_publisher ${PROJECT_NAME}_solver ${orocos_kdl_LIBRARIES}) # Tests if (CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) add_rostest_gtest(test_one_link ${CMAKE_CURRENT_SOURCE_DIR}/test/test_one_link.launch test/test_one_link.cpp) target_link_libraries(test_one_link ${catkin_LIBRARIES} ${PROJECT_NAME}_solver) add_rostest_gtest(test_two_links_fixed_joint ${CMAKE_CURRENT_SOURCE_DIR}/test/test_two_links_fixed_joint.launch test/test_two_links_fixed_joint.cpp) target_link_libraries(test_two_links_fixed_joint ${catkin_LIBRARIES} ${PROJECT_NAME}_solver) add_rostest_gtest(test_two_links_moving_joint ${CMAKE_CURRENT_SOURCE_DIR}/test/test_two_links_moving_joint.launch test/test_two_links_moving_joint.cpp) target_link_libraries(test_two_links_moving_joint ${catkin_LIBRARIES} ${PROJECT_NAME}_solver) # Download needed data file catkin_download_test_data( joint_states_indexed_bag http://wiki.ros.org/robot_state_publisher/data?action=AttachFile&do=get&target=joint_states_indexed.bag DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test FILENAME joint_states_indexed.bag MD5 793e0b566ebe4698265a936b92fa2bba) add_rostest_gtest(test_joint_states_bag ${CMAKE_CURRENT_SOURCE_DIR}/test/test_joint_states_bag.launch test/test_joint_states_bag.cpp) target_link_libraries(test_joint_states_bag ${catkin_LIBRARIES} ${PROJECT_NAME}_solver) add_rostest_gtest(test_subclass ${CMAKE_CURRENT_SOURCE_DIR}/test/test_subclass.launch test/test_subclass.cpp) target_link_libraries(test_subclass ${catkin_LIBRARIES} ${PROJECT_NAME}_solver joint_state_listener) install(FILES test/one_link.urdf test/pr2.urdf test/two_links_fixed_joint.urdf test/two_links_moving_joint.urdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/test) endif() # Install library install(TARGETS ${PROJECT_NAME}_solver joint_state_listener ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) # Install executable install(TARGETS ${PROJECT_NAME} state_publisher RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) robot_state_publisher-1.14.0/doc.dox000066400000000000000000000003671353130462100174640ustar00rootroot00000000000000/** @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 **/ robot_state_publisher-1.14.0/include/000077500000000000000000000000001353130462100176205ustar00rootroot00000000000000robot_state_publisher-1.14.0/include/robot_state_publisher/000077500000000000000000000000001353130462100242225ustar00rootroot00000000000000robot_state_publisher-1.14.0/include/robot_state_publisher/joint_state_listener.h000066400000000000000000000057351353130462100306350ustar00rootroot00000000000000/********************************************************************* * 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 #include "robot_state_publisher/robot_state_publisher.h" using namespace std; using namespace ros; using namespace KDL; typedef boost::shared_ptr JointStateConstPtr; typedef std::map MimicMap; 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, const MimicMap& m, const urdf::Model& model = urdf::Model()); /// Destructor ~JointStateListener(); protected: virtual void callbackJointState(const JointStateConstPtr& state); virtual void callbackFixedJoint(const ros::TimerEvent& e); std::string tf_prefix_; Duration publish_interval_; robot_state_publisher::RobotStatePublisher state_publisher_; Subscriber joint_state_sub_; ros::Timer timer_; ros::Time last_callback_time_; std::map last_publish_time_; MimicMap mimic_; bool use_tf_static_; bool ignore_timestamp_; }; } #endif robot_state_publisher-1.14.0/include/robot_state_publisher/robot_state_publisher.h000066400000000000000000000064761353130462100310120ustar00rootroot00000000000000/********************************************************************* * 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 #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, const urdf::Model& model = urdf::Model()); /// 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 */ virtual void publishTransforms(const std::map& joint_positions, const ros::Time& time, const std::string& tf_prefix); virtual void publishFixedTransforms(const std::string& tf_prefix, bool use_tf_static = false); protected: virtual void addChildren(const KDL::SegmentMap::const_iterator segment); std::map segments_, segments_fixed_; const urdf::Model& model_; tf2_ros::TransformBroadcaster tf_broadcaster_; tf2_ros::StaticTransformBroadcaster static_tf_broadcaster_; }; } #endif robot_state_publisher-1.14.0/package.xml000066400000000000000000000041131353130462100203110ustar00rootroot00000000000000 robot_state_publisher 1.14.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. Ioan Sucan Jackie Kay Wim Meeussen Chris Lalancette Ian McMahon Shane Loretz BSD http://wiki.ros.org/robot_state_publisher catkin eigen kdl_parser orocos_kdl rosconsole roscpp rostime sensor_msgs tf tf2_ros tf2_kdl liburdfdom-headers-dev catkin eigen kdl_parser orocos_kdl rosconsole roscpp rostime sensor_msgs tf tf2_ros tf2_kdl rostest robot_state_publisher-1.14.0/src/000077500000000000000000000000001353130462100167645ustar00rootroot00000000000000robot_state_publisher-1.14.0/src/joint_state_listener.cpp000066400000000000000000000170521353130462100237250ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ /* Author: Wim Meeussen */ #include #include #include #include #include "robot_state_publisher/robot_state_publisher.h" #include "robot_state_publisher/joint_state_listener.h" using namespace std; using namespace ros; using namespace KDL; using namespace robot_state_publisher; JointStateListener::JointStateListener(const KDL::Tree& tree, const MimicMap& m, const urdf::Model& model) : state_publisher_(tree, model), mimic_(m) { ros::NodeHandle n_tilde("~"); ros::NodeHandle n; // set publish frequency double publish_freq; n_tilde.param("publish_frequency", publish_freq, 50.0); // set whether to use the /tf_static latched static transform broadcaster n_tilde.param("use_tf_static", use_tf_static_, true); // ignore_timestamp_ == true, joins_states messages are accepted, no matter their timestamp n_tilde.param("ignore_timestamp", ignore_timestamp_, false); // get the tf_prefix parameter from the closest namespace std::string tf_prefix_key; n_tilde.searchParam("tf_prefix", tf_prefix_key); n_tilde.param(tf_prefix_key, tf_prefix_, std::string("")); publish_interval_ = ros::Duration(1.0/max(publish_freq, 1.0)); // Setting tcpNoNelay tells the subscriber to ask publishers that connect // to set TCP_NODELAY on their side. This prevents some joint_state messages // from being bundled together, increasing the latency of one of the messages. ros::TransportHints transport_hints; transport_hints.tcpNoDelay(true); // subscribe to joint state joint_state_sub_ = n.subscribe("joint_states", 1, &JointStateListener::callbackJointState, this, transport_hints); // trigger to publish fixed joints // if using static transform broadcaster, this will be a oneshot trigger and only run once timer_ = n_tilde.createTimer(publish_interval_, &JointStateListener::callbackFixedJoint, this, use_tf_static_); } JointStateListener::~JointStateListener() {} void JointStateListener::callbackFixedJoint(const ros::TimerEvent& e) { (void)e; state_publisher_.publishFixedTransforms(tf_prefix_, use_tf_static_); } void JointStateListener::callbackJointState(const JointStateConstPtr& state) { if (state->name.size() != state->position.size()){ if (state->position.empty()){ const int throttleSeconds = 300; ROS_WARN_THROTTLE(throttleSeconds, "Robot state publisher ignored a JointState message about joint(s) " "\"%s\"(,...) whose position member was empty. This message will " "not reappear for %d seconds.", state->name[0].c_str(), throttleSeconds); } else { ROS_ERROR("Robot state publisher ignored an invalid JointState message"); } return; } // check if we moved backwards in time (e.g. when playing a bag file) ros::Time now = ros::Time::now(); if (last_callback_time_ > now) { // force re-publish of joint transforms ROS_WARN("Moved backwards in time (probably because ROS clock was reset), re-publishing joint transforms!"); last_publish_time_.clear(); } ros::Duration warning_threshold(30.0); if ((state->header.stamp + warning_threshold) < now) { ROS_WARN_THROTTLE(10, "Received JointState is %f seconds old.", (now - state->header.stamp).toSec()); } last_callback_time_ = now; // determine least recently published joint ros::Time last_published = 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 (ignore_timestamp_ || 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])); } for (MimicMap::iterator i = mimic_.begin(); i != mimic_.end(); i++) { if(joint_positions.find(i->second->joint_name) != joint_positions.end()) { double pos = joint_positions[i->second->joint_name] * i->second->multiplier + i->second->offset; joint_positions.insert(make_pair(i->first, pos)); } } state_publisher_.publishTransforms(joint_positions, state->header.stamp, tf_prefix_); // 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; ///////////////////////////////////////// begin deprecation warning std::string exe_name = argv[0]; std::size_t slash = exe_name.find_last_of("/"); if (slash != std::string::npos) { exe_name = exe_name.substr(slash + 1); } if (exe_name == "state_publisher") { ROS_WARN("The 'state_publisher' executable is deprecated. Please use 'robot_state_publisher' instead"); } ///////////////////////////////////////// end deprecation warning // gets the location of the robot description on the parameter server urdf::Model model; if (!model.initParam("robot_description")) return 1; KDL::Tree tree; if (!kdl_parser::treeFromUrdfModel(model, tree)) { ROS_ERROR("Failed to extract kdl tree from xml robot description"); return 1; } MimicMap mimic; for(std::map< std::string, urdf::JointSharedPtr >::iterator i = model.joints_.begin(); i != model.joints_.end(); i++) { if(i->second->mimic) { mimic.insert(make_pair(i->first, i->second->mimic)); } } JointStateListener state_publisher(tree, mimic, model); ros::spin(); return 0; } robot_state_publisher-1.14.0/src/robot_state_publisher.cpp000066400000000000000000000131001353130462100240650ustar00rootroot00000000000000/********************************************************************* * 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 "robot_state_publisher/robot_state_publisher.h" using namespace std; using namespace ros; namespace robot_state_publisher { RobotStatePublisher::RobotStatePublisher(const KDL::Tree& tree, const urdf::Model& model) : model_(model) { // 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 = GetTreeElementSegment(segment->second).getName(); const std::vector& children = GetTreeElementChildren(segment->second); for (unsigned int i=0; isecond); SegmentPair s(GetTreeElementSegment(children[i]->second), root, child.getName()); if (child.getJoint().getType() == KDL::Joint::None) { if (model_.getJoint(child.getJoint().getName()) && model_.getJoint(child.getJoint().getName())->type == urdf::Joint::FLOATING) { ROS_INFO("Floating joint. Not adding segment from %s to %s. This TF can not be published based on joint_states info", root.c_str(), child.getName().c_str()); } else { 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, const std::string& tf_prefix) { ROS_DEBUG("Publishing transforms for moving joints"); std::vector tf_transforms; // 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()) { geometry_msgs::TransformStamped tf_transform = tf2::kdlToTransform(seg->second.segment.pose(jnt->second)); tf_transform.header.stamp = time; tf_transform.header.frame_id = tf::resolve(tf_prefix, seg->second.root); tf_transform.child_frame_id = tf::resolve(tf_prefix, seg->second.tip); tf_transforms.push_back(tf_transform); } else { ROS_WARN_THROTTLE(10, "Joint state with name: \"%s\" was received but not found in URDF", jnt->first.c_str()); } } tf_broadcaster_.sendTransform(tf_transforms); } // publish fixed transforms void RobotStatePublisher::publishFixedTransforms(const std::string& tf_prefix, bool use_tf_static) { ROS_DEBUG("Publishing transforms for fixed joints"); std::vector tf_transforms; geometry_msgs::TransformStamped tf_transform; // loop over all fixed segments for (map::const_iterator seg=segments_fixed_.begin(); seg != segments_fixed_.end(); seg++) { geometry_msgs::TransformStamped tf_transform = tf2::kdlToTransform(seg->second.segment.pose(0)); tf_transform.header.stamp = ros::Time::now(); if (!use_tf_static) { tf_transform.header.stamp += ros::Duration(0.5); } tf_transform.header.frame_id = tf::resolve(tf_prefix, seg->second.root); tf_transform.child_frame_id = tf::resolve(tf_prefix, seg->second.tip); tf_transforms.push_back(tf_transform); } if (use_tf_static) { static_tf_broadcaster_.sendTransform(tf_transforms); } else { tf_broadcaster_.sendTransform(tf_transforms); } } } robot_state_publisher-1.14.0/test/000077500000000000000000000000001353130462100171545ustar00rootroot00000000000000robot_state_publisher-1.14.0/test/one_link.urdf000066400000000000000000000001041353130462100216270ustar00rootroot00000000000000 robot_state_publisher-1.14.0/test/pr2.urdf000066400000000000000000003626031353130462100205530ustar00rootroot00000000000000 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 robot_state_publisher-1.14.0/test/test_joint_states_bag.cpp000066400000000000000000000074271353130462100242500ustar00rootroot00000000000000/********************************************************************* * 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 #include "robot_state_publisher/joint_state_listener.h" using namespace ros; using namespace tf2_ros; 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"); Buffer buffer; TransformListener tf(buffer); ROS_INFO("Waiting for bag to complete"); Duration(15.0).sleep(); ASSERT_TRUE(buffer.canTransform("base_link", "torso_lift_link", Time())); ASSERT_TRUE(buffer.canTransform("base_link", "r_gripper_palm_link", Time())); ASSERT_TRUE(buffer.canTransform("base_link", "r_gripper_palm_link", Time())); ASSERT_TRUE(buffer.canTransform("l_gripper_palm_link", "r_gripper_palm_link", Time())); ASSERT_TRUE(buffer.canTransform("l_gripper_palm_link", "fl_caster_r_wheel_link", Time())); ASSERT_FALSE(buffer.canTransform("base_link", "wim_link", Time())); geometry_msgs::TransformStamped t = buffer.lookupTransform("base_link", "r_gripper_palm_link", Time()); EXPECT_NEAR(t.transform.translation.x, 0.769198, EPS); EXPECT_NEAR(t.transform.translation.y, -0.188800, EPS); EXPECT_NEAR(t.transform.translation.z, 0.764914, EPS); t = buffer.lookupTransform("l_gripper_palm_link", "r_gripper_palm_link", Time()); EXPECT_NEAR(t.transform.translation.x, 0.000384222, EPS); EXPECT_NEAR(t.transform.translation.y, -0.376021, EPS); EXPECT_NEAR(t.transform.translation.z, -1.0858e-05, EPS); SUCCEED(); } int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "test_robot_state_publisher"); g_argc = argc; g_argv = argv; int res = RUN_ALL_TESTS(); return res; } robot_state_publisher-1.14.0/test/test_joint_states_bag.launch000066400000000000000000000007501353130462100247300ustar00rootroot00000000000000 robot_state_publisher-1.14.0/test/test_one_link.cpp000066400000000000000000000062571353130462100225270ustar00rootroot00000000000000/********************************************************************* * 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 tf2_ros; using namespace robot_state_publisher; #define EPS 0.01 class TestPublisher : public testing::Test { public: JointStateListener* publisher; protected: /// constructor TestPublisher() {} /// Destructor ~TestPublisher() {} }; TEST_F(TestPublisher, test) { { ros::NodeHandle n_tilde; std::string robot_description; ASSERT_TRUE(n_tilde.getParam("robot_description", robot_description)); } ROS_INFO("Creating tf listener"); Buffer buffer; TransformListener tf(buffer); // ROS_INFO("Publishing joint state to robot state publisher"); /*ros::NodeHandle n; ros::Publisher js_pub = n.advertise("test_one_link/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::spinOnce(); ros::Duration(0.1).sleep(); }*/ ASSERT_TRUE(buffer.canTransform("link1", "link1", Time())); SUCCEED(); } int main(int argc, char** argv) { ros::init(argc, argv, "test_one_link"); testing::InitGoogleTest(&argc, argv); int res = RUN_ALL_TESTS(); return res; } robot_state_publisher-1.14.0/test/test_one_link.launch000066400000000000000000000004621353130462100232070ustar00rootroot00000000000000 robot_state_publisher-1.14.0/test/test_subclass.cpp000066400000000000000000000071031353130462100225370ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2016, Open Source Robotics Foundation, 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 Willow Garage, Inc. 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. */ #include #include #include #include "robot_state_publisher/joint_state_listener.h" #include "robot_state_publisher/robot_state_publisher.h" namespace robot_state_publisher_test { class AccessibleJointStateListener : public robot_state_publisher::JointStateListener { public: AccessibleJointStateListener( const KDL::Tree& tree, const MimicMap& m, const urdf::Model& model) : robot_state_publisher::JointStateListener(tree, m, model) { } bool usingTfStatic() const { return use_tf_static_; } }; class AccessibleRobotStatePublisher : public robot_state_publisher::RobotStatePublisher { public: AccessibleRobotStatePublisher(const KDL::Tree& tree, const urdf::Model& model) : robot_state_publisher::RobotStatePublisher(tree, model) { } const urdf::Model & getModel() const { return model_; } }; } // robot_state_publisher_test TEST(TestRobotStatePubSubclass, robot_state_pub_subclass) { urdf::Model model; model.initParam("robot_description"); KDL::Tree tree; if (!kdl_parser::treeFromUrdfModel(model, tree)){ ROS_ERROR("Failed to extract kdl tree from xml robot description"); FAIL(); } MimicMap mimic; for(std::map< std::string, urdf::JointSharedPtr >::iterator i = model.joints_.begin(); i != model.joints_.end(); i++){ if(i->second->mimic){ mimic.insert(make_pair(i->first, i->second->mimic)); } } robot_state_publisher_test::AccessibleRobotStatePublisher state_pub(tree, model); EXPECT_EQ(model.name_, state_pub.getModel().name_); EXPECT_EQ(model.root_link_, state_pub.getModel().root_link_); robot_state_publisher_test::AccessibleJointStateListener state_listener(tree, mimic, model); EXPECT_TRUE(state_listener.usingTfStatic()); } int main(int argc, char** argv) { ros::init(argc, argv, "test_subclass"); testing::InitGoogleTest(&argc, argv); int res = RUN_ALL_TESTS(); return res; } robot_state_publisher-1.14.0/test/test_subclass.launch000066400000000000000000000004061353130462100232260ustar00rootroot00000000000000 robot_state_publisher-1.14.0/test/test_two_links_fixed_joint.cpp000066400000000000000000000064701353130462100253210ustar00rootroot00000000000000/********************************************************************* * 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 #include "robot_state_publisher/joint_state_listener.h" using namespace ros; using namespace tf2_ros; using namespace robot_state_publisher; #define EPS 0.01 class TestPublisher : public testing::Test { public: JointStateListener* publisher; protected: /// constructor TestPublisher() {} /// Destructor ~TestPublisher() {} }; TEST_F(TestPublisher, test) { { ros::NodeHandle n_tilde; std::string robot_description; ASSERT_TRUE(n_tilde.getParam("robot_description", robot_description)); } ROS_INFO("Creating tf listener"); Buffer buffer; TransformListener listener(buffer); for (unsigned int i = 0; i < 100 && !buffer.canTransform("link1", "link2", Time()); i++) { ros::Duration(0.1).sleep(); ros::spinOnce(); } ASSERT_TRUE(buffer.canTransform("link1", "link2", Time())); ASSERT_FALSE(buffer.canTransform("base_link", "wim_link", Time())); geometry_msgs::TransformStamped t = buffer.lookupTransform("link1", "link2", Time()); EXPECT_NEAR(t.transform.translation.x, 5.0, EPS); EXPECT_NEAR(t.transform.translation.y, 0.0, EPS); EXPECT_NEAR(t.transform.translation.z, 0.0, EPS); SUCCEED(); } int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "test_two_links_fixed_joint"); int res = RUN_ALL_TESTS(); return res; } robot_state_publisher-1.14.0/test/test_two_links_fixed_joint.launch000066400000000000000000000005711353130462100260050ustar00rootroot00000000000000 robot_state_publisher-1.14.0/test/test_two_links_moving_joint.cpp000066400000000000000000000074221353130462100255170ustar00rootroot00000000000000/********************************************************************* * 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 #include "robot_state_publisher/joint_state_listener.h" using namespace ros; using namespace tf2_ros; using namespace robot_state_publisher; #define EPS 0.01 class TestPublisher : public testing::Test { public: JointStateListener* publisher; protected: /// constructor TestPublisher() {} /// Destructor ~TestPublisher() {} }; TEST_F(TestPublisher, test) { { ros::NodeHandle n_tilde; std::string robot_description; ASSERT_TRUE(n_tilde.getParam("robot_description", robot_description)); } ROS_INFO("Creating tf listener"); Buffer buffer; TransformListener listener(buffer); ROS_INFO("Publishing joint state to robot state publisher"); ros::NodeHandle n; ros::Publisher js_pub = n.advertise("joint_states", 10); sensor_msgs::JointState js_msg; js_msg.name.push_back("joint1"); js_msg.position.push_back(M_PI); ros::Duration(1).sleep(); 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(); } for (unsigned int i = 0; i < 100 && !buffer.canTransform("link1", "link2", Time()); i++) { ros::Duration(0.1).sleep(); ros::spinOnce(); } EXPECT_FALSE(buffer.canTransform("base_link", "wim_link", Time())); ASSERT_TRUE(buffer.canTransform("link1", "link2", Time())); geometry_msgs::TransformStamped t = buffer.lookupTransform("link1", "link2", Time()); EXPECT_NEAR(t.transform.translation.x, 5.0, EPS); EXPECT_NEAR(t.transform.translation.y, 0.0, EPS); EXPECT_NEAR(t.transform.translation.z, 0.0, EPS); SUCCEED(); } int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "test_two_links_moving_joint"); ros::NodeHandle node; int res = RUN_ALL_TESTS(); return res; } robot_state_publisher-1.14.0/test/test_two_links_moving_joint.launch000066400000000000000000000005501353130462100262020ustar00rootroot00000000000000 robot_state_publisher-1.14.0/test/two_links_fixed_joint.urdf000066400000000000000000000003571353130462100244360ustar00rootroot00000000000000 robot_state_publisher-1.14.0/test/two_links_moving_joint.urdf000066400000000000000000000003651353130462100246350ustar00rootroot00000000000000