Merge pull request #11 from elephantrobotics/humble-dev

fix(agv_pro_bringup): add launch with RViz condition

agv_pro_base/include/agv_pro_base/agv_pro_driver.h

@@ -15,6 +15,8 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #include <agv_pro_msgs/srv/set_digital_output.hpp>
 #include <agv_pro_msgs/srv/get_digital_input.hpp>
+#include <agv_pro_msgs/srv/set_led_color.hpp>
+#include <agv_pro_msgs/srv/set_led_mode.hpp>
 
 #define SEND_DATA_SIZE 14                               // Total bytes in a command frame to ESP32(version>=V1.0.8)
 #define RECEIVE_FRAME_SIZE 31                           // Total bytes in a frame from ESP32(version>=V1.0.8)
@@ -23,6 +25,8 @@
 #define POWER_ON 0x10
 #define GET_POWER_STATE 0x12
 #define SET_AUTO_REPORT_STATE 0x23
+#define SET_LED_COLOR 0x34
+#define SET_LED_MODE 0x3A
 #define SET_OUTPUT_IO 0x40
 #define GET_INPUT_IO 0x41
 
@@ -196,6 +200,14 @@ private:
     const std::shared_ptr<agv_pro_msgs::srv::GetDigitalInput::Request> request,
     std::shared_ptr<agv_pro_msgs::srv::GetDigitalInput::Response> response);
 
+  void handleSetLedColor(
+    const std::shared_ptr<agv_pro_msgs::srv::SetLedColor::Request> request,
+    std::shared_ptr<agv_pro_msgs::srv::SetLedColor::Response> response);
+
+  void handleSetLedMode(
+    const std::shared_ptr<agv_pro_msgs::srv::SetLedMode::Request> request,
+    std::shared_ptr<agv_pro_msgs::srv::SetLedMode::Response> response);
+
   boost::asio::io_service io_;
   std::unique_ptr<boost::asio::serial_port> serial_port_;
 
@@ -262,6 +274,8 @@ private:
   rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_sub;
   rclcpp::Service<agv_pro_msgs::srv::SetDigitalOutput>::SharedPtr set_output_service;
   rclcpp::Service<agv_pro_msgs::srv::GetDigitalInput>::SharedPtr get_input_service;
+  rclcpp::Service<agv_pro_msgs::srv::SetLedColor>::SharedPtr set_led_service;
+  rclcpp::Service<agv_pro_msgs::srv::SetLedMode>::SharedPtr set_led_mode_service;
 
   sensor_msgs::msg::Imu imu_data;
   std::unique_ptr<tf2_ros::TransformBroadcaster> odomBroadcaster;

agv_pro_base/src/agv_pro_ros.cpp

@@ -280,7 +280,7 @@ void AGV_PRO::handleSetDigitalOutput(
 
   uint8_t status = response_frame[4];
   if (status == 0x01) {
-    RCLCPP_INFO(this->get_logger(), "SetDigitalOutput succeeded");
+    RCLCPP_DEBUG(this->get_logger(), "SetDigitalOutput succeeded");
     response->success = true;
     response->message = "Success";
   } else {
@@ -315,13 +315,96 @@ void AGV_PRO::handleGetDigitalInput(
     RCLCPP_ERROR(this->get_logger(), "GetDigitalInput failed with status: 0x%02X", status);
     response->success = false;
   } else {
-    RCLCPP_INFO(this->get_logger(), "GetDigitalInput succeeded, state: %u", status);
+    RCLCPP_DEBUG(this->get_logger(), "GetDigitalInput succeeded, state: %u", status);
     response->state = static_cast<int32_t>(status);
     response->success = true;
     response->message = "Success";
   }
 }
 
+void AGV_PRO::handleSetLedColor(
+  const std::shared_ptr<agv_pro_msgs::srv::SetLedColor::Request> request,
+  std::shared_ptr<agv_pro_msgs::srv::SetLedColor::Response> response)
+{
+  if (request->position < 0 || request->position > 1) {
+    RCLCPP_ERROR(this->get_logger(), "Invalid LED position: %d", request->position);
+    response->success = false;
+    response->message = "Invalid LED position";
+    return;
+  }
+
+  if (request->brightness < 0 || request->brightness > 255) {
+    RCLCPP_ERROR(this->get_logger(), "Invalid brightness: %d", request->brightness);
+    response->success = false;
+    response->message = "Invalid brightness";
+    return;
+  }
+
+  if (request->r < 0 || request->r > 255 ||
+      request->g < 0 || request->g > 255 ||
+      request->b < 0 || request->b > 255) {
+    RCLCPP_ERROR(
+      this->get_logger(),
+      "Invalid RGB value: r=%d g=%d b=%d",
+      request->r, request->g, request->b
+    );
+    response->success = false;
+    response->message = "Invalid RGB value";
+    return;
+  }
+
+  uint8_t position   = static_cast<uint8_t>(request->position);
+  uint8_t brightness = static_cast<uint8_t>(request->brightness);
+  uint8_t r          = static_cast<uint8_t>(request->r);
+  uint8_t g          = static_cast<uint8_t>(request->g);
+  uint8_t b          = static_cast<uint8_t>(request->b);
+
+  auto frame = build_serial_frame(SET_LED_COLOR, {position, brightness, r, g, b});
+  send_serial_frame(frame, true);
+
+  const std::vector<uint8_t> expected_header = {0xFE, 0xFE, 0x0B, SET_LED_COLOR};
+  auto response_frame = read_serial_response(expected_header, 8, 5.0);
+
+  // print_hex("recv_buf", response_frame); //debug
+
+  uint8_t status = response_frame[4];
+  if (status == 0x01) {
+    RCLCPP_DEBUG(this->get_logger(), "SetLedColor succeeded");
+    response->success = true;
+    response->message = "Success";
+  } else {
+    RCLCPP_ERROR(this->get_logger(), "SetLedColor failed with status: 0x%02X", status);
+    response->success = false;
+    response->message = "Failed with status code";
+  }
+}
+
+void AGV_PRO::handleSetLedMode(
+  const std::shared_ptr<agv_pro_msgs::srv::SetLedMode::Request> request,
+  std::shared_ptr<agv_pro_msgs::srv::SetLedMode::Response> response)
+{
+  uint8_t mode = request->mode ? 0x01 : 0x00;
+
+  auto frame = build_serial_frame(SET_LED_MODE, {mode});
+  send_serial_frame(frame, true);
+
+  const std::vector<uint8_t> expected_header = {0xFE, 0xFE, 0x0B, SET_LED_MODE};
+  auto response_frame = read_serial_response(expected_header, 8, 5.0);
+
+  // print_hex("recv_buf", response_frame); //debug
+
+  uint8_t status = response_frame[4];
+  if (status == 0x01) {
+    RCLCPP_DEBUG(this->get_logger(), "SetLedMode succeeded");
+    response->success = true;
+    response->message = "Success";
+  } else {
+    RCLCPP_ERROR(this->get_logger(), "SetLedMode failed with status: 0x%02X", status);
+    response->success = false;
+    response->message = "Failed with status code";
+  }
+}
+
 bool AGV_PRO::readData()
 {
   std::vector<uint8_t> buf_length(1);
@@ -569,6 +652,16 @@ AGV_PRO::AGV_PRO(std::string node_name):rclcpp::Node(node_name)
     std::bind(&AGV_PRO::handleGetDigitalInput, this, std::placeholders::_1, std::placeholders::_2)
   );
 
+  set_led_service = this->create_service<agv_pro_msgs::srv::SetLedColor>(
+    "set_led_color",
+    std::bind(&AGV_PRO::handleSetLedColor, this, std::placeholders::_1, std::placeholders::_2)
+  );
+
+  set_led_mode_service = this->create_service<agv_pro_msgs::srv::SetLedMode>(
+    "set_led_mode",
+    std::bind(&AGV_PRO::handleSetLedMode, this, std::placeholders::_1, std::placeholders::_2)
+  );
+
   lastTime = this->get_clock()->now();
       
   try{

agv_pro_bringup/launch/agv_pro_bringup.launch.py

@@ -1,12 +1,13 @@
 import os
 from launch import LaunchDescription
+from launch.conditions import IfCondition
 from launch_ros.actions import Node,PushRosNamespace
 from launch.actions import DeclareLaunchArgument,IncludeLaunchDescription
 from launch.substitutions import Command,LaunchConfiguration,PythonExpression
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from ament_index_python.packages import get_package_share_directory
 
-def include_lidar(pkg_name, launch_file, lidar_type, expected_type):
+def include_lidar(pkg_name, launch_file, enable_lidar, lidar_type, expected_type):
 
     return IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
@@ -16,16 +17,20 @@ def include_lidar(pkg_name, launch_file, lidar_type, expected_type):
                 launch_file
             )
         ),
-        condition=PythonExpression(
-            ["'", lidar_type, "' == '", expected_type, "'"]
+        condition=IfCondition(
+            PythonExpression([
+                "'", enable_lidar, "' == 'true' and '",
+                lidar_type, "' == '", expected_type, "'"
+            ])
         )
     )
 
 def generate_launch_description():
 
-    port_name_arg = LaunchConfiguration('port_name',default='/dev/agvpro_controller')
-    namespace = LaunchConfiguration('namespace', default='')
-    lidar_type = LaunchConfiguration('lidar_type', default='n10p')
+    port_name_arg = LaunchConfiguration('port_name')
+    namespace = LaunchConfiguration('namespace')
+    lidar_type = LaunchConfiguration('lidar_type')
+    enable_lidar = LaunchConfiguration('enable_lidar')
 
     urdf_file = os.path.join(
         get_package_share_directory('agv_pro_description'),
@@ -42,7 +47,7 @@ def generate_launch_description():
 
     declare_port_name_arg = DeclareLaunchArgument(
         'port_name', 
-        default_value=port_name_arg,
+        default_value='/dev/agvpro_controller',
         description='port name, e.g. /dev/ttyACM0'
     )
 
@@ -52,9 +57,15 @@ def generate_launch_description():
         description='Namespace for nodes'
     )
 
+    declare_enable_lidar_arg = DeclareLaunchArgument(
+        'enable_lidar',
+        default_value='true',
+        description='Whether to launch lidar drivers'
+    )
+
     declare_lidar_type_arg = DeclareLaunchArgument(
         'lidar_type',
-        default_value=lidar_type,
+        default_value='n10p',
         description='Lidar type: n10p | mid360 | l2'
     )
 
@@ -87,15 +98,16 @@ def generate_launch_description():
     )
 
     lidar_launchs = [
-        include_lidar('lslidar_driver', 'lsn10p_launch.py', lidar_type, 'n10p'),
-        include_lidar('livox_ros_driver2', 'msg_MID360_launch.py', lidar_type, 'mid360'),
-        include_lidar('unitree_lidar_ros2', 'launch.py', lidar_type, 'l2'),
+        include_lidar('lslidar_driver', 'lsn10p_launch.py', enable_lidar, lidar_type, 'n10p'),
+        include_lidar('livox_ros_driver2', 'MID360_launch.py',enable_lidar, lidar_type, 'mid360'),
+        include_lidar('unitree_lidar_ros2', 'launch.py', enable_lidar, lidar_type, 'l2'),
     ]
 
     return LaunchDescription(
         [
             declare_port_name_arg,
             declare_namespace_arg,
+            declare_enable_lidar_arg,
             declare_lidar_type_arg,
             ns_action,
             agv_pro_node,

agv_pro_msgs/CMakeLists.txt

@@ -17,6 +17,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/AGVProStatus.msg"
   "srv/SetDigitalOutput.srv"
   "srv/GetDigitalInput.srv"
+  "srv/SetLedColor.srv"
+  "srv/SetLedMode.srv"
   DEPENDENCIES std_msgs
 )
 

agv_pro_msgs/scripts/ros_client.py

@@ -0,0 +1,133 @@
+#!/usr/bin/env python3
+import rclpy
+import time
+from rclpy.node import Node
+
+from agv_pro_msgs.srv import (
+    SetDigitalOutput,
+    GetDigitalInput,
+    SetLedColor,
+    SetLedMode
+)
+
+class AGVIOClient(Node):
+    def __init__(self):
+        super().__init__('agv_io_client')
+
+        # Create service client
+        self.cli_set_io = self.create_client(SetDigitalOutput, 'set_digital_output')
+        self.cli_get_io = self.create_client(GetDigitalInput, 'get_digital_input')
+        self.cli_led_output = self.create_client(SetLedColor, 'set_led_color')
+        self.cli_led_mode = self.create_client(SetLedMode, 'set_led_mode')
+
+        # Wait until all services are available
+        self._wait_for_services()
+
+    def _wait_for_services(self):
+        """Wait for all services to become available."""
+        clients = [
+            self.cli_set_io,
+            self.cli_get_io,
+            self.cli_led_output,
+            self.cli_led_mode
+        ]
+
+        for cli in clients:
+            while not cli.wait_for_service(timeout_sec=1.0):
+                pass
+
+    def _call_service(self, client, request):
+        future = client.call_async(request)
+        rclpy.spin_until_future_complete(self, future, timeout_sec=5.0)
+
+        if future.result() is not None:
+            return future.result()
+        else:
+            self.get_logger().error(f'Service call failed: {future.exception()}')
+            return None
+
+    # ------------------------------
+    # Set digital output
+    # ------------------------------
+    def set_digital_output(self, pin: int, state: int) -> bool:
+        """Set digital output pin state."""
+        req = SetDigitalOutput.Request()
+        req.pin = pin
+        req.state = state
+
+        res = self._call_service(self.cli_set_io, req)
+        return res.success if res else False
+
+    # ------------------------------
+    # Get digital input
+    # ------------------------------
+    def get_digital_input(self, pin: int):
+        """Read digital input pin state."""
+        req = GetDigitalInput.Request()
+        req.pin = pin
+
+        res = self._call_service(self.cli_get_io, req)
+        return res.state if (res and res.success) else None
+
+    # ------------------------------
+    # Set LED color
+    # ------------------------------
+    def set_led_color(self,
+                      position: int,
+                      brightness: int,
+                      r: int,
+                      g: int,
+                      b: int) -> bool:
+        """Set LED RGB color and brightness."""
+        req = SetLedColor.Request()
+        req.position = position
+        req.brightness = brightness
+        req.r = r
+        req.g = g
+        req.b = b
+
+        res = self._call_service(self.cli_led_output, req)
+        return res.success if res else False
+
+    # ------------------------------
+    # Set LED mode
+    # ------------------------------
+    def set_led_mode(self, mode: bool) -> bool:
+        """Set LED mode (True/False)."""
+        req = SetLedMode.Request()
+        req.mode = mode
+
+        res = self._call_service(self.cli_led_mode, req)
+        return res.success if res else False
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    client = AGVIOClient()
+
+    ################
+    # Example usage:
+    ################
+
+    # client.set_digital_output(pin=1, state=1)
+    # client.get_digital_input(pin=2)
+
+    # client.set_led_mode(True)
+    # for i in range(10):
+    #     client.set_led_color(0, 100, 255, 255, 0)
+    #     client.set_led_color(1, 100, 255, 255, 0)
+    #     time.sleep(0.5)
+    #     client.set_led_color(0, 0, 0, 0, 0)
+    #     client.set_led_color(1, 0, 0, 0, 0)
+    #     time.sleep(0.5)
+
+    # client.set_led_color(0, 100, 255, 255, 0)
+    # client.set_led_color(1, 100, 255, 255, 0)
+    # client.set_led_mode(True)
+
+    client.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

agv_pro_msgs/srv/SetLedColor.srv

@@ -0,0 +1,8 @@
+int32 position
+int32 brightness
+int32 r
+int32 g
+int32 b
+---
+bool success
+string message

agv_pro_msgs/srv/SetLedMode.srv

@@ -0,0 +1,4 @@
+bool mode
+---
+bool success
+string message

livox_ros_driver2/.gitignore

@@ -1,4 +1,3 @@
 .vscode
 build
-package.xml
-__pycache__
+__pycache__

livox_ros_driver2/CMakeLists.txt

@@ -1,336 +1,140 @@
-# judge which cmake codes to use 
-if(ROS_EDITION STREQUAL "ROS1")
+# Copyright(c) 2020 livoxtech limited.
 
-  # Copyright(c) 2019 livoxtech limited.
+cmake_minimum_required(VERSION 3.14)
+project(livox_ros_driver2)
 
-  cmake_minimum_required(VERSION 3.0)
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
 
-
-  #---------------------------------------------------------------------------------------
-  # Start livox_ros_driver2 project
-  #---------------------------------------------------------------------------------------
-  include(cmake/version.cmake)
-  project(livox_ros_driver2 VERSION ${LIVOX_ROS_DRIVER2_VERSION} LANGUAGES CXX)
-  message(STATUS "livox_ros_driver2 version: ${LIVOX_ROS_DRIVER2_VERSION}")
-
-  #---------------------------------------------------------------------------------------
-  # Add ROS Version MACRO
-  #---------------------------------------------------------------------------------------
-  add_definitions(-DBUILDING_ROS1)
-
-  #---------------------------------------------------------------------------------------
-  # find package and the dependecy
-  #---------------------------------------------------------------------------------------
-  find_package(Boost 1.54 REQUIRED COMPONENTS
-    system
-    thread
-    chrono
-  )
-
-  ## Find catkin macros and libraries
-  ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-  ## is used, also find other catkin packages
-  find_package(catkin REQUIRED COMPONENTS
-    roscpp
-    rospy
-    sensor_msgs
-    std_msgs
-    message_generation
-    rosbag
-    pcl_ros
-  )
-
-  ## Find pcl lib
-  find_package(PCL REQUIRED)
-
-  ## Generate messages in the 'msg' folder
-  add_message_files(FILES
-    CustomPoint.msg
-    CustomMsg.msg
-  #   Message2.msg
-  )
-
-  ## Generate added messages and services with any dependencies listed here
-  generate_messages(DEPENDENCIES
-    std_msgs
-  )
-
-  find_package(PkgConfig)
-  pkg_check_modules(APR apr-1)
-  if (APR_FOUND)
-    message(${APR_INCLUDE_DIRS})
-    message(${APR_LIBRARIES})
-  endif (APR_FOUND)
-
-  ###################################
-  ## catkin specific configuration ##
-  ###################################
-  ## The catkin_package macro generates cmake config files for your package
-  ## Declare things to be passed to dependent projects
-  ## INCLUDE_DIRS: uncomment this if your package contains header files
-  ## LIBRARIES: libraries you create in this project that dependent projects als    o need
-  ## CATKIN_DEPENDS: catkin_packages dependent projects also need
-  ## DEPENDS: system dependencies of this project that dependent projects also n    eed
-  catkin_package(CATKIN_DEPENDS
-    roscpp rospy std_msgs message_runtime
-    pcl_ros
-  )
-
-  #---------------------------------------------------------------------------------------
-  # Set default build to release
-  #---------------------------------------------------------------------------------------
-  if(NOT CMAKE_BUILD_TYPE)
-    set(CMAKE_BUILD_TYPE "Release" CACHE STRING "Choose Release or Debug" FORCE)
-  endif()
-
-  #---------------------------------------------------------------------------------------
-  # Compiler config
-  #---------------------------------------------------------------------------------------
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 14)
-  set(CMAKE_CXX_STANDARD_REQUIRED ON)
-  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
 
-  ## make sure the livox_lidar_sdk_static library is installed
-  find_library(LIVOX_LIDAR_SDK_LIBRARY  liblivox_lidar_sdk_static.a    /usr/local/lib)
+list(INSERT CMAKE_MODULE_PATH 0 "${PROJECT_SOURCE_DIR}/cmake/modules")
 
-  ## PCL library
-  link_directories(${PCL_LIBRARY_DIRS})
-  add_definitions(${PCL_DEFINITIONS})
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter)
+endif()
 
-  #---------------------------------------------------------------------------------------
-  # generate excutable and add libraries
-  #---------------------------------------------------------------------------------------
-  add_executable(${PROJECT_NAME}_node
-    ""
-  )
+# Printf version info
+include(cmake/version.cmake)
+project(${PROJECT_NAME} VERSION ${LIVOX_ROS_DRIVER2_VERSION} LANGUAGES CXX)
+message(STATUS "${PROJECT_NAME} version: ${LIVOX_ROS_DRIVER2_VERSION}")
 
-  #---------------------------------------------------------------------------------------
-  # precompile macro and compile option
-  #---------------------------------------------------------------------------------------
-  target_compile_options(${PROJECT_NAME}_node
-    PRIVATE $<$<CXX_COMPILER_ID:GNU>:-Wall>
-  )
+#---------------------------------------------------------------------------------------
+# Add ROS Version MACRO
+#---------------------------------------------------------------------------------------
+add_definitions(-DBUILDING_ROS2)
 
-  #---------------------------------------------------------------------------------------
-  # add projects that depend on
-  #---------------------------------------------------------------------------------------
-  add_dependencies(${PROJECT_NAME}_node ${PROJECT_NAME}_generate_messages_cpp)
+# find dependencies
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+find_package(PCL REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
 
-  #---------------------------------------------------------------------------------------
-  # source file
-  #---------------------------------------------------------------------------------------
-  target_sources(${PROJECT_NAME}_node
-    PRIVATE
-    src/driver_node.cpp
-    src/lds.cpp
-    src/lds_lidar.cpp
-    src/lddc.cpp
-    src/livox_ros_driver2.cpp
+# check apr
+find_package(PkgConfig)
+pkg_check_modules(APR apr-1)
+if (APR_FOUND)
+  message(${APR_INCLUDE_DIRS})
+  message(${APR_LIBRARIES})
+endif (APR_FOUND)
 
-    src/comm/comm.cpp
-    src/comm/ldq.cpp
-    src/comm/semaphore.cpp
-    src/comm/lidar_imu_data_queue.cpp
-    src/comm/cache_index.cpp
-    src/comm/pub_handler.cpp
+# generate custom msg headers
+set(LIVOX_INTERFACES livox_interfaces2)
+rosidl_generate_interfaces(${LIVOX_INTERFACES}
+  "msg/CustomPoint.msg"
+  "msg/CustomMsg.msg"
+  DEPENDENCIES builtin_interfaces std_msgs
+  LIBRARY_NAME ${PROJECT_NAME}
+)
 
-    src/parse_cfg_file/parse_cfg_file.cpp
-    src/parse_cfg_file/parse_livox_lidar_cfg.cpp
+## make sure the livox_lidar_sdk_shared library is installed
+find_library(LIVOX_LIDAR_SDK_LIBRARY liblivox_lidar_sdk_shared.so /usr/local/lib REQUIRED)
 
-    src/call_back/lidar_common_callback.cpp
-    src/call_back/livox_lidar_callback.cpp
-  )
+##
+find_path(LIVOX_LIDAR_SDK_INCLUDE_DIR
+  NAMES "livox_lidar_api.h" "livox_lidar_def.h"
+  REQUIRED)
 
-  #---------------------------------------------------------------------------------------
-  # include file
-  #---------------------------------------------------------------------------------------
-  target_include_directories(${PROJECT_NAME}_node
-    PUBLIC
-    ${catkin_INCLUDE_DIRS}
-    ${PCL_INCLUDE_DIRS}
-    ${APR_INCLUDE_DIRS}
-    3rdparty
-    src
-  )
+## PCL library
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
 
-  #---------------------------------------------------------------------------------------
-  # link libraries
-  #---------------------------------------------------------------------------------------
-  target_link_libraries(${PROJECT_NAME}_node
-    ${LIVOX_LIDAR_SDK_LIBRARY}
-    ${Boost_LIBRARY}
-    ${catkin_LIBRARIES}
-    ${PCL_LIBRARIES}
-    ${APR_LIBRARIES}
-  )
+# livox ros2 driver target
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/livox_ros_driver2.cpp
+  src/lddc.cpp
+  src/driver_node.cpp
+  src/lds.cpp
+  src/lds_lidar.cpp
 
+  src/comm/comm.cpp
+  src/comm/ldq.cpp
+  src/comm/semaphore.cpp
+  src/comm/lidar_imu_data_queue.cpp
+  src/comm/cache_index.cpp
+  src/comm/pub_handler.cpp
 
-  #---------------------------------------------------------------------------------------
-  # Install
-  #---------------------------------------------------------------------------------------
+  src/parse_cfg_file/parse_cfg_file.cpp
+  src/parse_cfg_file/parse_livox_lidar_cfg.cpp
 
-  install(TARGETS ${PROJECT_NAME}_node
-    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-  )
+  src/call_back/lidar_common_callback.cpp
+  src/call_back/livox_lidar_callback.cpp
+)
 
-  install(DIRECTORY launch_ROS1/
-    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch_ROS1
-  )
+target_include_directories(${PROJECT_NAME} PRIVATE ${livox_sdk_INCLUDE_DIRS})
 
-  #---------------------------------------------------------------------------------------
-  # end of CMakeList.txt
-  #---------------------------------------------------------------------------------------
+# livox ros2 driver target
+rosidl_get_typesupport_target(cpp_typesupport_target
+${LIVOX_INTERFACES} "rosidl_typesupport_cpp")
+target_link_libraries(${PROJECT_NAME} "${cpp_typesupport_target}")
 
+# include file direcotry
+target_include_directories(${PROJECT_NAME} PUBLIC
+  ${PCL_INCLUDE_DIRS}
+  ${APR_INCLUDE_DIRS}
+  ${LIVOX_LIDAR_SDK_INCLUDE_DIR}
+  ${LIVOX_INTERFACES_INCLUDE_DIRECTORIES}   # for custom msgs
+  3rdparty
+  src
+)
 
-else(ROS_EDITION STREQUAL "ROS2")
+# link libraries
+target_link_libraries(${PROJECT_NAME}
+  ${LIVOX_LIDAR_SDK_LIBRARY}
+  ${LIVOX_INTERFACE_TARGET}   # for custom msgs
+  ${PPT_LIBRARY}
+  ${Boost_LIBRARY}
+  ${PCL_LIBRARIES}
+  ${APR_LIBRARIES}
+)
 
-  # Copyright(c) 2020 livoxtech limited.
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "livox_ros::DriverNode"
+  EXECUTABLE ${PROJECT_NAME}_node
+)
 
-  cmake_minimum_required(VERSION 3.14)
-  project(livox_ros_driver2)
+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()
 
-  # 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()
-
-  list(INSERT CMAKE_MODULE_PATH 0 "${PROJECT_SOURCE_DIR}/cmake/modules")
-
-  if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-    add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter)
-  endif()
-
-  # Printf version info
-  include(cmake/version.cmake)
-  project(${PROJECT_NAME} VERSION ${LIVOX_ROS_DRIVER2_VERSION} LANGUAGES CXX)
-  message(STATUS "${PROJECT_NAME} version: ${LIVOX_ROS_DRIVER2_VERSION}")
-
-  #---------------------------------------------------------------------------------------
-  # Add ROS Version MACRO
-  #---------------------------------------------------------------------------------------
-  add_definitions(-DBUILDING_ROS2)
-
-  # find dependencies
-  # uncomment the following section in order to fill in
-  # further dependencies manually.
-  # find_package(<dependency> REQUIRED)
-  find_package(ament_cmake_auto REQUIRED)
-  ament_auto_find_build_dependencies()
-  find_package(PCL REQUIRED)
-  find_package(std_msgs REQUIRED)
-  find_package(builtin_interfaces REQUIRED)
-  find_package(rosidl_default_generators REQUIRED)
-
-  # check apr
-  find_package(PkgConfig)
-  pkg_check_modules(APR apr-1)
-  if (APR_FOUND)
-    message(${APR_INCLUDE_DIRS})
-    message(${APR_LIBRARIES})
-  endif (APR_FOUND)
-
-  # generate custom msg headers
-  set(LIVOX_INTERFACES livox_interfaces2)
-  rosidl_generate_interfaces(${LIVOX_INTERFACES}
-    "msg/CustomPoint.msg"
-    "msg/CustomMsg.msg"
-    DEPENDENCIES builtin_interfaces std_msgs
-    LIBRARY_NAME ${PROJECT_NAME}
-  )
-
-  ## make sure the livox_lidar_sdk_shared library is installed
-  find_library(LIVOX_LIDAR_SDK_LIBRARY liblivox_lidar_sdk_shared.so /usr/local/lib REQUIRED)
-
-  ##
-  find_path(LIVOX_LIDAR_SDK_INCLUDE_DIR
-    NAMES "livox_lidar_api.h" "livox_lidar_def.h"
-    REQUIRED)
-
-  ## PCL library
-  link_directories(${PCL_LIBRARY_DIRS})
-  add_definitions(${PCL_DEFINITIONS})
-
-  # livox ros2 driver target
-  ament_auto_add_library(${PROJECT_NAME} SHARED
-    src/livox_ros_driver2.cpp
-    src/lddc.cpp
-    src/driver_node.cpp
-    src/lds.cpp
-    src/lds_lidar.cpp
-
-    src/comm/comm.cpp
-    src/comm/ldq.cpp
-    src/comm/semaphore.cpp
-    src/comm/lidar_imu_data_queue.cpp
-    src/comm/cache_index.cpp
-    src/comm/pub_handler.cpp
-
-    src/parse_cfg_file/parse_cfg_file.cpp
-    src/parse_cfg_file/parse_livox_lidar_cfg.cpp
-
-    src/call_back/lidar_common_callback.cpp
-    src/call_back/livox_lidar_callback.cpp
-  )
-
-  target_include_directories(${PROJECT_NAME} PRIVATE ${livox_sdk_INCLUDE_DIRS})
-
-  # get include directories of custom msg headers
-  if(HUMBLE_ROS STREQUAL "humble")
-    rosidl_get_typesupport_target(cpp_typesupport_target
-    ${LIVOX_INTERFACES} "rosidl_typesupport_cpp")
-    target_link_libraries(${PROJECT_NAME} "${cpp_typesupport_target}")
-  else()
-    set(LIVOX_INTERFACE_TARGET "${LIVOX_INTERFACES}__rosidl_typesupport_cpp")
-    add_dependencies(${PROJECT_NAME} ${LIVOX_INTERFACES})
-    get_target_property(LIVOX_INTERFACES_INCLUDE_DIRECTORIES ${LIVOX_INTERFACE_TARGET} INTERFACE_INCLUDE_DIRECTORIES)
-  endif()
-
-  # include file direcotry
-  target_include_directories(${PROJECT_NAME} PUBLIC
-    ${PCL_INCLUDE_DIRS}
-    ${APR_INCLUDE_DIRS}
-    ${LIVOX_LIDAR_SDK_INCLUDE_DIR}
-    ${LIVOX_INTERFACES_INCLUDE_DIRECTORIES}   # for custom msgs
-    3rdparty
-    src
-  )
-
-  # link libraries
-  target_link_libraries(${PROJECT_NAME}
-    ${LIVOX_LIDAR_SDK_LIBRARY}
-    ${LIVOX_INTERFACE_TARGET}   # for custom msgs
-    ${PPT_LIBRARY}
-    ${Boost_LIBRARY}
-    ${PCL_LIBRARIES}
-    ${APR_LIBRARIES}
-  )
-
-  rclcpp_components_register_node(${PROJECT_NAME}
-    PLUGIN "livox_ros::DriverNode"
-    EXECUTABLE ${PROJECT_NAME}_node
-  )
-
-  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()
-
-  ament_auto_package(INSTALL_TO_SHARE
-    config
-    launch_ROS2
-  )
-
-endif()
+ament_auto_package(INSTALL_TO_SHARE
+  config
+  launch
+)

livox_ros_driver2/launch/MID360_launch.py

@@ -0,0 +1,89 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PythonExpression
+from launch_ros.actions import Node
+
+################### user configure parameters for ros2 start ###################
+# xfer_format   = 0    # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format
+multi_topic   = 0    # 0-All LiDARs share the same topic, 1-One LiDAR one topic
+data_src      = 0    # 0-lidar, others-Invalid data src
+# publish_freq  = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc.
+output_type   = 0
+frame_id      = 'laser_link'
+lvx_file_path = '/home/livox/livox_test.lvx'
+cmdline_bd_code = 'livox0000000001'
+
+cur_path = os.path.split(os.path.realpath(__file__))[0] + '/'
+cur_config_path = cur_path + '../config'
+rviz_config_path = os.path.join(cur_config_path, 'display_point_cloud_ROS2.rviz')
+user_config_path = os.path.join(cur_config_path, 'MID360_config.json')
+################### user configure parameters for ros2 end #####################
+
+def generate_launch_description():
+
+    use_mid360_rviz = LaunchConfiguration('use_mid360_rviz')
+    xfer_format = LaunchConfiguration('xfer_format')
+    publish_freq = LaunchConfiguration('publish_freq')
+
+    declare_rviz_arg = DeclareLaunchArgument(
+        'use_mid360_rviz',
+        default_value='false',
+        description='Enable RViz for MID360 LiDAR visualization'
+    )
+
+    declare_xfer_format_arg = DeclareLaunchArgument(
+        'xfer_format',
+        default_value='0',
+        description='MID360 format: 0=Pointcloud2(PointXYZRTL), 1=customized pointcloud format'
+    )
+
+    declare_publish_freq_arg = DeclareLaunchArgument(
+        'publish_freq',
+        default_value='10.0',
+        description='MID360 LiDAR publish frequency in Hz (e.g., 5.0, 10.0, 20.0, 50.0)'
+    )
+
+    livox_ros2_params = [
+        {"xfer_format": xfer_format},
+        {"multi_topic": multi_topic},
+        {"data_src": data_src},
+        {"publish_freq": publish_freq},
+        {"output_data_type": output_type},
+        {"frame_id": frame_id},
+        {"lvx_file_path": lvx_file_path},
+        {"user_config_path": user_config_path},
+        {"cmdline_input_bd_code": cmdline_bd_code}
+    ]
+
+    livox_driver = Node(
+        package='livox_ros_driver2',
+        executable='livox_ros_driver2_node',
+        name='livox_lidar_publisher',
+        output='screen',
+        parameters=livox_ros2_params
+        )
+
+    livox_rviz = Node(
+            package='rviz2',
+            executable='rviz2',
+            name='mid360_lidar_rviz',
+            output='screen',
+            arguments=['--display-config', rviz_config_path],
+            condition=IfCondition(
+                PythonExpression([
+                    "'", use_mid360_rviz, "' == 'true' and '",
+                    xfer_format, "' == '0'"
+                ])
+            )
+        )
+
+    return LaunchDescription([
+        declare_rviz_arg,
+        declare_xfer_format_arg,
+        declare_publish_freq_arg,
+        livox_driver,
+        livox_rviz,
+    ])

unitree_lidar_ros2/CMakeLists.txt

@@ -1,12 +1,13 @@
 cmake_minimum_required(VERSION 3.5)
+
+cmake_policy(SET CMP0074 NEW)
+
 project(unitree_lidar_ros2)
 
-# 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 17)
 endif()
@@ -24,42 +25,51 @@ find_package(PCL REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(sensor_msgs REQUIRED)
 
+# SDK include path
 include_directories(
-  ${PCL_INCLUDE_DIRS} 
   include
-  ../unitree_lidar_sdk/include
-)
-
-link_directories(
-  ${PCL_LIBRARY_DIRS}
-  ../unitree_lidar_sdk/lib/${CMAKE_SYSTEM_PROCESSOR}
+  /usr/local/include/unitree_lidar_sdk
+  ${PCL_INCLUDE_DIRS}
 )
 
 add_definitions(${PCL_DEFINITIONS})
 
-add_executable(unitree_lidar_ros2_node src/unitree_lidar_ros2_node.cpp)
+# SDK library path by architecture
+if(CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64")
+  set(UNITREE_LIDAR_SDK_LIB
+    /usr/local/lib/unitree_lidar_sdk/aarch64/libunilidar_sdk2.a
+  )
+else()
+  set(UNITREE_LIDAR_SDK_LIB
+    /usr/local/lib/unitree_lidar_sdk/x86_64/libunilidar_sdk2.a
+  )
+endif()
 
-target_link_libraries( unitree_lidar_ros2_node
-  ${Boost_SYSTEM_LIBRARY}
+add_executable(unitree_lidar_ros2_node
+  src/unitree_lidar_ros2_node.cpp
+)
+
+target_link_libraries(
+  unitree_lidar_ros2_node
+  ${UNITREE_LIDAR_SDK_LIB}
   ${PCL_LIBRARIES}
-  unilidar_sdk2
 )
 
 ament_target_dependencies(
   unitree_lidar_ros2_node
-  rclcpp std_msgs 
+  rclcpp
+  std_msgs
   sensor_msgs
-  geometry_msgs 
+  geometry_msgs
   tf2_ros
   pcl_conversions
 )
 
 install(TARGETS
-unitree_lidar_ros2_node
+  unitree_lidar_ros2_node
   DESTINATION lib/${PROJECT_NAME}
 )
 
-# install rviz file and launch file
 install(DIRECTORY launch/
   DESTINATION share/${PROJECT_NAME}/launch
 )
@@ -68,7 +78,6 @@ install(DIRECTORY rviz/
   DESTINATION share/${PROJECT_NAME}/rviz
 )
 
-# install config files if they exist
 if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/config")
   install(DIRECTORY config/
     DESTINATION share/${PROJECT_NAME}/config

unitree_lidar_ros2/launch/launch.py

@@ -9,12 +9,12 @@ from ament_index_python.packages import get_package_share_directory
 
 def generate_launch_description():
     
-    use_rviz = LaunchConfiguration('use_rviz')
+    use_l2_rviz = LaunchConfiguration('use_rviz')
 
-    declare_use_rviz = DeclareLaunchArgument(
-        'use_rviz',
+    declare_rviz_arg = DeclareLaunchArgument(
+        'use_l2_rviz',
         default_value='false',
-        description='Whether to start RViz'
+        description='Whether to launch RViz for Unitree L2 LiDAR visualization'
     )
 
     pkg_share = get_package_share_directory('unitree_lidar_ros2')
@@ -57,12 +57,12 @@ def generate_launch_description():
        executable='rviz2',
        name='rviz2',
        arguments=['-d', rviz_config_file],
-       condition=IfCondition(use_rviz),
+       condition=IfCondition(use_l2_rviz),
        output='log'
     )
     return LaunchDescription(
         [
-            declare_use_rviz,
+            declare_rviz_arg,
             node1, 
             rviz_node,
         ]