Refactor the ROS 2 code to be more modern

CMakeLists.txt

@@ -11,66 +11,52 @@ 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} 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})
+ament_target_dependencies(${PROJECT_NAME}
+  builtin_interfaces
+  geometry_msgs
+  kdl_parser
+  orocos_kdl
+  rclcpp
+  rclcpp_components
+  sensor_msgs
+  std_msgs
+  tf2_ros
+  urdf
+)
 
-add_library(
-  joint_state_listener
-  src/joint_state_listener.cpp)
-target_link_libraries(
-  joint_state_listener
-  ${PROJECT_NAME}_solver
-  ${orocos_kdl_LIBRARIES})
+rclcpp_components_register_nodes(${PROJECT_NAME}
+  "robot_state_publisher::RobotStatePublisher")
 
 add_executable(
+  ${PROJECT_NAME}_node
+  src/robot_state_publisher_node.cpp)
+ament_target_dependencies(${PROJECT_NAME}_node
+  rclcpp
+)
+target_link_libraries(${PROJECT_NAME}_node
   ${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})
+)
 
 install(
   TARGETS
-  ${PROJECT_NAME}_solver
-  joint_state_listener
+  ${PROJECT_NAME}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
@@ -80,25 +66,25 @@ install(DIRECTORY include/
   DESTINATION include)
 
 install(
-  TARGETS ${PROJECT_NAME}
+  TARGETS ${PROJECT_NAME}_node
   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()
 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})
 ament_package()

README.md

@@ -1,3 +1,21 @@
-# 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".
+
+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.

include/robot_state_publisher/robot_state_publisher.h → include/robot_state_publisher/robot_state_publisher.hpp

@@ -34,72 +34,85 @@
 
 /* Author: Wim Meeussen */
 
-#ifndef ROBOT_STATE_PUBLISHER__ROBOT_STATE_PUBLISHER_H_
-#define ROBOT_STATE_PUBLISHER__ROBOT_STATE_PUBLISHER_H_
-
+#ifndef ROBOT_STATE_PUBLISHER__ROBOT_STATE_PUBLISHER_HPP_
+#define ROBOT_STATE_PUBLISHER__ROBOT_STATE_PUBLISHER_HPP_
+
+#include <builtin_interfaces/msg/time.hpp>
+#include <kdl/tree.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/joint_state.hpp>
+#include <std_msgs/msg/string.hpp>
+#include <tf2_ros/static_transform_broadcaster.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <urdf/model.h>
+
+#include <chrono>
 #include <map>
+#include <memory>
 #include <string>
+#include <vector>
 
-#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"
+typedef std::map<std::string, urdf::JointMimicSharedPtr> MimicMap;
 
 namespace robot_state_publisher
 {
 
-class SegmentPair
+class SegmentPair final
 {
 public:
-  SegmentPair(const KDL::Segment & p_segment, const std::string & p_root, const std::string & p_tip)
+  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, tip;
+  std::string root;
+  std::string tip;
 };
 
-class RobotStatePublisher
+class RobotStatePublisher final : public rclcpp::Node
 {
 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);
+  /// Constructor
+  explicit RobotStatePublisher(const rclcpp::NodeOptions & options);
 
   /// Destructor
-  virtual ~RobotStatePublisher() {}
+  ~RobotStatePublisher();
 
+private:
   /** 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(
+  void publishTransforms(
     const std::map<std::string, double> & 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<std::string, SegmentPair> segments_, segments_fixed_;
-  const urdf::Model & model_;
-  tf2_ros::TransformBroadcaster tf_broadcaster_;
-  tf2_ros::StaticTransformBroadcaster static_tf_broadcaster_;
-  std_msgs::msg::String model_xml_;
+    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<rclcpp::Parameter> & parameters);
+
+  std::map<std::string, SegmentPair> segments_;
+  std::map<std::string, SegmentPair> segments_fixed_;
+  std::unique_ptr<urdf::Model> model_;
+  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+  std::unique_ptr<tf2_ros::StaticTransformBroadcaster> static_tf_broadcaster_;
   rclcpp::Publisher<std_msgs::msg::String>::SharedPtr description_pub_;
-  rclcpp::Clock::SharedPtr clock_;
+  std::chrono::milliseconds publish_interval_ms_;
+  rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_sub_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::Time last_callback_time_;
+  std::map<std::string, builtin_interfaces::msg::Time> 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_H_
+
+#endif  // ROBOT_STATE_PUBLISHER__ROBOT_STATE_PUBLISHER_HPP_

launch/rsp-launch-urdf-file.xml

@@ -0,0 +1,43 @@
+<!--
+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 the find-pkg-share substitution).
+-->
+<launch>
+  <node pkg="robot_state_publisher" exec="robot_state_publisher_node">
+    <param name='robot_description' value="$(command 'cat $(find-pkg-share robot_state_publisher)/urdf/test-desc.urdf')"/>
+  </node>
+</launch>

launch/rsp-launch-urdf-file1.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 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():
+    urdf_dir = os.path.join(FindPackageShare('robot_state_publisher').find('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_node',
+                                  output='both',
+                                  parameters=[params])
+
+    return launch.LaunchDescription([rsp])