Refactor the ROS 2 code to be more modern (#126)

* Major refactor of robot_state_publisher.

1.  Simplify the code (collapse it down to one class)
2.  Make it composable
3.  Update the documentation.
4.  Add the ability to change parameters on the fly
5.  Port the rest of the missing features from ROS 1
6.  Add tests
7.  Make the robot_description a parameter instead of a command-line argument
8.  Add example launch files

Signed-off-by: Chris Lalancette <[email protected]>

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:=$<TARGET_FILE:test_two_links_fixed_joint>")
+
+  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:=$<TARGET_FILE:test_two_links_moving_joint>")
 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()

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).

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.

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.