diff --git a/CMakeLists.txt b/CMakeLists.txt index 1e3c0ea..1fa19f1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,66 +11,43 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(kdl_parser REQUIRED) +find_package(orocos_kdl REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(sensor_msgs REQUIRED) -find_package(orocos_kdl REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2_ros REQUIRED) find_package(urdf REQUIRED) find_package(urdfdom_headers REQUIRED) -find_package(kdl_parser REQUIRED) -find_package(tf2_ros REQUIRED) -include_directories( - include - ${geometry_msgs_INCLUDE_DIRS} - ${kdl_parser_INCLUDE_DIRS} - ${orocos_kdl_INCLUDE_DIRS} - ${urdf_INCLUDE_DIRS} - ${urdfdom_headers_INCLUDE_DIRS} - ${rclcpp_INCLUDE_DIRS} - ${sensor_msgs_INCLUDE_DIRS} - ${tf2_ros_INCLUDE_DIRS}) +include_directories(include) add_library( - ${PROJECT_NAME}_solver + ${PROJECT_NAME}_node SHARED src/robot_state_publisher.cpp) -link_directories(${orocos_kdl_LIBRARY_DIRS}) -target_link_libraries( - ${PROJECT_NAME}_solver - ${geometry_msgs_LIBRARIES} - ${kdl_parser_LIBRARIES} - ${orocos_kdl_LIBRARIES} - ${rclcpp_LIBRARIES} - ${sensor_msgs_LIBRARIES} - ${tf2_ros_LIBRARIES} - ${urdf_LIBRARIES} - ${urdfdom_LIBRARIES}) - -add_library( - joint_state_listener - src/joint_state_listener.cpp) -target_link_libraries( - joint_state_listener - ${PROJECT_NAME}_solver - ${orocos_kdl_LIBRARIES}) +ament_target_dependencies(${PROJECT_NAME}_node + builtin_interfaces + geometry_msgs + kdl_parser + orocos_kdl + rclcpp + rclcpp_components + sensor_msgs + std_msgs + tf2_ros + urdf +) -add_executable( - ${PROJECT_NAME} - src/joint_state_listener.cpp) -target_link_libraries( - ${PROJECT_NAME} - joint_state_listener - ${orocos_kdl_LIBRARIES} - ${kdl_parser_LIBRARIES} - ${urdf_LIBRARIES} - ${rclcpp_LIBRARIES} - ${tf2_ros_LIBRARIES} - ${sensor_msgs_LIBRARIES}) +rclcpp_components_register_node(${PROJECT_NAME}_node + PLUGIN "robot_state_publisher::RobotStatePublisher" + EXECUTABLE robot_state_publisher) install( TARGETS - ${PROJECT_NAME}_solver - joint_state_listener + ${PROJECT_NAME}_node ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -79,26 +56,42 @@ install( install(DIRECTORY include/ DESTINATION include) -install( - TARGETS ${PROJECT_NAME} - DESTINATION lib/${PROJECT_NAME} +install(DIRECTORY launch urdf + DESTINATION share/${PROJECT_NAME} ) if(BUILD_TESTING) - find_package(ament_cmake_cppcheck REQUIRED) - find_package(ament_cmake_cpplint REQUIRED) - find_package(ament_cmake_lint_cmake REQUIRED) - find_package(ament_cmake_uncrustify REQUIRED) - # This forces cppcheck to consider all files in this project to be C++, - # including the headers which end with .h, which cppcheck would normally - # consider to be C instead. - ament_cppcheck(LANGUAGE "c++") - ament_cpplint() - ament_lint_cmake() - ament_uncrustify() + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + find_package(launch_testing_ament_cmake REQUIRED) + ament_find_gtest() + + include_directories(${GTEST_INCLUDE_DIRS}) + + add_executable(test_two_links_fixed_joint test/test_two_links_fixed_joint.cpp) + ament_target_dependencies(test_two_links_fixed_joint + rclcpp + sensor_msgs + tf2_ros + ) + target_link_libraries(test_two_links_fixed_joint ${GTEST_LIBRARIES}) + add_launch_test(test/two_links_fixed_joint-launch.py + ARGS "test_exe:=$") + + add_executable(test_two_links_moving_joint test/test_two_links_moving_joint.cpp) + ament_target_dependencies(test_two_links_moving_joint + rclcpp + sensor_msgs + tf2_ros + ) + target_link_libraries(test_two_links_moving_joint ${GTEST_LIBRARIES}) + add_launch_test(test/two_links_moving_joint-launch.py + ARGS "test_exe:=$") endif() -ament_export_libraries( - ${PROJECT_NAME}_solver - joint_state_listener) +ament_export_dependencies(builtin_interfaces orocos_kdl rclcpp sensor_msgs std_msgs tf2_ros urdf) +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}_node) ament_package() diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 0000000..5e190bd --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,3 @@ +Any contribution that you make to this repository will +be under the BSD license 2.0, as dictated by that +[license](https://opensource.org/licenses/BSD-3-Clause). diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..24d43da --- /dev/null +++ b/LICENSE @@ -0,0 +1,30 @@ +All rights reserved. + +Software License Agreement (BSD License 2.0) + +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 copyright holder 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. diff --git a/README.md b/README.md index e080249..644ca7c 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,25 @@ -# ROS2 Version of Robot State Publisher +Robot State Publisher +===================== -This package contains a forked version of the original ROS Robot State Publisher with all modifications to compile within a ROS2 Ecosystem. +This package contains the Robot State Publisher, a node and a class to publish the state of a robot to tf2. +Once the state gets published, it is available to all components in the system that also use tf2. +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. + +Examples showing how to pass the `robot_description` parameter using a launch file are available in the 'launch' subdirectory. + +Published Topics +---------------- +* `robot_description` (`std_msgs/msg/String`) - The description of the robot URDF as a string. Republishes the value set in the `robot_description` parameter, which is useful for getting informed of dynamic changes to the URDF. Published using the "transient local" quality of service, so subscribers should also be "transient local". +* `tf` (`tf2_msgs/msg/TFMessage`) - The transforms corresponding to the movable joints of the robot. +* `tf_static` (`tf2_msgs/msg/TFMessage`) - The transforms corresponding to the static joints of the robot. + +Subscribed Topics +----------------- +* `joint_states` (`sensor_msgs/msg/JointState`) - The joint state updates to the robot poses. The RobotStatePublisher class takes these updates, does transformations (such as mimic joints), and then publishes the results on the tf2 topics. + +Parameters +---------- +* `robot_description` (string) - The original description of the robot in URDF form. This *must* be set at robot_state_publisher startup time, or the node will fail to start. Updates to this parameter will be reflected in the `robot_description` topic. +* `publish_frequency` (double) - The frequency at which fixed transforms will be republished to the network. Defaults to 50.0. +* `use_tf_static` (bool) - Whether to publish fixed joints on the static broadcaster (`/tf_static` topic) or on the dynamic one (`/tf` topic). Defaults to true, so it publishes on the `/tf_static` topic. +* `ignore_timestamp` (bool) - Whether to accept all joint states no matter what the timestamp (true), or to only publish joint state updates if they are newer than the last publish_frequency (false). Defaults to false. diff --git a/doc.dox b/doc.dox deleted file mode 100644 index 9267270..0000000 --- a/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/include/robot_state_publisher/joint_state_listener.h b/include/robot_state_publisher/joint_state_listener.h deleted file mode 100644 index 6d43cb3..0000000 --- a/include/robot_state_publisher/joint_state_listener.h +++ /dev/null @@ -1,90 +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__JOINT_STATE_LISTENER_H_ -#define ROBOT_STATE_PUBLISHER__JOINT_STATE_LISTENER_H_ - -#include -#include -#include -#include - -#include "kdl/tree.hpp" -#include "rclcpp/rclcpp.hpp" -#include "sensor_msgs/msg/joint_state.hpp" -#include "urdf/model.h" - -#include "robot_state_publisher/robot_state_publisher.h" - -typedef std::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( - rclcpp::Node::SharedPtr node, const KDL::Tree & tree, const MimicMap & m, - const std::string & urdf_xml, const urdf::Model & model = urdf::Model()); - - /// Destructor - ~JointStateListener(); - -protected: - virtual void callbackJointState(const sensor_msgs::msg::JointState::SharedPtr state); - virtual void callbackFixedJoint(); - - rclcpp::Node::SharedPtr node_; - std::string tf_prefix_; - std::chrono::seconds publish_interval_; - robot_state_publisher::RobotStatePublisher state_publisher_; - rclcpp::Subscription::SharedPtr joint_state_sub_; - rclcpp::TimerBase::SharedPtr timer_; - std::chrono::time_point last_callback_time_; - std::map> last_publish_time_; - MimicMap mimic_; - bool use_tf_static_; - bool ignore_timestamp_; -}; - -} // namespace robot_state_publisher - -#endif // ROBOT_STATE_PUBLISHER__JOINT_STATE_LISTENER_H_ diff --git a/include/robot_state_publisher/robot_state_publisher.h b/include/robot_state_publisher/robot_state_publisher.h deleted file mode 100644 index ab8f847..0000000 --- a/include/robot_state_publisher/robot_state_publisher.h +++ /dev/null @@ -1,105 +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__ROBOT_STATE_PUBLISHER_H_ -#define ROBOT_STATE_PUBLISHER__ROBOT_STATE_PUBLISHER_H_ - -#include -#include - -#include "kdl/frames.hpp" -#include "kdl/segment.hpp" -#include "kdl/tree.hpp" - -#include "tf2_ros/static_transform_broadcaster.h" -#include "tf2_ros/transform_broadcaster.h" -#include "urdf/model.h" - -#include "std_msgs/msg/string.hpp" - -#include "rclcpp/rclcpp.hpp" - -namespace robot_state_publisher -{ - -class SegmentPair -{ -public: - SegmentPair(const KDL::Segment & p_segment, const std::string & p_root, const std::string & p_tip) - : segment(p_segment), root(p_root), tip(p_tip) {} - - KDL::Segment segment; - std::string root, tip; -}; - -class RobotStatePublisher -{ -public: - /** Constructor - * \param tree The kinematic model of a robot, represented by a KDL Tree - */ - RobotStatePublisher( - rclcpp::Node::SharedPtr node_handle, const KDL::Tree & tree, - const urdf::Model & model, const std::string & model_xml); - - /// Destructor - virtual ~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 builtin_interfaces::msg::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_; - std_msgs::msg::String model_xml_; - rclcpp::Publisher::SharedPtr description_pub_; - rclcpp::Clock::SharedPtr clock_; -}; - -} // namespace robot_state_publisher -#endif // ROBOT_STATE_PUBLISHER__ROBOT_STATE_PUBLISHER_H_ diff --git a/include/robot_state_publisher/robot_state_publisher.hpp b/include/robot_state_publisher/robot_state_publisher.hpp new file mode 100644 index 0000000..a36f4a1 --- /dev/null +++ b/include/robot_state_publisher/robot_state_publisher.hpp @@ -0,0 +1,113 @@ +// Copyright (c) 2008, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 copyright holders 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__ROBOT_STATE_PUBLISHER_HPP_ +#define ROBOT_STATE_PUBLISHER__ROBOT_STATE_PUBLISHER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +using MimicMap = std::map; + +namespace robot_state_publisher +{ + +class SegmentPair final +{ +public: + explicit 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; + std::string tip; +}; + +class RobotStatePublisher : public rclcpp::Node +{ +public: + /// Constructor + explicit RobotStatePublisher(const rclcpp::NodeOptions & options); + +protected: + /** 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 builtin_interfaces::msg::Time & time); + + void publishFixedTransforms(); + void addChildren(const KDL::SegmentMap::const_iterator segment); + void callbackJointState(const sensor_msgs::msg::JointState::SharedPtr state); + void setupURDF(const std::string & urdf_xml); + rcl_interfaces::msg::SetParametersResult parameterUpdate( + const std::vector & parameters); + + std::map segments_; + std::map segments_fixed_; + std::unique_ptr model_; + std::unique_ptr tf_broadcaster_; + std::unique_ptr static_tf_broadcaster_; + rclcpp::Publisher::SharedPtr description_pub_; + std::chrono::milliseconds publish_interval_ms_; + rclcpp::Subscription::SharedPtr joint_state_sub_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Time last_callback_time_; + std::map last_publish_time_; + MimicMap mimic_; + bool use_tf_static_; + bool ignore_timestamp_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_cb_; +}; + +} // namespace robot_state_publisher + +#endif // ROBOT_STATE_PUBLISHER__ROBOT_STATE_PUBLISHER_HPP_ diff --git a/launch/rsp-launch-urdf-file.xml b/launch/rsp-launch-urdf-file.xml new file mode 100644 index 0000000..e238327 --- /dev/null +++ b/launch/rsp-launch-urdf-file.xml @@ -0,0 +1,43 @@ + + + + + + + + diff --git a/launch/rsp-launch-urdf-file1.py b/launch/rsp-launch-urdf-file1.py new file mode 100644 index 0000000..0896799 --- /dev/null +++ b/launch/rsp-launch-urdf-file1.py @@ -0,0 +1,57 @@ +# Copyright (c) 2020 Open Source Robotics Foundation, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# 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 copyright holder 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. + +# This launch file shows how to launch robot_state_publisher with a simple +# URDF read from a file (found using FindPackageShare) and passed as the +# 'robot_description' parameter. + +import os + +import launch +import launch_ros.actions +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + pkg_share = FindPackageShare('robot_state_publisher').find('robot_state_publisher') + urdf_dir = os.path.join(pkg_share, 'urdf') + urdf_file = os.path.join(urdf_dir, 'test-desc.urdf') + with open(urdf_file, 'r') as infp: + robot_desc = infp.read() + + params = {'robot_description': robot_desc} + rsp = launch_ros.actions.Node(package='robot_state_publisher', + node_executable='robot_state_publisher', + output='both', + parameters=[params]) + + return launch.LaunchDescription([rsp]) diff --git a/launch/rsp-launch-urdf-file2.py b/launch/rsp-launch-urdf-file2.py new file mode 100644 index 0000000..628e9ef --- /dev/null +++ b/launch/rsp-launch-urdf-file2.py @@ -0,0 +1,57 @@ +# Copyright (c) 2020 Open Source Robotics Foundation, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# 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 copyright holder 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. + +# This launch file shows how to launch robot_state_publisher with a simple +# URDF read from a file (found using ament_get_package_share_directory) and +# passed as the 'robot_description' parameter. + +import os + +from ament_index_python.packages import get_package_share_directory + +import launch +import launch_ros.actions + + +def generate_launch_description(): + urdf_dir = os.path.join(get_package_share_directory('robot_state_publisher'), 'urdf') + urdf_file = os.path.join(urdf_dir, 'test-desc.urdf') + with open(urdf_file, 'r') as infp: + robot_desc = infp.read() + + params = {'robot_description': robot_desc} + rsp = launch_ros.actions.Node(package='robot_state_publisher', + node_executable='robot_state_publisher', + output='both', + parameters=[params]) + + return launch.LaunchDescription([rsp]) diff --git a/launch/rsp-launch-urdf-inline.py b/launch/rsp-launch-urdf-inline.py new file mode 100644 index 0000000..0fe0c0d --- /dev/null +++ b/launch/rsp-launch-urdf-inline.py @@ -0,0 +1,60 @@ +# Copyright (c) 2020 Open Source Robotics Foundation, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# 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 copyright holder 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. + +# This launch file shows how to launch robot_state_publisher with a +# simple URDF passed as the 'robot_description' parameter. + +import launch +import launch_ros.actions + + +def generate_launch_description(): + robot_desc = """\ + + + + + + + + + + +""" + + params = {'robot_description': robot_desc} + rsp = launch_ros.actions.Node(package='robot_state_publisher', + node_executable='robot_state_publisher', + output='both', + parameters=[params]) + + return launch.LaunchDescription([rsp]) diff --git a/launch/rsp-launch-urdf-inline.xml b/launch/rsp-launch-urdf-inline.xml new file mode 100644 index 0000000..b1300b5 --- /dev/null +++ b/launch/rsp-launch-urdf-inline.xml @@ -0,0 +1,56 @@ + + + + + + + + + diff --git a/launch/rsp-launch-xacro-api.py b/launch/rsp-launch-xacro-api.py new file mode 100644 index 0000000..5c85905 --- /dev/null +++ b/launch/rsp-launch-xacro-api.py @@ -0,0 +1,57 @@ +# Copyright (c) 2020 Open Source Robotics Foundation, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# 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 copyright holder 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. + +# This launch file shows how to launch robot_state_publisher with a URDF +# first processed using the xacro python API. + +import os + +import launch +import launch_ros.actions +from launch_ros.substitutions import FindPackageShare + +import xacro + + +def generate_launch_description(): + pkg_share = FindPackageShare('robot_state_publisher').find('robot_state_publisher') + urdf_dir = os.path.join(pkg_share, 'urdf') + xacro_file = os.path.join(urdf_dir, 'test-desc.urdf.xacro') + doc = xacro.process_file(xacro_file) + robot_desc = doc.toprettyxml(indent=' ') + params = {'robot_description': robot_desc} + rsp = launch_ros.actions.Node(package='robot_state_publisher', + node_executable='robot_state_publisher', + output='both', + parameters=[params]) + + return launch.LaunchDescription([rsp]) diff --git a/launch/rsp-launch-xacro-command-subst.py b/launch/rsp-launch-xacro-command-subst.py new file mode 100644 index 0000000..21f5508 --- /dev/null +++ b/launch/rsp-launch-xacro-command-subst.py @@ -0,0 +1,55 @@ +# Copyright (c) 2020 Open Source Robotics Foundation, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# 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 copyright holder 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. + +# This launch file shows how to launch robot_state_publisher with a URDF +# first processed using the xacro command-line program. + +import os + +import launch +import launch.substitutions +import launch_ros.actions +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + pkg_share = FindPackageShare('robot_state_publisher').find('robot_state_publisher') + urdf_dir = os.path.join(pkg_share, 'urdf') + xacro_file = os.path.join(urdf_dir, 'test-desc.urdf.xacro') + robot_desc = launch.substitutions.Command('xacro %s' % xacro_file) + params = {'robot_description': robot_desc} + rsp = launch_ros.actions.Node(package='robot_state_publisher', + node_executable='robot_state_publisher', + output='both', + parameters=[params]) + + return launch.LaunchDescription([rsp]) diff --git a/launch/rsp-launch-xacro-command-subst.xml b/launch/rsp-launch-xacro-command-subst.xml new file mode 100644 index 0000000..66539d2 --- /dev/null +++ b/launch/rsp-launch-xacro-command-subst.xml @@ -0,0 +1,44 @@ + + + + + + + + + diff --git a/launch/rsp-launch-xacro-popen.py b/launch/rsp-launch-xacro-popen.py new file mode 100644 index 0000000..ad175e2 --- /dev/null +++ b/launch/rsp-launch-xacro-popen.py @@ -0,0 +1,56 @@ +# Copyright (c) 2020 Open Source Robotics Foundation, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# 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 copyright holder 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. + +# This launch file shows how to launch robot_state_publisher with a URDF +# first processed using the xacro command-line program using Popen. + +import os +import subprocess + +import launch +import launch_ros.actions +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + pkg_share = FindPackageShare('robot_state_publisher').find('robot_state_publisher') + urdf_dir = os.path.join(pkg_share, 'urdf') + xacro_file = os.path.join(urdf_dir, 'test-desc.urdf.xacro') + p = subprocess.Popen(['xacro', xacro_file], stdout=subprocess.PIPE, stderr=subprocess.PIPE) + robot_desc, stderr = p.communicate() + params = {'robot_description': robot_desc.decode('utf-8')} + rsp = launch_ros.actions.Node(package='robot_state_publisher', + node_executable='robot_state_publisher', + output='both', + parameters=[params]) + + return launch.LaunchDescription([rsp]) diff --git a/package.xml b/package.xml index fe79015..0b49de0 100644 --- a/package.xml +++ b/package.xml @@ -3,35 +3,50 @@ robot_state_publisher 2.3.1 - ROS2 version of the robot_state_publisher package + + This package allows you to publish the state of a robot to + tf2. Once the state gets published, it is + available to all components in the system that also use tf2. + 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. + Karsten Knese BSD ament_cmake + builtin_interfaces geometry_msgs kdl_parser orocos_kdl rclcpp + rclcpp_components sensor_msgs + std_msgs tf2_ros urdf urdfdom_headers + builtin_interfaces geometry_msgs kdl_parser orocos_kdl rclcpp + rclcpp_components sensor_msgs + std_msgs tf2_ros urdf urdfdom_headers + ament_cmake_gtest ament_lint_auto ament_lint_common + launch_ros + launch_testing_ament_cmake ament_cmake - diff --git a/src/joint_state_listener.cpp b/src/joint_state_listener.cpp deleted file mode 100644 index 5c3b59d..0000000 --- a/src/joint_state_listener.cpp +++ /dev/null @@ -1,233 +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 "kdl/tree.hpp" -#include "kdl_parser/kdl_parser.hpp" -#include "urdf/model.h" - -#include "rclcpp/rclcpp.hpp" - -#include "robot_state_publisher/joint_state_listener.h" -#include "robot_state_publisher/robot_state_publisher.h" - -using namespace std::chrono_literals; - -namespace robot_state_publisher -{ - -JointStateListener::JointStateListener( - rclcpp::Node::SharedPtr node, const KDL::Tree & tree, - const MimicMap & m, const std::string & urdf_xml, const urdf::Model & model) -: node_(node), - state_publisher_(node, tree, model, urdf_xml), - mimic_(m) -{ - /* - * legacy code - // 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)); - */ - - use_tf_static_ = true; - ignore_timestamp_ = false; - tf_prefix_ = ""; - // auto publish_freq = 50.0; - // publish_interval_ = std::chrono::seconds(1.0/std::max(publish_freq, 1.0)); - publish_interval_ = 1s; - - // subscribe to joint state - joint_state_sub_ = node_->create_subscription( - "joint_states", 10, std::bind( - &JointStateListener::callbackJointState, this, - std::placeholders::_1)); - - // trigger to publish fixed joints - // if using static transform broadcaster, this will be a oneshot trigger and only run once - timer_ = node_->create_wall_timer(1s, std::bind(&JointStateListener::callbackFixedJoint, this)); -} - -JointStateListener::~JointStateListener() -{} - -void JointStateListener::callbackFixedJoint() -{ - state_publisher_.publishFixedTransforms(tf_prefix_, use_tf_static_); -} - -void JointStateListener::callbackJointState(const sensor_msgs::msg::JointState::SharedPtr state) -{ - if (state->name.size() != state->position.size()) { - if (state->position.empty()) { - fprintf(stderr, "Robot state publisher ignored a JointState message about joint(s) \"%s\"\n", - state->name[0].c_str()); - } else { - fprintf(stderr, "Robot state publisher ignored an invalid JointState message"); - } - return; - } - - // check if we moved backwards in time (e.g. when playing a bag file) - auto now = std::chrono::system_clock::now(); - if (last_callback_time_ > now) { - // force re-publish of joint ransforms - fprintf(stderr, - "Moved backwards in time, re-publishing joint transforms!\n"); - last_publish_time_.clear(); - } - last_callback_time_ = now; - - // determine least recently published joint - auto last_published = now; - for (unsigned int i = 0; i < state->name.size(); i++) { - auto 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_ || true) { - // get joint positions from state message - std::map joint_positions; - for (unsigned int i = 0; i < state->name.size(); i++) { - joint_positions.insert(std::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(std::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; i < state->name.size(); i++) { - // last_publish_time_[state->name[i]] - // = std::chrono::time_point(msg_nanoseconds); - } - } -} -} // namespace robot_state_publisher - -bool read_urdf_xml(const std::string & filename, std::string & xml_string) -{ - std::fstream xml_file(filename.c_str(), std::fstream::in); - if (xml_file.is_open()) { - while (xml_file.good()) { - std::string line; - std::getline(xml_file, line); - xml_string += (line + "\n"); - } - xml_file.close(); - return true; - } else { - fprintf(stderr, "Could not open file [%s] for parsing.\n", filename.c_str()); - return false; - } -} - -// ---------------------------------- -// ----- MAIN ----------------------- -// ---------------------------------- -int main(int argc, char ** argv) -{ - // Initialize ros - rclcpp::init(argc, argv); - - // gets the location of the robot description on the parameter server - if (argc < 2) { - fprintf(stderr, "Robot State Publisher 2 requires a urdf file name\n"); - return -1; - } - fprintf(stderr, "Initialize urdf model from file: %s\n", argv[1]); - urdf::Model model; - if (!model.initFile(argv[1])) { - fprintf(stderr, "Unable to initialize urdf::model from %s\n", argv[1]); - return -1; - } - - KDL::Tree tree; - if (!kdl_parser::treeFromUrdfModel(model, tree)) { - fprintf(stderr, "Failed to extract kdl tree from xml robot description\n"); - return -1; - } - - std::map mimic; - for (auto i = model.joints_.begin(); i != model.joints_.end(); i++) { - if (i->second->mimic) { - mimic.insert(make_pair(i->first, i->second->mimic)); - } - } - - auto segments_map = tree.getSegments(); - for (auto segment : segments_map) { - fprintf(stderr, "got segment %s\n", segment.first.c_str()); - } - - // Read the URDF XML data (this should always succeed) - std::string urdf_xml; - if (!read_urdf_xml(argv[1], urdf_xml)) { - fprintf(stderr, "failed to read urdf xml '%s'\n", argv[1]); - return -1; - } - - auto node = std::make_shared("robot_state_publisher"); - robot_state_publisher::JointStateListener state_publisher(node, tree, mimic, urdf_xml, model); - - rclcpp::spin(node); - return 0; -} diff --git a/src/robot_state_publisher.cpp b/src/robot_state_publisher.cpp index 940a277..9d27355 100644 --- a/src/robot_state_publisher.cpp +++ b/src/robot_state_publisher.cpp @@ -1,60 +1,64 @@ -/********************************************************************* - * 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. - *********************************************************************/ +// Copyright (c) 2008, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 copyright holders 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.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include #include +#include +#include #include -#include #include - -#include "kdl/frames_io.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" - -#include "robot_state_publisher/robot_state_publisher.h" +#include namespace robot_state_publisher { -inline -KDL::Frame transformToKDL(const geometry_msgs::msg::TransformStamped & t) +namespace { - return KDL::Frame(KDL::Rotation::Quaternion(t.transform.rotation.x, t.transform.rotation.y, - t.transform.rotation.z, t.transform.rotation.w), - KDL::Vector(t.transform.translation.x, t.transform.translation.y, - t.transform.translation.z)); -} inline geometry_msgs::msg::TransformStamped kdlToTransform(const KDL::Frame & k) @@ -68,110 +72,339 @@ geometry_msgs::msg::TransformStamped kdlToTransform(const KDL::Frame & k) return t; } -RobotStatePublisher::RobotStatePublisher( - rclcpp::Node::SharedPtr node_handle, - const KDL::Tree & tree, - const urdf::Model & model, - const std::string & model_xml) -: model_(model), - tf_broadcaster_(node_handle), - static_tf_broadcaster_(node_handle), - clock_(node_handle->get_clock()) +} // namespace + +RobotStatePublisher::RobotStatePublisher(const rclcpp::NodeOptions & options) +: rclcpp::Node("robot_state_publisher", options) { - // walk the tree and add segments to segments_ - addChildren(tree.getRootSegment()); + // get the XML + std::string urdf_xml = this->declare_parameter("robot_description", std::string("")); + if (urdf_xml.empty()) { + // If the robot_description is empty, we fall back to looking at the + // command-line arguments. Since this is deprecated, we print a warning + // but continue on. + try { + if (options.arguments().size() > 1) { + RCLCPP_WARN( + get_logger(), + "No robot_description parameter, but command-line argument available." + " Assuming argument is name of URDF file." + " This backwards compatibility fallback will be removed in the future."); + std::ifstream in(options.arguments()[1], std::ios::in | std::ios::binary); + if (in) { + in.seekg(0, std::ios::end); + urdf_xml.resize(in.tellg()); + in.seekg(0, std::ios::beg); + in.read(&urdf_xml[0], urdf_xml.size()); + in.close(); + } else { + throw std::system_error( + errno, + std::system_category(), + "Failed to open URDF file: " + std::string(options.arguments()[1])); + } + } else { + throw std::runtime_error("robot_description parameter must not be empty"); + } + } catch (const std::runtime_error & err) { + RCLCPP_FATAL(get_logger(), "%s", err.what()); + throw; + } + } + + // set publish frequency + double publish_freq = this->declare_parameter("publish_frequency", 20.0); + if (publish_freq < 0.0 || publish_freq > 1000.0) { + throw std::runtime_error("publish_frequency must be between 0 and 1000"); + } + publish_interval_ms_ = + std::chrono::milliseconds(static_cast(1000.0 / publish_freq)); + + // set whether to use the /tf_static latched static transform broadcaster + use_tf_static_ = this->declare_parameter("use_tf_static", true); + if (!use_tf_static_) { + RCLCPP_WARN(get_logger(), "use_tf_static is deprecated and will be removed in the future"); + } + + // ignore_timestamp_ == true, joint_state messages are accepted, no matter their timestamp + ignore_timestamp_ = this->declare_parameter("ignore_timestamp", false); - model_xml_.data = model_xml; - node_handle->declare_parameter("robot_description", model_xml); - description_pub_ = node_handle->create_publisher( + tf_broadcaster_ = std::make_unique(this); + static_tf_broadcaster_ = std::make_unique(this); + + description_pub_ = this->create_publisher( "robot_description", // Transient local is similar to latching in ROS 1. rclcpp::QoS(1).transient_local()); + setupURDF(urdf_xml); + + // subscribe to joint state + joint_state_sub_ = this->create_subscription( + "joint_states", 10, std::bind( + &RobotStatePublisher::callbackJointState, this, + std::placeholders::_1)); + + // trigger to publish fixed joints + timer_ = + this->create_wall_timer(publish_interval_ms_, + std::bind(&RobotStatePublisher::publishFixedTransforms, this)); + + // Now that we have successfully declared the parameters and done all + // necessary setup, install the callback for updating parameters. + param_cb_ = + add_on_set_parameters_callback(std::bind(&RobotStatePublisher::parameterUpdate, this, + std::placeholders::_1)); +} + +void RobotStatePublisher::setupURDF(const std::string & urdf_xml) +{ + model_ = std::make_unique(); + + // Initialize the model + if (!model_->initString(urdf_xml)) { + throw std::runtime_error("Unable to initialize urdf::model from robot description"); + } + + // Initialize the KDL tree + KDL::Tree tree; + if (!kdl_parser::treeFromUrdfModel(*model_, tree)) { + throw std::runtime_error("Failed to extract kdl tree from robot description"); + } + + // Initialize the mimic map + mimic_.clear(); + for (const std::pair & i : model_->joints_) { + if (i.second->mimic) { + mimic_.insert(std::make_pair(i.first, i.second->mimic)); + } + } + + KDL::SegmentMap segments_map = tree.getSegments(); + for (const std::pair & segment : segments_map) { + RCLCPP_INFO(get_logger(), "got segment %s", segment.first.c_str()); + } + + // walk the tree and add segments to segments_ + segments_.clear(); + segments_fixed_.clear(); + addChildren(tree.getRootSegment()); + + auto msg = std::make_unique(); + msg->data = urdf_xml; + // Publish the robot description - description_pub_->publish(model_xml_); + description_pub_->publish(std::move(msg)); } // add children to correct maps void RobotStatePublisher::addChildren(const KDL::SegmentMap::const_iterator segment) { - auto root = GetTreeElementSegment(segment->second).getName(); + const std::string & root = GetTreeElementSegment(segment->second).getName(); - auto children = GetTreeElementChildren(segment->second); + std::vector children = GetTreeElementChildren(segment->second); for (unsigned int i = 0; i < children.size(); i++) { const KDL::Segment & child = GetTreeElementSegment(children[i]->second); 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) + if (model_->getJoint(child.getJoint().getName()) && + model_->getJoint(child.getJoint().getName())->type == urdf::Joint::FLOATING) { - fprintf(stderr, - "Floating joint. Not adding segment from %s to %s.\n", + RCLCPP_INFO(get_logger(), + "Floating joint. Not adding segment from %s to %s.", root.c_str(), child.getName().c_str()); } else { segments_fixed_.insert(make_pair(child.getJoint().getName(), s)); - fprintf(stderr, "Adding fixed segment from %s to %s\n", root.c_str(), + RCLCPP_DEBUG(get_logger(), "Adding fixed segment from %s to %s", root.c_str(), child.getName().c_str()); } } else { segments_.insert(make_pair(child.getJoint().getName(), s)); - fprintf(stderr, "Adding moving segment from %s to %s\n", root.c_str(), + RCLCPP_DEBUG(get_logger(), "Adding moving segment from %s to %s", root.c_str(), child.getName().c_str()); } addChildren(children[i]); } } - // publish moving transforms void RobotStatePublisher::publishTransforms( const std::map & joint_positions, - const builtin_interfaces::msg::Time & time, - const std::string & tf_prefix) + const builtin_interfaces::msg::Time & time) { - // fprintf(stderr, "Publishing transforms for moving joints\n"); + RCLCPP_DEBUG(get_logger(), "Publishing transforms for moving joints"); std::vector tf_transforms; // loop over all joints - for (auto jnt = joint_positions.begin(); jnt != joint_positions.end(); ++jnt) { - auto seg = segments_.find(jnt->first); + for (const std::pair & jnt : joint_positions) { + std::map::iterator seg = segments_.find(jnt.first); if (seg != segments_.end()) { geometry_msgs::msg::TransformStamped tf_transform = - kdlToTransform(seg->second.segment.pose(jnt->second)); + kdlToTransform(seg->second.segment.pose(jnt.second)); tf_transform.header.stamp = time; - tf_transform.header.frame_id = tf_prefix + "/" + seg->second.root; - tf_transform.child_frame_id = tf_prefix + "/" + seg->second.tip; + tf_transform.header.frame_id = seg->second.root; + tf_transform.child_frame_id = seg->second.tip; tf_transforms.push_back(tf_transform); } } - tf_broadcaster_.sendTransform(tf_transforms); + tf_broadcaster_->sendTransform(tf_transforms); } // publish fixed transforms -void RobotStatePublisher::publishFixedTransforms(const std::string & tf_prefix, bool use_tf_static) +void RobotStatePublisher::publishFixedTransforms() { - // fprintf(stderr, "Publishing transforms for fixed joints\n"); + RCLCPP_DEBUG(get_logger(), "Publishing transforms for fixed joints"); std::vector tf_transforms; - geometry_msgs::msg::TransformStamped tf_transform; // loop over all fixed segments - for (auto seg = segments_fixed_.begin(); seg != segments_fixed_.end(); seg++) { - geometry_msgs::msg::TransformStamped tf_transform = kdlToTransform(seg->second.segment.pose(0)); - rclcpp::Time now = clock_->now(); - if (!use_tf_static) { - now = now + rclcpp::Duration(std::chrono::milliseconds(500)); // 0.5sec in NS + for (const std::pair & seg : segments_fixed_) { + geometry_msgs::msg::TransformStamped tf_transform = kdlToTransform(seg.second.segment.pose(0)); + rclcpp::Time now = this->now(); + if (!use_tf_static_) { + now = now + rclcpp::Duration(std::chrono::milliseconds(500)); } tf_transform.header.stamp = now; - tf_transform.header.frame_id = tf_prefix + "/" + seg->second.root; - tf_transform.child_frame_id = tf_prefix + "/" + seg->second.tip; + tf_transform.header.frame_id = seg.second.root; + tf_transform.child_frame_id = seg.second.tip; tf_transforms.push_back(tf_transform); } - if (use_tf_static) { - static_tf_broadcaster_.sendTransform(tf_transforms); + if (use_tf_static_) { + static_tf_broadcaster_->sendTransform(tf_transforms); } else { - tf_broadcaster_.sendTransform(tf_transforms); + tf_broadcaster_->sendTransform(tf_transforms); + } +} + +void RobotStatePublisher::callbackJointState(const sensor_msgs::msg::JointState::SharedPtr state) +{ + if (state->name.size() != state->position.size()) { + if (state->position.empty()) { + RCLCPP_WARN(get_logger(), "Robot state publisher ignored a JointState message about joint(s) " + "\"%s\"(,...) whose position member was empty.", state->name[0].c_str()); + } else { + RCLCPP_ERROR(get_logger(), "Robot state publisher ignored an invalid JointState message"); + } + return; + } + + // check if we moved backwards in time (e.g. when playing a bag file) + rclcpp::Time now = this->now(); + if (last_callback_time_.nanoseconds() > now.nanoseconds()) { + // force re-publish of joint ransforms + RCLCPP_WARN(get_logger(), + "Moved backwards in time, re-publishing joint transforms!"); + last_publish_time_.clear(); + } + last_callback_time_ = now; + + // determine least recently published joint + rclcpp::Time last_published = now; + for (size_t i = 0; i < state->name.size(); i++) { + rclcpp::Time t(last_publish_time_[state->name[i]]); + last_published = (t.nanoseconds() < last_published.nanoseconds()) ? t : last_published; + } + // note: if a joint was seen for the first time, + // then last_published is zero. + + // check if we need to publish + rclcpp::Time current_time(state->header.stamp); + rclcpp::Time max_publish_time = last_published + rclcpp::Duration(publish_interval_ms_); + if (ignore_timestamp_ || current_time.nanoseconds() >= max_publish_time.nanoseconds()) { + // get joint positions from state message + std::map joint_positions; + for (size_t i = 0; i < state->name.size(); i++) { + joint_positions.insert(std::make_pair(state->name[i], state->position[i])); + } + + for (const std::pair & i : mimic_) { + 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(std::make_pair(i.first, pos)); + } + } + + publishTransforms(joint_positions, state->header.stamp); + + // store publish time in joint map + for (size_t i = 0; i < state->name.size(); i++) { + last_publish_time_[state->name[i]] = state->header.stamp; + } } } +rcl_interfaces::msg::SetParametersResult RobotStatePublisher::parameterUpdate( + const std::vector & parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + for (const rclcpp::Parameter & parameter : parameters) { + if (parameter.get_name() == "robot_description") { + // First make sure that it is still a string + if (parameter.get_type() != rclcpp::ParameterType::PARAMETER_STRING) { + result.successful = false; + result.reason = "URDF must be a string"; + break; + } + + // Now get the parameter + std::string new_urdf = parameter.as_string(); + // And ensure that it isn't empty + if (new_urdf.empty()) { + result.successful = false; + result.reason = "Empty URDF is not allowed"; + break; + } + + setupURDF(new_urdf); + } else if (parameter.get_name() == "use_tf_static") { + if (parameter.get_type() != rclcpp::ParameterType::PARAMETER_BOOL) { + result.successful = false; + result.reason = "use_tf_static must be a boolean"; + break; + } + use_tf_static_ = parameter.as_bool(); + } else if (parameter.get_name() == "ignore_timestamp") { + if (parameter.get_type() != rclcpp::ParameterType::PARAMETER_BOOL) { + result.successful = false; + result.reason = "ignore_timestamp must be a boolean"; + break; + } + ignore_timestamp_ = parameter.as_bool(); + } else if (parameter.get_name() == "publish_frequency") { + if (parameter.get_type() != rclcpp::ParameterType::PARAMETER_DOUBLE) { + result.successful = false; + result.reason = "publish_frequency must be a double"; + break; + } + + double publish_freq = parameter.as_double(); + if (publish_freq < 0.0 || publish_freq > 1000.0) { + result.successful = false; + result.reason = "publish_frequency must be between 0 and 1000"; + break; + } + std::chrono::milliseconds new_publish_interval = + std::chrono::milliseconds(static_cast(1000.0 / publish_freq)); + + if (new_publish_interval != publish_interval_ms_) { + publish_interval_ms_ = new_publish_interval; + timer_->cancel(); + timer_ = + this->create_wall_timer(publish_interval_ms_, + std::bind(&RobotStatePublisher::publishFixedTransforms, this)); + } + } else { + result.successful = false; + result.reason = "Invalid parameter"; + break; + } + } + + return result; +} } // namespace robot_state_publisher + +RCLCPP_COMPONENTS_REGISTER_NODE(robot_state_publisher::RobotStatePublisher) diff --git a/test/test_two_links_fixed_joint.cpp b/test/test_two_links_fixed_joint.cpp new file mode 100644 index 0000000..dc1f421 --- /dev/null +++ b/test/test_two_links_fixed_joint.cpp @@ -0,0 +1,78 @@ +// Copyright (c) 2008, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 copyright holders 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 +#include + +#include +#include +#include +#include + +#define EPS 0.01 + +TEST(test_publisher, test_two_joints) +{ + auto node = rclcpp::Node::make_shared("rsp_test_two_links_fixed_joint"); + + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf2_ros::Buffer buffer(clock); + tf2_ros::TransformListener tfl(buffer, node, true); + + for (unsigned int i = 0; i < 100 && !buffer.canTransform("link1", "link2", rclcpp::Time()); i++) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + ASSERT_TRUE(buffer.canTransform("link1", "link2", rclcpp::Time())); + ASSERT_FALSE(buffer.canTransform("base_link", "wim_link", rclcpp::Time())); + + geometry_msgs::msg::TransformStamped t = buffer.lookupTransform("link1", "link2", rclcpp::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); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + + int res = RUN_ALL_TESTS(); + + rclcpp::shutdown(); + + return res; +} diff --git a/test/test_two_links_moving_joint.cpp b/test/test_two_links_moving_joint.cpp new file mode 100644 index 0000000..6b11e36 --- /dev/null +++ b/test/test_two_links_moving_joint.cpp @@ -0,0 +1,87 @@ +// Copyright (c) 2008, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 copyright holders 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 +#include + +#include +#include +#include +#include + +#define EPS 0.01 + +TEST(test_publisher, test_two_joints) +{ + const double pi = 3.14159265358979323846; + auto node = rclcpp::Node::make_shared("rsp_test_two_links_moving_joint"); + + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf2_ros::Buffer buffer(clock); + tf2_ros::TransformListener tfl(buffer, node, true); + + rclcpp::Publisher::SharedPtr pub = + node->create_publisher("joint_states", 10); + sensor_msgs::msg::JointState js_msg; + js_msg.name.push_back("joint1"); + js_msg.position.push_back(pi); + js_msg.header.stamp = node->now(); + + for (unsigned int i = 0; i < 100 && !buffer.canTransform("link1", "link2", rclcpp::Time()); i++) { + pub->publish(js_msg); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + ASSERT_TRUE(buffer.canTransform("link1", "link2", rclcpp::Time())); + ASSERT_FALSE(buffer.canTransform("base_link", "wim_link", rclcpp::Time())); + + geometry_msgs::msg::TransformStamped t = buffer.lookupTransform("link1", "link2", rclcpp::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); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + + int res = RUN_ALL_TESTS(); + + rclcpp::shutdown(); + + return res; +} diff --git a/test/two_links_fixed_joint-launch.py b/test/two_links_fixed_joint-launch.py new file mode 100644 index 0000000..8155576 --- /dev/null +++ b/test/two_links_fixed_joint-launch.py @@ -0,0 +1,92 @@ +# Copyright (c) 2020 Open Source Robotics Foundation, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# 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 copyright holder 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. + +import os +import unittest + +import launch +from launch import LaunchDescription +from launch_ros.actions import Node +import launch_testing +import launch_testing.actions +import launch_testing.asserts + + +def generate_test_description(): + test_exe_arg = launch.actions.DeclareLaunchArgument( + 'test_exe', + description='Path to executable test', + ) + + process_under_test = launch.actions.ExecuteProcess( + cmd=[launch.substitutions.LaunchConfiguration('test_exe')], + output='screen', + ) + + urdf_file = os.path.join(os.path.dirname(__file__), 'two_links_fixed_joint.urdf') + with open(urdf_file, 'r') as infp: + robot_desc = infp.read() + + params = {'robot_description': robot_desc} + node_robot_state_publisher = Node( + package='robot_state_publisher', + node_executable='robot_state_publisher', + output='screen', + parameters=[params] + ) + + return LaunchDescription([ + node_robot_state_publisher, + test_exe_arg, + process_under_test, + launch_testing.util.KeepAliveProc(), + launch_testing.actions.ReadyToTest(), + ]), locals() + + +class TestFixedJoint(unittest.TestCase): + + def test_termination(self, process_under_test, proc_info): + proc_info.assertWaitForShutdown(process=process_under_test, timeout=(10)) + + +@launch_testing.post_shutdown_test() +class FixedJointTestAfterShutdown(unittest.TestCase): + + def test_exit_code(self, process_under_test, proc_info): + # Check that all processes in the launch (in this case, there's just one) exit + # with code 0 + launch_testing.asserts.assertExitCodes( + proc_info, + [launch_testing.asserts.EXIT_OK], + process_under_test + ) diff --git a/test/two_links_fixed_joint.urdf b/test/two_links_fixed_joint.urdf new file mode 100644 index 0000000..a59ae88 --- /dev/null +++ b/test/two_links_fixed_joint.urdf @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/test/two_links_moving_joint-launch.py b/test/two_links_moving_joint-launch.py new file mode 100644 index 0000000..09c673b --- /dev/null +++ b/test/two_links_moving_joint-launch.py @@ -0,0 +1,92 @@ +# Copyright (c) 2020 Open Source Robotics Foundation, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# 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 copyright holder 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. + +import os +import unittest + +import launch +from launch import LaunchDescription +from launch_ros.actions import Node +import launch_testing +import launch_testing.actions +import launch_testing.asserts + + +def generate_test_description(): + test_exe_arg = launch.actions.DeclareLaunchArgument( + 'test_exe', + description='Path to executable test', + ) + + process_under_test = launch.actions.ExecuteProcess( + cmd=[launch.substitutions.LaunchConfiguration('test_exe')], + output='screen', + ) + + urdf_file = os.path.join(os.path.dirname(__file__), 'two_links_moving_joint.urdf') + with open(urdf_file, 'r') as infp: + robot_desc = infp.read() + + params = {'robot_description': robot_desc} + node_robot_state_publisher = Node( + package='robot_state_publisher', + node_executable='robot_state_publisher', + output='screen', + parameters=[params] + ) + + return LaunchDescription([ + node_robot_state_publisher, + test_exe_arg, + process_under_test, + launch_testing.util.KeepAliveProc(), + launch_testing.actions.ReadyToTest(), + ]), locals() + + +class TestMovingJoint(unittest.TestCase): + + def test_termination(self, process_under_test, proc_info): + proc_info.assertWaitForShutdown(process=process_under_test, timeout=10) + + +@launch_testing.post_shutdown_test() +class MovingJointTestAfterShutdown(unittest.TestCase): + + def test_exit_code(self, process_under_test, proc_info): + # Check that all processes in the launch (in this case, there's just one) exit + # with code 0 + launch_testing.asserts.assertExitCodes( + proc_info, + [launch_testing.asserts.EXIT_OK], + process_under_test + ) diff --git a/test/two_links_moving_joint.urdf b/test/two_links_moving_joint.urdf new file mode 100644 index 0000000..20e2460 --- /dev/null +++ b/test/two_links_moving_joint.urdf @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/urdf/test-desc.urdf b/urdf/test-desc.urdf new file mode 100644 index 0000000..a90ffeb --- /dev/null +++ b/urdf/test-desc.urdf @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/urdf/test-desc.urdf.xacro b/urdf/test-desc.urdf.xacro new file mode 100644 index 0000000..b5d924b --- /dev/null +++ b/urdf/test-desc.urdf.xacro @@ -0,0 +1,13 @@ + + + + + + + + + + + + +