[LIVOX MID 360][CALIBRATION]: Adding calibration and filter to livox lidar MID 360

CMakeLists.txt

@@ -37,6 +37,10 @@ find_package(ament_cmake_auto REQUIRED)
 ament_auto_find_build_dependencies()
 find_package(PCL REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 
@@ -53,7 +57,7 @@ set(LIVOX_INTERFACES livox_interfaces2)
 rosidl_generate_interfaces(${LIVOX_INTERFACES}
   "msg/CustomPoint.msg"
   "msg/CustomMsg.msg"
-  DEPENDENCIES builtin_interfaces std_msgs
+  DEPENDENCIES builtin_interfaces std_msgs 
   LIBRARY_NAME ${PROJECT_NAME}
 )
 
@@ -134,6 +138,17 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
+set(dependencies
+  tf2
+  tf2_geometry_msgs
+  geometry_msgs
+  std_srvs
+)
+
+ament_target_dependencies(${PROJECT_NAME}
+  ${dependencies}
+)
+
 ament_auto_package(INSTALL_TO_SHARE
   config
   launch

launch/msg_MID360_launch.py

@@ -1,22 +1,38 @@
 import os
+import launch
+
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch_ros.actions import Node
-import launch
+from launch_ros.actions import Node, LoadComposableNodes
+from launch_ros.descriptions import ComposableNode
+
+from launch.actions import (
+    GroupAction,
+    TimerAction,
+)
+
+# For respawing component container
+from launch.actions import RegisterEventHandler
+from launch.launch_context import LaunchContext
+from launch.events.process.process_exited import ProcessExited
+from launch.event_handlers.on_process_exit import OnProcessExit
 
 ################### 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 = "livox_frame"
+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"
 user_config_path = os.path.join(cur_config_path, "MID360_config.json")
+
+container_name = "livox_360_container"
+
 ################### user configure parameters for ros2 end #####################
 
 livox_ros2_params = [
@@ -33,19 +49,89 @@
 
 
 def generate_launch_description():
-    livox_driver = Node(
-        package="livox_ros_driver2",
-        executable="livox_ros_driver2_node",
-        name="livox_lidar_publisher",
-        output="screen",
-        respawn=True,
-        respawn_delay=5.0,
-        parameters=livox_ros2_params,
+    # livox_driver = Node(
+    #     package="livox_ros_driver2",
+    #     executable="livox_ros_driver2_node",
+    #     name="livox_lidar_publisher",
+    #     output="screen",
+    #     respawn=True,
+    #     respawn_delay=5.0,
+    #     parameters=livox_ros2_params,
+    # )
+    def livox360_composed_launch():
+        return GroupAction(
+            actions=[
+                Node(
+                    name=container_name,
+                    package="rclcpp_components",
+                    executable="component_container_isolated",
+                    output="both",
+                ),
+                TimerAction(
+                    period=5.0,
+                    actions=[
+                        LoadComposableNodes(
+                            target_container=container_name,
+                            composable_node_descriptions=[
+                                ComposableNode(
+                                    package="pcl_ros",
+                                    plugin="pcl_ros::CropBox",
+                                    name="pcl_box_filter",
+                                    remappings=[
+                                        ("input", "/livox/lidar"),
+                                        ("output", "/livox/filtered"),
+                                    ],
+                                    parameters=[
+                                        {
+                                            "input_frame": "laser_link",
+                                            "output_frame": "laser_link",
+                                            "min_x": -0.5,
+                                            "max_x": 0.0,
+                                            "min_y": -0.3,
+                                            "max_y": 0.3,
+                                            "min_z": -1.0,
+                                            "max_z": 1.4,
+                                            "keep_organized": False,
+                                            "negative": True,
+                                        }
+                                    ],
+                                ),
+                                ComposableNode(
+                                    package="livox_ros_driver2",
+                                    plugin="livox_ros::DriverNode",
+                                    name="livox_lidar_publisher",
+                                    parameters=livox_ros2_params,
+                                ),
+                            ],
+                        ),
+                    ],
+                ),
+            ],
+        )
+
+    def relaunch_livox360_component(event: ProcessExited, context: LaunchContext):
+        if (
+            event.returncode != 0
+            and "component_container_isolated" in event.action.name
+            and container_name in event.cmd[-1]
+        ):
+            print(
+                "\n\nProcess [{}] exited, pid: {}, return code: {}\n\n".format(
+                    event.action.name, event.pid, event.returncode
+                )
+            )
+            print(f"respawning: {event.cmd[-1].split('=')[-1]}...")
+            return livox360_composed_launch()  # respawn node action
+
+    respawn_livox360_composition_event_handler = RegisterEventHandler(
+        event_handler=OnProcessExit(on_exit=relaunch_livox360_component)
     )
 
     return LaunchDescription(
         [
-            livox_driver,
+            # livox_driver,
+            livox360_composed_launch(),
+            respawn_livox360_composition_event_handler,
             # launch.actions.RegisterEventHandler(
             #     event_handler=launch.event_handlers.OnProcessExit(
             #         target_action=livox_rviz,

package.xml

@@ -19,6 +19,10 @@
   <depend>pcl_conversions</depend>
   <depend>rcl_interfaces</depend>
   <depend>libpcl-all-dev</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>std_srvs</depend>
 
   <exec_depend>rosbag2</exec_depend>
   <exec_depend>rosidl_default_runtime</exec_depend>

src/lddc.cpp

@@ -31,6 +31,7 @@
 #include <iomanip>
 #include <math.h>
 #include <stdint.h>
+#include <numeric>
 
 #include "include/ros_headers.h"
 
@@ -148,6 +149,11 @@ void Lddc::DistributeImuData(void) {
   }
 
   lds_->imu_semaphore_.Wait();
+
+  #ifdef BUILDING_ROS2
+    Service<TriggerSrv>::SharedPtr calibration_srv_ptr = std::dynamic_pointer_cast<Service<TriggerSrv>>(GetCurrentCalibService());
+  #endif
+
   for (uint32_t i = 0; i < lds_->lidar_count_; i++) {
     uint32_t lidar_id = i;
     LidarDevice *lidar = &lds_->lidars_[lidar_id];
@@ -495,6 +501,71 @@ void Lddc::InitImuMsg(const ImuData& imu_data, ImuMsg& imu_msg, uint64_t& timest
   imu_msg.linear_acceleration.z = imu_data.acc_z;
 }
 
+void Lddc::StoreAccelerationValues(ImuMsg& imu_msg) {
+
+  _imu_accel_x_vector.push_back(imu_msg.linear_acceleration.x);
+  _imu_accel_y_vector.push_back(imu_msg.linear_acceleration.y);
+  _imu_accel_z_vector.push_back(imu_msg.linear_acceleration.z);
+
+  if (_imu_accel_x_vector.size() > 30)
+  {
+    quaternion_vector_filled = true;
+
+  _imu_accel_x_vector.pop_front();
+  _imu_accel_y_vector.pop_front();
+  _imu_accel_z_vector.pop_front();
+  }
+}
+
+bool Lddc::CalibrateLivoxCb(std_srvs::srv::Trigger::Request::SharedPtr req, std_srvs::srv::Trigger::Response::SharedPtr res){
+
+  if(quaternion_vector_filled){
+
+    #ifdef BUILDING_ROS1
+      PublisherPtr publisher_ptr = GetCurrentQuaternionPublisher();
+    #elif defined BUILDING_ROS2
+      Publisher<QuaternionMsg>::SharedPtr publisher_ptr = std::dynamic_pointer_cast<Publisher<QuaternionMsg>>(GetCurrentQuaternionPublisher());
+    #endif
+
+    double accel_x = std::accumulate(_imu_accel_x_vector.begin(), _imu_accel_x_vector.end(), 0.0) / _imu_accel_x_vector.size();
+    double accel_y = std::accumulate(_imu_accel_y_vector.begin(), _imu_accel_y_vector.end(), 0.0) / _imu_accel_y_vector.size();
+    double accel_z = std::accumulate(_imu_accel_z_vector.begin(), _imu_accel_z_vector.end(), 0.0) / _imu_accel_z_vector.size();
+
+    double livox_pitch = atan2((-accel_x), sqrt(accel_y * accel_y + accel_z * accel_z));
+    double livox_roll =  atan2(accel_y, sqrt(accel_x * accel_x + accel_z * accel_z)); 
+
+    std::cout << "ROLL: " << livox_roll*57.2958 << std::endl;
+    std::cout << "PITCH: " << livox_pitch*57.2958 << std::endl;
+    double livox__yaw = 0.0;
+
+    tf2::Quaternion _Quaternion;
+    _Quaternion.setRPY(livox_roll, livox_pitch, livox__yaw);
+    geometry_msgs::msg::Quaternion Quaternion_msg;
+    Quaternion_msg = tf2::toMsg(_Quaternion);
+      if (kOutputToRos == output_type_) {
+        publisher_ptr->publish(Quaternion_msg);
+      } else {
+    #ifdef BUILDING_ROS1
+        if (bag_ && enable_lidar_bag_) {
+          bag_->write(publisher_ptr->getTopic(), ros::Time(timestamp / 1000000000.0), cloud);
+        }
+    #endif
+    }
+
+    _imu_accel_x_vector.clear();
+    _imu_accel_y_vector.clear();
+    _imu_accel_z_vector.clear();
+
+    quaternion_vector_filled=false;
+
+    return true;
+  }
+  else{
+    return false;
+  }
+}
+
+
 void Lddc::PublishImuData(LidarImuDataQueue& imu_data_queue, const uint8_t index) {
   ImuData imu_data;
   if (!imu_data_queue.Pop(imu_data)) {
@@ -505,7 +576,7 @@ void Lddc::PublishImuData(LidarImuDataQueue& imu_data_queue, const uint8_t index
   ImuMsg imu_msg;
   uint64_t timestamp;
   InitImuMsg(imu_data, imu_msg, timestamp);
-
+  StoreAccelerationValues(imu_msg);
 #ifdef BUILDING_ROS1
   PublisherPtr publisher_ptr = GetCurrentImuPublisher(index);
 #elif defined BUILDING_ROS2
@@ -547,11 +618,29 @@ std::shared_ptr<rclcpp::PublisherBase> Lddc::CreatePublisher(uint8_t msg_type,
           "%s publish use imu format", topic_name.c_str());
       return cur_node_->create_publisher<ImuMsg>(topic_name,
           queue_size);
+    }
+    else if (kLivoxQuaternionMsg == msg_type)  {
+      rclcpp::PublisherOptionsWithAllocator<std::allocator<void>> options;
+      options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable;
+      DRIVER_INFO(*cur_node_,
+          "%s publish use quaternion format", topic_name.c_str());
+      return cur_node_->create_publisher<QuaternionMsg>(topic_name,rclcpp::QoS(1).keep_all().transient_local().reliable(), options);
     } else {
       PublisherPtr null_publisher(nullptr);
       return null_publisher;
     }
 }
+std::shared_ptr<rclcpp::ServiceBase> Lddc::CreateService(uint8_t service_type,
+    std::string &service_name) {
+    if (kCalibrationSrv == service_type) {
+      DRIVER_INFO(*cur_node_,
+          "Using a service trigger service for calibration");
+      return cur_node_->create_service<TriggerSrv>(service_name,std::bind(&Lddc::CalibrateLivoxCb, this, std::placeholders::_1, std::placeholders::_2) );
+    } else {
+      ServicePtr null_service(nullptr);
+      return null_service;
+    }
+}
 #endif
 
 #ifdef BUILDING_ROS1
@@ -688,6 +777,26 @@ std::shared_ptr<rclcpp::PublisherBase> Lddc::GetCurrentImuPublisher(uint8_t hand
     return global_imu_pub_;
   }
 }
+
+std::shared_ptr<rclcpp::PublisherBase> Lddc::GetCurrentQuaternionPublisher() {
+  uint32_t queue_size = kMinEthPacketQueueSize;
+  if (!quaternion_imu_pub_) {
+    std::string topic_name("livox/angles");
+    queue_size = queue_size * 8; // shared queue size is 256, for all lidars
+    quaternion_imu_pub_ = CreatePublisher(kLivoxQuaternionMsg, topic_name, queue_size);
+  }
+  return quaternion_imu_pub_;
+}
+
+std::shared_ptr<rclcpp::ServiceBase> Lddc::GetCurrentCalibService() {
+  if (!calib_service_) {
+    std::string service_name("livox/calibration");
+    calib_service_ = CreateService(kCalibrationSrv, service_name);
+  }
+
+  return calib_service_;
+}
+
 #endif
 
 void Lddc::CreateBagFile(const std::string &file_name) {