Port to ROS2

.github/workflows/ubuntu.yml

@@ -19,7 +19,7 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        distro: [noetic]
+        distro: [foxy, humble, rolling]
     container:
       image: ros:${{ matrix.distro }}
       env:

CMakeLists.txt

@@ -1,57 +1,58 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5.0)
 project(trajectory_preview)
 
-add_compile_options(-std=c++11)
-
-find_package(catkin REQUIRED COMPONENTS rviz sensor_msgs trajectory_msgs)
+add_compile_options(-std=c++17)
 
 find_package(Qt5 REQUIRED COMPONENTS Widgets)
 
-catkin_package(
-  INCLUDE_DIRS
-  include
-  LIBRARIES
-  ${PROJECT_NAME}_widget
-  ${PROJECT_NAME}_panel
-  CATKIN_DEPENDS
-  rviz
-  sensor_msgs
-  trajectory_msgs)
+set(ROS2_DEPS
+    rviz_common
+    sensor_msgs
+    trajectory_msgs
+    pluginlib)
+
+find_package(ament_cmake REQUIRED)
+foreach(dep ${ROS2_DEPS})
+  find_package(${dep} REQUIRED)
+endforeach()
 
 # ######################################################################################################################
 # Build ##
 # ######################################################################################################################
-
-include_directories(include ${catkin_INCLUDE_DIRS} ${CMAKE_CURRENT_BINARY_DIR})
-
 qt5_wrap_ui(UI_MOC ui/trajectory_preview_widget.ui)
 
 qt5_wrap_cpp(WIDGET_MOC include/${PROJECT_NAME}/trajectory_preview_widget.h src/trajectory_preview_impl.h)
 
 qt5_wrap_cpp(PANEL_MOC include/${PROJECT_NAME}/trajectory_preview_panel.h)
 
 # Widget Library
-add_library(${PROJECT_NAME}_widget src/trajectory_preview_widget.cpp ${UI_MOC} ${WIDGET_MOC})
-target_link_libraries(${PROJECT_NAME}_widget ${catkin_LIBRARIES} Qt5::Widgets)
+add_library(${PROJECT_NAME}_widget SHARED src/trajectory_preview_widget.cpp ${UI_MOC} ${WIDGET_MOC})
+target_include_directories(${PROJECT_NAME}_widget PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+                                                         "$<INSTALL_INTERFACE:include>")
+target_include_directories(${PROJECT_NAME}_widget PRIVATE "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>")
+target_link_libraries(${PROJECT_NAME}_widget Qt5::Widgets)
+ament_target_dependencies(${PROJECT_NAME}_widget ${ROS2_DEPS})
 
 # Panel Library
-add_library(${PROJECT_NAME}_panel src/trajectory_preview_panel.cpp ${PANEL_MOC})
-target_link_libraries(
-  ${PROJECT_NAME}_panel
-  ${catkin_LIBRARIES}
-  Qt5::Widgets
-  ${PROJECT_NAME}_widget)
+add_library(${PROJECT_NAME}_panel SHARED src/trajectory_preview_panel.cpp ${PANEL_MOC})
+target_link_libraries(${PROJECT_NAME}_panel Qt5::Widgets ${PROJECT_NAME}_widget)
 
 # ######################################################################################################################
 # Install ##
 # ######################################################################################################################
 
-install(
-  TARGETS ${PROJECT_NAME}_widget ${PROJECT_NAME}_panel
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
+# Install the headers
+install(DIRECTORY include/${PROJECT_NAME} DESTINATION include)
+
+# Install the library(ies)
+install(TARGETS ${PROJECT_NAME}_widget ${PROJECT_NAME}_panel EXPORT ${PROJECT_NAME}-targets DESTINATION lib)
+
+install(FILES plugin_description.xml DESTINATION share/${PROJECT_NAME})
+
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
+
+pluginlib_export_plugin_description_file(rviz_common plugin_description.xml)
 
-install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
-install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
-install(FILES plugin_description.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+ament_export_targets(${PROJECT_NAME}-targets HAS_LIBRARY_TARGET)
+ament_export_dependencies(${ROS2_DEPS})
+ament_package()

include/trajectory_preview/trajectory_preview_panel.h

@@ -18,17 +18,17 @@
 #ifndef TRAJECTORY_PREVIEW_PANEL_H
 #define TRAJECTORY_PREVIEW_PANEL_H
 
-#include <rviz/panel.h>
+#include <rviz_common/panel.hpp>
 
 namespace trajectory_preview
 {
 class TrajectoryPreviewWidget;
 
-class TrajectoryPreviewPanel : public rviz::Panel
+class TrajectoryPreviewPanel : public rviz_common::Panel
 {
   Q_OBJECT
 public:
-  using rviz::Panel::Panel;
+  using rviz_common::Panel::Panel;
 
   void onInitialize() override;
 

include/trajectory_preview/trajectory_preview_widget.h

@@ -19,7 +19,8 @@
 #define TRAJECTORY_PREVIEW_TRAJECTORY_PREVIEW_WIDGET_H
 
 #include <QWidget>
-#include <trajectory_msgs/JointTrajectory.h>
+#include <trajectory_msgs/msg/joint_trajectory.hpp>
+#include <rclcpp/node.hpp>
 
 namespace Ui
 {
@@ -43,15 +44,8 @@ class TrajectoryPreviewWidget : public QWidget
    * @param input_traj_topic
    * @param output_state_topic
    */
-  void initializeROS(const std::string& input_traj_topic, const std::string& output_state_topic);
-
-  /**
-   * @brief Programmatically sets the displayed trajectory (rather than waiting
-   * for one to
-   * be published)
-   * @param trajectory
-   */
-  void setTrajectory(const trajectory_msgs::JointTrajectory& trajectory);
+  void initializeROS(rclcpp::Node::SharedPtr node, const std::string& input_traj_topic,
+                     const std::string& output_state_topic);
 
 protected Q_SLOTS:
   // User Interactions

launch/robot_model_preview_pipeline.launch.xml

@@ -1,18 +1,18 @@
 <?xml version="1.0."?>
 <launch>
-  <arg name="root_frame" doc="Root frame of the nominal TF tree, at which to connect the preview TF tree" />
-  <arg name="prefix" default="preview"/>
+  <arg name="robot_description" description="Robot description string" />
+  <arg name="root_frame" description="Root frame of the nominal TF tree, at which to connect the preview TF tree" />
+  <arg name="preview_joint_states_topic" default="/preview"/>
 
-  <node pkg="joint_state_publisher" type="joint_state_publisher" name="$(arg prefix)_joint_state_publisher">
-    <remap from="joint_states" to="$(arg prefix)/joint_states"/>
-    <rosparam param="source_list">[trajectory/joint_states]</rosparam>
+  <node pkg="joint_state_publisher" exec="joint_state_publisher" namespace="preview">
+    <param name="source_list" value="[$(var preview_joint_states_topic)]"/>
   </node>
 
-  <node pkg="robot_state_publisher" type="robot_state_publisher" name="$(arg prefix)_robot_state_publisher">
-    <remap from="joint_states" to="$(arg prefix)/joint_states"/>
-    <param name="tf_prefix" value="$(arg prefix)"/>
+  <node pkg="robot_state_publisher" exec="robot_state_publisher" namespace="preview">
+    <param name="robot_description" value="$(var robot_description)"/>
+    <param name="frame_prefix" value="preview/"/>
   </node>
 
-  <node pkg="tf2_ros" type="static_transform_publisher" args="0 0 0 0 0 0 $(arg root_frame) preview/$(arg root_frame)" name="$(arg prefix)_static_tf2_pub"/>
+  <node pkg="tf2_ros" exec="static_transform_publisher" args="0 0 0 0 0 0 $(var root_frame) preview/$(var root_frame)" namespace="preview"/>
 
 </launch>

package.xml

@@ -1,5 +1,6 @@
 <?xml version="1.0"?>
-<package format="2">
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
   <name>trajectory_preview</name>
   <version>0.0.0</version>
   <description>The trajectory_preview package</description>
@@ -9,17 +10,18 @@
 
   <license>Apache 2.0</license>
 
-  <buildtool_depend>catkin</buildtool_depend>
-  <depend>rviz</depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>rviz_common</depend>
   <depend>sensor_msgs</depend>
   <depend>trajectory_msgs</depend>
   <depend>libqt5-widgets</depend>
   <depend>qtbase5-dev</depend>
+  <depend>pluginlib</depend>
   <exec_depend>joint_state_publisher</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
   <exec_depend>tf2_ros</exec_depend>
 
   <export>
-    <rviz plugin="${prefix}/plugin_description.xml"/>
+    <build_type>ament_cmake</build_type>
   </export>
 </package>

plugin_description.xml

@@ -1,7 +1,7 @@
-<library path="lib/libtrajectory_preview_panel">
+<library path="trajectory_preview_panel">
   <class name="trajectory_preview/TrajectoryPreviewPanel"
          type="trajectory_preview::TrajectoryPreviewPanel"
-         base_class_type="rviz::Panel">
+         base_class_type="rviz_common::Panel">
     <description>
       A panel previewing robot trajectories
     </description>

src/robot_trajectory.h

@@ -38,26 +38,27 @@
 #define TRAJECTORY_PREVIEW_ROBOT_TRAJECTORY_H
 
 #include <deque>
-#include <ros/console.h>
-#include <sensor_msgs/JointState.h>
-#include <trajectory_msgs/JointTrajectory.h>
+#include <rclcpp/time.hpp>
+#include <sensor_msgs/msg/joint_state.hpp>
+#include <trajectory_msgs/msg/joint_trajectory.hpp>
 
 namespace trajectory_preview
 {
 class RobotTrajectory
 {
 public:
-  RobotTrajectory(const trajectory_msgs::JointTrajectory& trajectory) : trajectory_(trajectory)
+  RobotTrajectory(const trajectory_msgs::msg::JointTrajectory& trajectory) : trajectory_(trajectory)
   {
-    ros::Time last_time_stamp = trajectory_.header.stamp;
-    ros::Time this_time_stamp = last_time_stamp;
+    rclcpp::Time last_time_stamp = trajectory_.header.stamp;
+    rclcpp::Time this_time_stamp = last_time_stamp;
 
     for (std::size_t i = 0; i < trajectory_.points.size(); ++i)
     {
-      this_time_stamp = trajectory_.header.stamp + trajectory_.points[i].time_from_start;
+      this_time_stamp =
+          rclcpp::Time(trajectory_.header.stamp) + rclcpp::Duration(trajectory_.points[i].time_from_start);
 
       waypoints_.push_back(trajectory_.points[i]);
-      duration_from_previous_.push_back((this_time_stamp - last_time_stamp).toSec());
+      duration_from_previous_.push_back((this_time_stamp - last_time_stamp).seconds());
 
       last_time_stamp = this_time_stamp;
     }
@@ -97,7 +98,7 @@ class RobotTrajectory
     }
   }
 
-  bool getStateAtDurationFromStart(const double request_duration, sensor_msgs::JointState& output_state) const
+  bool getStateAtDurationFromStart(const double request_duration, sensor_msgs::msg::JointState& output_state) const
   {
     // If there are no waypoints we can't do anything
     if (waypoints_.empty())
@@ -108,9 +109,6 @@ class RobotTrajectory
     double blend = 1.0;
     findWayPointIndicesForDurationAfterStart(request_duration, before, after, blend);
 
-    //    ROS_INFO("Interpolating %.3f of the way between index %d and %d.",
-    //    blend, before, after);
-
     output_state = interpolate(waypoints_[before], waypoints_[after], blend);
 
     return true;
@@ -135,10 +133,10 @@ class RobotTrajectory
   }
 
 private:
-  sensor_msgs::JointState interpolate(const trajectory_msgs::JointTrajectoryPoint& start,
-                                      const trajectory_msgs::JointTrajectoryPoint& end, const double t) const
+  sensor_msgs::msg::JointState interpolate(const trajectory_msgs::msg::JointTrajectoryPoint& start,
+                                           const trajectory_msgs::msg::JointTrajectoryPoint& end, const double t) const
   {
-    sensor_msgs::JointState out;
+    sensor_msgs::msg::JointState out;
     out.name = trajectory_.joint_names;
     out.position.resize(out.name.size());
 
@@ -150,9 +148,9 @@ class RobotTrajectory
     return out;
   }
 
-  trajectory_msgs::JointTrajectory trajectory_;
+  trajectory_msgs::msg::JointTrajectory trajectory_;
 
-  std::deque<trajectory_msgs::JointTrajectoryPoint> waypoints_;
+  std::deque<trajectory_msgs::msg::JointTrajectoryPoint> waypoints_;
 
   std::deque<double> duration_from_previous_;
 };