moving to ROS2

CMakeLists.txt

@@ -1,66 +1,77 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5)
 project(vrpn_client_ros)
 
-find_package(catkin REQUIRED COMPONENTS geometry_msgs roscpp tf2_ros)
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+# find_package(catkin REQUIRED COMPONENTS geometry_msgs tf2_ros)
 find_package(VRPN REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
 
-catkin_package(
-  INCLUDE_DIRS include
-  LIBRARIES vrpn_client_ros
-  CATKIN_DEPENDS geometry_msgs tf2_ros
-  DEPENDS VRPN
-)
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
 
-set_directory_properties(PROPERTIES COMPILE_OPTIONS "-std=c++11;-Wall")
 
-include_directories(
-  include
-  ${catkin_INCLUDE_DIRS}
-  SYSTEM ${VRPN_INCLUDE_DIR}
-)
+include_directories(include)
 
 add_library(vrpn_client_ros
-  src/vrpn_client_ros.cpp
-)
-target_link_libraries(vrpn_client_ros
-  ${catkin_LIBRARIES}
-  ${VRPN_LIBRARIES}
-)
+            src/vrpn_client_ros.cpp)
+ament_target_dependencies(vrpn_client_ros rclcpp std_msgs geometry_msgs VRPN)
 
-add_executable(vrpn_tracker_node
-  src/vrpn_tracker_node.cpp)
-add_dependencies(vrpn_tracker_node vrpn_client_ros)
-target_link_libraries(vrpn_tracker_node
-  vrpn_client_ros
-  ${catkin_LIBRARIES}
-)
+add_executable(vrpn_tracker_node src/vrpn_tracker_node.cpp)
+target_link_libraries(vrpn_tracker_node vrpn_client_ros)
+ament_target_dependencies(vrpn_tracker_node rclcpp std_msgs geometry_msgs VRPN)
 
-add_executable(vrpn_client_node
-  src/vrpn_client_node.cpp)
-add_dependencies(vrpn_client_node vrpn_client_ros)
-target_link_libraries(vrpn_client_node
-  vrpn_client_ros
-  ${catkin_LIBRARIES}
-)
+add_executable(vrpn_client_node src/vrpn_client_node.cpp)
+target_link_libraries(vrpn_client_node vrpn_client_ros)
+ament_target_dependencies(vrpn_client_node rclcpp std_msgs geometry_msgs VRPN)
 
-install(TARGETS vrpn_client_ros vrpn_tracker_node vrpn_client_node
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+install(DIRECTORY
+  launch
+  DESTINATION share/${PROJECT_NAME}/
 )
 
-install(DIRECTORY include/${PROJECT_NAME}/
-  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+install(DIRECTORY
+  config
+  DESTINATION share/${PROJECT_NAME}/
 )
 
-install(DIRECTORY launch
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+install(TARGETS
+  vrpn_client_ros
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin)
+
+install(TARGETS
+  vrpn_tracker_node
+  vrpn_client_node
+  DESTINATION lib/${PROJECT_NAME}
 )
 
-if(CATKIN_ENABLE_TESTING)
-  find_package(roslaunch REQUIRED)
-  find_package(roslint REQUIRED)
-  roslint_cpp()
-  roslint_add_test()
-  roslaunch_add_file_check(launch)
-endif()
+ament_package()

README.md

@@ -0,0 +1,21 @@
+# vrpn_client_ros
+This fork aims at porting the original code from the kinetic-devel branch to ROS2.
+
+## Requirements
+
+The code requires VRPN to work. Unfortunately, currently the package is only
+available in melodic and older ROS releases and can be installed with:
+```
+    apt-get install ros-melodic-vrpn
+```
+
+If you want to stick to ROS2 or do not have access to melodic packages you can
+clone the following repository to the src folder of your ros_overlay_ws:
+```
+git clone --single-branch --branch debian/melodic/vrpn [email protected]:ros-drivers-gbp/vrpn-release.git
+```
+
+## What works?
+
+I only use pose in my project, so I did not port anything else (TF, twist, accel). Also multiple sensors per tracker are not ported.
+If there is anyone who would like to use the other features and is willing to test them, I'd be happy to help.

config/sample.params.yaml

@@ -0,0 +1,8 @@
+vrpn_client_node:
+  ros__parameters:
+    server: 192.168.0.107
+    port: 3883
+    frame_id: "world"
+    use_server_time: false
+    refresh_tracker_frequency: 0.2
+    update_frequency: 30.0

include/vrpn_client_ros/vrpn_client_ros.h

@@ -34,11 +34,12 @@
 
 #include "vrpn_client_ros/vrpn_client_ros.h"
 
-#include "ros/ros.h"
-#include "geometry_msgs/PoseStamped.h"
-#include "geometry_msgs/TwistStamped.h"
-#include "geometry_msgs/AccelStamped.h"
-#include "geometry_msgs/TransformStamped.h"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/logging.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/twist_stamped.h"
+#include "geometry_msgs/msg/accel_stamped.h"
+#include "geometry_msgs/msg/transform_stamped.h"
 
 #include <vrpn_Tracker.h>
 #include <vrpn_Connection.h>
@@ -61,13 +62,13 @@ namespace vrpn_client_ros
      * Create and initialize VrpnTrackerRos using an existing underlying VRPN connection object. The underlying
      * connection object is responsible for calling the tracker's mainloop.
      */
-    VrpnTrackerRos(std::string tracker_name, ConnectionPtr connection, ros::NodeHandle nh);
+    VrpnTrackerRos(std::string tracker_name, ConnectionPtr connection, rclcpp::Node::SharedPtr nh);
 
     /**
      * Create and initialize VrpnTrackerRos, creating a new connection to tracker_name@host. This constructor will
      * register timer callbacks on nh to call mainloop.
      */
-    VrpnTrackerRos(std::string tracker_name, std::string host, ros::NodeHandle nh);
+    VrpnTrackerRos(std::string tracker_name, std::string host, rclcpp::Node::SharedPtr nh);
 
     ~VrpnTrackerRos();
 
@@ -78,19 +79,20 @@ namespace vrpn_client_ros
 
   private:
     TrackerRemotePtr tracker_remote_;
-    std::vector<ros::Publisher> pose_pubs_, twist_pubs_, accel_pubs_;
-    ros::NodeHandle output_nh_;
-    bool use_server_time_, broadcast_tf_, process_sensor_id_;
+    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_pub_;
+    //std::vector<ros::Publisher> pose_pubs_, twist_pubs_, accel_pubs_;
+    rclcpp::Node::SharedPtr output_nh_;
+    bool use_server_time_, broadcast_tf_, mainloop_executed_;
     std::string tracker_name;
 
-    ros::Timer mainloop_timer;
+    rclcpp::TimerBase::SharedPtr mainloop_timer;
 
-    geometry_msgs::PoseStamped pose_msg_;
-    geometry_msgs::TwistStamped twist_msg_;
-    geometry_msgs::AccelStamped accel_msg_;
-    geometry_msgs::TransformStamped transform_stamped_;
+    geometry_msgs::msg::PoseStamped pose_msg_;
+    // geometry_msgs::TwistStamped twist_msg_;
+    // geometry_msgs::AccelStamped accel_msg_;
+    // geometry_msgs::TransformStamped transform_stamped_;
 
-    void init(std::string tracker_name, ros::NodeHandle nh, bool create_mainloop_timer);
+    void init(std::string tracker_name, rclcpp::Node::SharedPtr nh, bool create_mainloop_timer);
 
     static void VRPN_CALLBACK handle_pose(void *userData, const vrpn_TRACKERCB tracker_pose);
 
@@ -102,16 +104,15 @@ namespace vrpn_client_ros
   class VrpnClientRos
   {
   public:
-
     typedef std::shared_ptr<VrpnClientRos> Ptr;
     typedef std::unordered_map<std::string, VrpnTrackerRos::Ptr> TrackerMap;
 
     /**
      * Create and initialize VrpnClientRos object in the private_nh namespace.
      */
-    VrpnClientRos(ros::NodeHandle nh, ros::NodeHandle private_nh);
+    VrpnClientRos(rclcpp::Node::SharedPtr nh, rclcpp::Node::SharedPtr private_nh);
 
-    static std::string getHostStringFromParams(ros::NodeHandle host_nh);
+    static std::string getHostStringFromParams(rclcpp::Node::SharedPtr host_nh);
 
     /**
      * Call mainloop of underlying VRPN connection and all registered VrpnTrackerRemote objects.
@@ -125,7 +126,7 @@ namespace vrpn_client_ros
 
   private:
     std::string host_;
-    ros::NodeHandle output_nh_;
+    rclcpp::Node::SharedPtr output_nh_;
 
     /**
      * Underlying VRPN connection object
@@ -137,7 +138,8 @@ namespace vrpn_client_ros
      */
     TrackerMap trackers_;
 
-    ros::Timer refresh_tracker_timer_, mainloop_timer;
+
+    rclcpp::TimerBase::SharedPtr refresh_tracker_timer, mainloop_timer;
   };
 }  // namespace vrpn_client_ros
 

launch/sample.launch.py

@@ -0,0 +1,26 @@
+from pathlib import Path
+
+from ament_index_python.packages import get_package_share_directory
+import launch
+import launch.actions
+import launch.substitutions
+import launch_ros.actions
+
+
+parameters_file_path = Path(
+    get_package_share_directory('vrpn_client_ros'),
+    'config',
+    'sample.params.yaml'
+)
+
+
+def generate_launch_description():
+    return launch.LaunchDescription([
+        launch_ros.actions.Node(
+            package='vrpn_client_ros',
+            node_executable='vrpn_client_node',
+            output='screen',
+            emulate_tty=True,
+            parameters=[parameters_file_path],
+        ),
+    ])

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>vrpn_client_ros</name>
   <version>0.2.2</version>
   <description>ROS client nodes for the <a href="https://github.com/vrpn/vrpn/wiki">VRPN</a> library, compatible with VICON, OptiTrack, and other <a href="https://github.com/vrpn/vrpn/wiki/Supported-hardware-devices">hardware interfaces</a>.</description>
@@ -8,16 +9,16 @@
   <maintainer email="[email protected]">Paul Bovbel</maintainer>
 
   <license>BSD</license>
-  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>geometry_msgs</depend>
-  <depend>roscpp</depend>
   <depend>tf2_ros</depend>
-  <depend>vrpn</depend>
+  <depend>VRPN</depend>
 
-  <test_depend>roslaunch</test_depend>
-  <test_depend>roslint</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
 
   <export>
+    <build_type>ament_cmake</build_type>
   </export>
 </package>

src/vrpn_client_node.cpp

@@ -33,10 +33,10 @@
 
 int main(int argc, char **argv)
 {
-  ros::init(argc, argv, "vrpn_client_node");
-  ros::NodeHandle nh, private_nh("~");
-  vrpn_client_ros::VrpnClientRos client(nh, private_nh);
-  ros::spin();
+  rclcpp::init(argc, argv);
+  auto nh = rclcpp::Node::make_shared("vrpn_client_node");
+  vrpn_client_ros::VrpnClientRos client(nh, nh);
+  rclcpp::spin(nh);
   return 0;
 }