Ros2 port (#1)

ROS 2 port * Port CMakeLists.txt * Port package.xml * Port client.cpp * Port node.cpp Fix compilation Signed-off-by: Hunter L. Allen <[email protected]>
RoverRobotics-forks · Apr 29, 2019 · d58e162 · d58e162
1 parent 95d935f
commit d58e162
Show file tree

Hide file tree

Showing 4 changed files with 134 additions and 80 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -1,41 +1,53 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.9.5)
 project(rplidar_ros)
 
+# 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)
+endif()
+
 set(RPLIDAR_SDK_PATH "./sdk/")
 
-FILE(GLOB RPLIDAR_SDK_SRC 
+FILE(GLOB RPLIDAR_SDK_SRC
   "${RPLIDAR_SDK_PATH}/src/arch/linux/*.cpp"
   "${RPLIDAR_SDK_PATH}/src/hal/*.cpp"
   "${RPLIDAR_SDK_PATH}/src/*.cpp"
 )
 
-find_package(catkin REQUIRED COMPONENTS
-  roscpp
-  rosconsole
-  sensor_msgs
+set(req_deps
+  "rclcpp"
+#  "rosconsole"
+  "sensor_msgs"
 )
 
+find_package(ament_cmake_auto REQUIRED)
+find_package(ament_cmake_ros REQUIRED)
+
+ament_auto_find_build_dependencies(REQUIRED ${req_deps})
+
 include_directories(
   ${RPLIDAR_SDK_PATH}/include
   ${RPLIDAR_SDK_PATH}/src
-  ${catkin_INCLUDE_DIRS}
 )
 
-catkin_package()
+ament_auto_add_executable(rplidarNode src/node.cpp ${RPLIDAR_SDK_SRC})
+ament_target_dependencies(rplidarNode ${req_deps})
 
-add_executable(rplidarNode src/node.cpp ${RPLIDAR_SDK_SRC})
-target_link_libraries(rplidarNode ${catkin_LIBRARIES})
+ament_auto_add_executable(rplidarNodeClient src/client.cpp)
+ament_target_dependencies(rplidarNodeClient ${req_deps})
 
-add_executable(rplidarNodeClient src/client.cpp)
-target_link_libraries(rplidarNodeClient ${catkin_LIBRARIES})
-
-install(TARGETS rplidarNode rplidarNodeClient
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+install(
+  TARGETS rplidarNode rplidarNodeClient
+  DESTINATION lib/${PROJECT_NAME}
 )
 
-install(DIRECTORY launch rviz sdk
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-  USE_SOURCE_PERMISSIONS
-)
+# TODO(hallen): port this
+# install(DIRECTORY launch rviz sdk
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+#   USE_SOURCE_PERMISSIONS
+# )
+
+ament_auto_package()
diff --git a/package.xml b/package.xml
@@ -1,20 +1,26 @@
 <?xml version="1.0"?>
-<package>
+<package format="3">
   <name>rplidar_ros</name>
   <version>1.10.0</version>
   <description>The rplidar ros package, support rplidar A2/A1 and A3/S1</description>
 
   <maintainer email="[email protected]">Slamtec ROS Maintainer</maintainer>
   <license>BSD</license>
 
-  <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>rosconsole</build_depend>
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>ament_cmake_ros</buildtool_depend>
+
+  <build_depend>rclcpp</build_depend>
+  <!-- <build_depend>rosconsole</build_depend> -->
   <build_depend>sensor_msgs</build_depend>
   <build_depend>std_srvs</build_depend>
-  <run_depend>roscpp</run_depend>
-  <run_depend>rosconsole</run_depend>
-  <run_depend>sensor_msgs</run_depend>
-  <run_depend>std_srvs</run_depend>
 
+  <exec_depend>rclcpp</exec_depend>
+  <!-- <exec_depend>rosconsole</exec_depend> -->
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_srvs</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
 </package>
diff --git a/src/client.cpp b/src/client.cpp
@@ -31,21 +31,24 @@
  *
  *  Copyright 2009 - 2014 RoboPeak Team
  *  http://www.robopeak.com
- * 
+ *
  */
 
 
-#include "ros/ros.h"
-#include "sensor_msgs/LaserScan.h"
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+
+#define ROS_INFO RCUTILS_LOG_INFO
 
+#define M_PI 3.1415926535897932384626433832795
 #define RAD2DEG(x) ((x)*180./M_PI)
 
-void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
+void scanCallback(const std::shared_ptr<sensor_msgs::msg::LaserScan> scan)
 {
     int count = scan->scan_time / scan->time_increment;
     ROS_INFO("I heard a laser scan %s[%d]:", scan->header.frame_id.c_str(), count);
     ROS_INFO("angle_range, %f, %f", RAD2DEG(scan->angle_min), RAD2DEG(scan->angle_max));
-  
+
     for(int i = 0; i < count; i++) {
         float degree = RAD2DEG(scan->angle_min + scan->angle_increment * i);
         ROS_INFO(": [%f, %f]", degree, scan->ranges[i]);
@@ -54,12 +57,9 @@ void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
 
 int main(int argc, char **argv)
 {
-    ros::init(argc, argv, "rplidar_node_client");
-    ros::NodeHandle n;
-
-    ros::Subscriber sub = n.subscribe<sensor_msgs::LaserScan>("/scan", 1000, scanCallback);
-
-    ros::spin();
-
+    rclcpp::init(argc, argv);
+    auto node = rclcpp::Node::make_shared("rplidar_node_client");
+    auto sub = node->create_subscription<sensor_msgs::msg::LaserScan>("/scan", scanCallback);
+    rclcpp::spin(node);
     return 0;
 }
diff --git a/src/node.cpp b/src/node.cpp
@@ -32,31 +32,43 @@
  *
  */
 
-#include "ros/ros.h"
-#include "sensor_msgs/LaserScan.h"
-#include "std_srvs/Empty.h"
+/* #include "ros/ros.h" */
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+#include <std_srvs/srv/empty.hpp>
+#include <rclcpp/time.hpp>
+#include <rclcpp/time_source.hpp>
+#include <rclcpp/clock.hpp>
+
+#include <string>
+
 #include "rplidar.h"
 
 #ifndef _countof
 #define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0]))
 #endif
 
+#define M_PI 3.1415926535897932384626433832795
 #define DEG2RAD(x) ((x)*M_PI/180.)
 
+#define ROS_INFO RCUTILS_LOG_INFO
+#define ROS_ERROR RCUTILS_LOG_ERROR
+#define ROS_DEBUG RCUTILS_LOG_DEBUG
+
 using namespace rp::standalone::rplidar;
 
 RPlidarDriver * drv = NULL;
 
-void publish_scan(ros::Publisher *pub,
+void publish_scan(std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::LaserScan>> pub,
                   rplidar_response_measurement_node_hq_t *nodes,
-                  size_t node_count, ros::Time start,
+                  size_t node_count, rclcpp::Time start,
                   double scan_time, bool inverted,
                   float angle_min, float angle_max,
                   float max_distance,
                   std::string frame_id)
 {
     static int scan_count = 0;
-    sensor_msgs::LaserScan scan_msg;
+    sensor_msgs::msg::LaserScan scan_msg;
 
     scan_msg.header.stamp = start;
     scan_msg.header.frame_id = frame_id;
@@ -136,7 +148,7 @@ bool checkRPLIDARHealth(RPlidarDriver * drv)
     rplidar_response_device_health_t healthinfo;
 
     op_result = drv->getHealth(healthinfo);
-    if (IS_OK(op_result)) { 
+    if (IS_OK(op_result)) {
         ROS_INFO("RPLidar health status : %d", healthinfo.status);
         if (healthinfo.status == RPLIDAR_STATUS_ERROR) {
             ROS_ERROR("Error, rplidar internal error detected. Please reboot the device to retry.");
@@ -151,8 +163,8 @@ bool checkRPLIDARHealth(RPlidarDriver * drv)
     }
 }
 
-bool stop_motor(std_srvs::Empty::Request &req,
-                               std_srvs::Empty::Response &res)
+bool stop_motor(const std::shared_ptr<std_srvs::srv::Empty::Request> req,
+                std::shared_ptr<std_srvs::srv::Empty::Response> res)
 {
   if(!drv)
        return false;
@@ -163,8 +175,8 @@ bool stop_motor(std_srvs::Empty::Request &req,
   return true;
 }
 
-bool start_motor(std_srvs::Empty::Request &req,
-                               std_srvs::Empty::Response &res)
+bool start_motor(const std::shared_ptr<std_srvs::srv::Empty::Request> req,
+                 std::shared_ptr<std_srvs::srv::Empty::Response> res)
 {
   if(!drv)
        return false;
@@ -180,8 +192,14 @@ static float getAngle(const rplidar_response_measurement_node_hq_t& node)
 }
 
 int main(int argc, char * argv[]) {
-    ros::init(argc, argv, "rplidar_node");
-
+    rclcpp::init(argc, argv);
+    auto node = rclcpp::Node::make_shared("rplidar_node");
+    /* connect to ROS clock */
+    rclcpp::Clock::SharedPtr clock;
+    rclcpp::TimeSource timesource;
+    clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
+    timesource.attachClock(clock);
+
     std::string channel_type;
     std::string tcp_ip;
     std::string serial_port;
@@ -193,18 +211,27 @@ int main(int argc, char * argv[]) {
     float max_distance = 8.0;
     int angle_compensate_multiple = 1;//it stand of angle compensate at per 1 degree
     std::string scan_mode;
-    ros::NodeHandle nh;
-    ros::Publisher scan_pub = nh.advertise<sensor_msgs::LaserScan>("scan", 1000);
-    ros::NodeHandle nh_private("~");
-    nh_private.param<std::string>("channel_type", channel_type, "serial");
-    nh_private.param<std::string>("tcp_ip", tcp_ip, "192.168.0.7"); 
-    nh_private.param<int>("tcp_port", tcp_port, 20108);
-    nh_private.param<std::string>("serial_port", serial_port, "/dev/ttyUSB0"); 
-    nh_private.param<int>("serial_baudrate", serial_baudrate, 115200/*256000*/);//ros run for A1 A2, change to 256000 if A3
-    nh_private.param<std::string>("frame_id", frame_id, "laser_frame");
-    nh_private.param<bool>("inverted", inverted, false);
-    nh_private.param<bool>("angle_compensate", angle_compensate, false);
-    nh_private.param<std::string>("scan_mode", scan_mode, std::string());
+    // ros::NodeHandle nh;
+    // auto scan_pub = nh.advertise<sensor_msgs::msg::LaserScan>("scan", 1000);
+
+    /* set up the QOS settings */
+    rmw_qos_profile_t qos = rmw_qos_profile_default;
+    qos.depth = 1;
+    qos.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+
+    /* scan publisher */
+    auto scan_pub = node->create_publisher<sensor_msgs::msg::LaserScan>("scan", qos);
+
+    /* set parameters */
+    node->get_parameter_or("channel_type", channel_type, std::string("serial"));
+    node->get_parameter_or("tcp_ip", tcp_ip, std::string("192.168.0.7"));
+    node->get_parameter_or("tcp_port", tcp_port, 20108);
+    node->get_parameter_or("serial_port", serial_port, std::string("/dev/ttyUSB0"));
+    node->get_parameter_or("serial_baudrate", serial_baudrate, 115200);
+    node->get_parameter_or("frame_id", frame_id, std::string("laser_frame"));
+    node->get_parameter_or("inverted", inverted, false);
+    node->get_parameter_or("angle_compensate", angle_compensate, false);
+    node->get_parameter_or("scan_mode", scan_mode, std::string());
 
     ROS_INFO("RPLIDAR running on ROS package rplidar_ros. SDK Version:"RPLIDAR_SDK_VERSION"");
 
@@ -254,8 +281,17 @@ int main(int argc, char * argv[]) {
         return -1;
     }
 
-    ros::ServiceServer stop_motor_service = nh.advertiseService("stop_motor", stop_motor);
-    ros::ServiceServer start_motor_service = nh.advertiseService("start_motor", start_motor);
+    // create stop motor service
+    auto stop_motor_service = node->create_service<std_srvs::srv::Empty>(
+        "stop_motor",
+        std::bind(stop_motor, std::placeholders::_1, std::placeholders::_2),
+        qos);
+
+    // create start motor service
+    auto start_motor_service = node->create_service<std_srvs::srv::Empty>(
+        "start_motor",
+        std::bind(start_motor, std::placeholders::_1, std::placeholders::_2),
+        qos);
 
     drv->startMotor();
 
@@ -292,7 +328,7 @@ int main(int argc, char * argv[]) {
     {
         //default frequent is 10 hz (by motor pwm value),  current_scan_mode.us_per_sample is the number of scan point per us
         angle_compensate_multiple = (int)(1000*1000/current_scan_mode.us_per_sample/10.0/360.0);
-        if(angle_compensate_multiple < 1) 
+        if(angle_compensate_multiple < 1)
           angle_compensate_multiple = 1;
         max_distance = current_scan_mode.max_distance;
         ROS_INFO("current scan mode: %s, max_distance: %.1f m, Point number: %.1fK , angle_compensate: %d",  current_scan_mode.scan_mode,
@@ -303,17 +339,17 @@ int main(int argc, char * argv[]) {
         ROS_ERROR("Can not start scan: %08x!", op_result);
     }
 
-    ros::Time start_scan_time;
-    ros::Time end_scan_time;
+    rclcpp::Time start_scan_time;
+    rclcpp::Time end_scan_time;
     double scan_duration;
-    while (ros::ok()) {
+    while (rclcpp::ok()) {
         rplidar_response_measurement_node_hq_t nodes[360*8];
         size_t   count = _countof(nodes);
 
-        start_scan_time = ros::Time::now();
+        start_scan_time = clock->now();
         op_result = drv->grabScanDataHq(nodes, count);
-        end_scan_time = ros::Time::now();
-        scan_duration = (end_scan_time - start_scan_time).toSec();
+        end_scan_time = clock->now();
+        scan_duration = (end_scan_time - start_scan_time).nanoseconds() * 1E-9;
 
         if (op_result == RESULT_OK) {
             op_result = drv->ascendScanData(nodes, count);
@@ -338,8 +374,8 @@ int main(int argc, char * argv[]) {
                             }
                         }
                     }
-  
-                    publish_scan(&scan_pub, angle_compensate_nodes, angle_compensate_nodes_count,
+
+                    publish_scan(scan_pub, angle_compensate_nodes, angle_compensate_nodes_count,
                              start_scan_time, scan_duration, inverted,
                              angle_min, angle_max, max_distance,
                              frame_id);
@@ -356,7 +392,7 @@ int main(int argc, char * argv[]) {
                     angle_min = DEG2RAD(getAngle(nodes[start_node]));
                     angle_max = DEG2RAD(getAngle(nodes[end_node]));
 
-                    publish_scan(&scan_pub, &nodes[start_node], end_node-start_node +1,
+                    publish_scan(scan_pub, &nodes[start_node], end_node-start_node +1,
                              start_scan_time, scan_duration, inverted,
                              angle_min, angle_max, max_distance,
                              frame_id);
@@ -366,14 +402,14 @@ int main(int argc, char * argv[]) {
                 float angle_min = DEG2RAD(0.0f);
                 float angle_max = DEG2RAD(359.0f);
 
-                publish_scan(&scan_pub, nodes, count,
+                publish_scan(scan_pub, nodes, count,
                              start_scan_time, scan_duration, inverted,
                              angle_min, angle_max, max_distance,
                              frame_id);
             }
         }
 
-        ros::spinOnce();
+        // rclcpp::spin(node);
     }
 
     // done!