V0.1

hinge_car_decision_nav_task_sensor/src/hinge_car_decision/CMakeLists.txt

@@ -0,0 +1,83 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(hinge_car_decision)
+
+set(JSON_INCLUDE_DIRS "/home/nvidia/sensor_ros_2/src/json/include")
+# set(HTTP_INCLUDE_DIRS "/home/user/Downloads/cpp-httplib-master")
+set(JSON_LIBRARIES "/home/nvidia/sensor_ros_2/src/json/include/nlohmann")
+
+set(PahoMqttC_LIBRARIES "/home/nvidia/sensor_ros_2/src/paho.mqtt.c-master/build/src/libpaho-mqtt3c.so")
+set(PahoMqttC_INCLUDE_DIRS "/home/nvidia/sensor_ros_2/src/paho.mqtt.c-master/src")
+
+find_package(catkin REQUIRED COMPONENTS
+  geometry_msgs
+  message_generation
+  roscpp
+  std_msgs
+  actionlib
+  actionlib_msgs
+  hinge_car_nav_msgs
+)
+
+# add_message_files(
+#   FILES
+#   HingeCarPath.msg
+#   Segment.msg
+#   SwitchLocation.msg
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   HingeCarNav.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   geometry_msgs#   
+#   std_msgs
+#   actionlib_msgs
+# )
+
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES http_connect
+#  CATKIN_DEPENDS geometry_msgs message_generation roscpp std_msgs
+#  DEPENDS system_lib
+)
+
+
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+  ${JSON_INCLUDE_DIRS}
+  ${PahoMqttC_INCLUDE_DIRS}
+)
+
+
+# # action client
+# add_executable(${PROJECT_NAME} src/hinge_car_decision_node.cpp)
+# target_link_libraries(${PROJECT_NAME}
+#   ${catkin_LIBRARIES}
+# )
+
+add_executable(${PROJECT_NAME} src/new_slag_task.cpp)
+# add_executable(${PROJECT_NAME} src/http_slag_task_node_new.cpp)
+target_link_libraries(${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+)
+
+# #action client
+# add_executable(server src/server.cpp)
+# target_link_libraries(server
+#   ${catkin_LIBRARIES}
+# )
+
+add_executable(mqtt_client src/mqtt_client.cpp)
+target_link_libraries(mqtt_client
+  ${catkin_LIBRARIES}
+  ${PahoMqttC_LIBRARIES}
+)

hinge_car_decision_nav_task_sensor/src/hinge_car_decision/package.xml

@@ -0,0 +1,78 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>hinge_car_decision</name>
+  <version>0.0.0</version>
+  <description>The hinge_car_decision package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="user@todo.todo">user</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/http_connect</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>actionlib</build_depend>
+  <build_depend>actionlib_msgs</build_depend>
+  <build_depend>hinge_car_nav_msgs</build_depend>
+  <build_export_depend>geometry_msgs</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>actionlib</build_export_depend>
+  <build_export_depend>actionlib_msgs</build_export_depend>
+  <build_export_depend>hinge_car_nav_msgs</build_export_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>actionlib</exec_depend>
+  <exec_depend>actionlib_msgs</exec_depend>
+  <exec_depend>hinge_car_nav_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

hinge_car_decision_nav_task_sensor/src/hinge_car_decision/src/hinge_car_decision_node.cpp

@@ -0,0 +1,506 @@
+#include <ros/ros.h>
+#include <httplib.h>
+#include <nlohmann/json.hpp>
+#include <thread>
+#include <chrono>
+#include <std_msgs/Int8.h>
+#include <std_msgs/Int32.h>
+#include <std_msgs/String.h>
+#include <actionlib/client/simple_action_client.h>
+#include <tf/tf.h>
+#include "geometry_msgs/PoseStamped.h"
+
+
+#include "hinge_car_nav_msgs/HingeCarPath.h"
+#include "hinge_car_nav_msgs/Segment.h"
+#include "hinge_car_nav_msgs/HingeCarNavAction.h"
+#include "hinge_car_nav_msgs/HingeCarNavActionFeedback.h"
+#include "hinge_car_nav_msgs/HingeCarNavFeedback.h"
+
+using namespace httplib;
+using json = nlohmann::json;
+
+//current_hinge_car_position_pub 发布反馈的当前车辆的位置
+ros::Publisher cancel_task_pub, op_fire_pub, op_mode_pub, duibao_pub, fangbao_pub, taibao_pub,fanbao_pub,
+              forward_max_speed_pub, back_max_speed_pub, autoenable_pub, brake_pub, current_hinge_car_posiotion_pub,
+              hinge_car_current_status_pub, pot_id_pub; 
+
+ros::Subscriber fangbao_status_feedback_sub_, taibao_status_feedback_sub_, fanbao_status_feedback_sub_, duibao_status_feedback_sub_;
+
+std_msgs::Int8 op_mode_msg, forward_max_speed_msg, back_max_speed_msg, automatic_enable_msg, brake_status_msg,open_fire_msg,
+               close_fire_msg, op_mode_remote_msg,duibao_cmd_msg,taibao_cmd_msg,fangbao_cmd_msg,fanbao_cmd_msg, hinge_car_status_msg;
+std_msgs::Int32 cancel_task_msg;
+std_msgs::String pot_id_msg;
+
+hinge_car_nav_msgs::HingeCarPath goal_path;
+hinge_car_nav_msgs::Segment seg_path;
+hinge_car_nav_msgs::HingeCarNavGoal action_client_msg;
+
+int forward_max_speed = 120;
+int back_max_speed = -120;
+int automatic_enable = 1;
+int brake_status = 1;
+int fangbao_status_feedback,fanbao_status_feedback,taibao_status_feedback,duibao_status_feedback;
+
+
+typedef actionlib::SimpleActionClient<hinge_car_nav_msgs::HingeCarNavAction> HingeCarNavClient;
+
+// HingeCarNavClient ac_("hinge_car_nav", true);
+
+void sigintHandler(int sig)
+{ 
+    ros::shutdown();
+}
+
+
+void fangbao_status_feedback_(const std_msgs::Int8::ConstPtr & msg){
+    fangbao_status_feedback = msg ->data;
+}
+
+void fanbao_status_feedback_(const std_msgs::Int8::ConstPtr & msg){
+    fanbao_status_feedback = msg ->data;
+}
+
+void taibao_status_feedback_(const std_msgs::Int8::ConstPtr & msg){
+    taibao_status_feedback = msg ->data;
+}
+
+void duibao_status_feedback_(const std_msgs::Int8::ConstPtr & msg){
+    duibao_status_feedback = msg ->data;
+}
+
+//接收反馈渣包车的位置和姿态
+void feedback_cb(const hinge_car_nav_msgs::HingeCarNavFeedbackConstPtr &feedback) {
+
+    geometry_msgs::PoseStamped pose = feedback->robot_pose;
+    current_hinge_car_posiotion_pub.publish(pose);
+
+    hinge_car_status_msg.data = 1;
+    hinge_car_current_status_pub.publish(hinge_car_status_msg);
+    ROS_INFO("feedback ROBET POSE: %f, %f", pose.pose.position.x,pose.pose.position.y);
+
+}
+
+// void loadSuccessNavtoE(double e_point_x, double e_point_y)
+// {
+//     ROS_INFO("叉包/放包完成,发送E点坐标");
+//     HingeCarNavClient ac_("hinge_car_nav", true);
+//     action_client_msg.goal_type = 5;    //设置目标类型为叉包
+//     action_client_msg.path.slagpot_pose.pose.position.x = e_point_x;
+//     action_client_msg.path.slagpot_pose.pose.position.y = e_point_y;
+
+//         // ac_.sendGoal(action_client_msg);    //发送目标轨迹
+//     ac_.sendGoal(action_client_msg, NULL, NULL, boost::bind(&feedback_cb, _1));
+
+//     ac_.waitForResult();
+
+//     if (ac_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
+//         ROS_INFO("back to E success");   
+//         hinge_car_status_msg.data = 0;
+//         hinge_car_current_status_pub.publish(hinge_car_status_msg);
+//     }  
+// }
+
+void loadSuccessNavtoE(double e_point_x, double e_point_y, double e_pose)
+{
+    ROS_INFO("叉包/放包完成,发送E点坐标");
+    HingeCarNavClient ac_("hinge_car_nav", true);
+    action_client_msg.goal_type = 5;    //设置目标类型为叉包
+    action_client_msg.path.slagpot_pose.pose.position.x = e_point_x;
+    action_client_msg.path.slagpot_pose.pose.position.y = e_point_y;
+
+    tf::Quaternion q = tf::createQuaternionFromRPY(0,0,e_pose);
+    action_client_msg.path.slagpot_pose.pose.orientation.x = q.getX();
+    action_client_msg.path.slagpot_pose.pose.orientation.y = q.getY();
+    action_client_msg.path.slagpot_pose.pose.orientation.z = q.getZ();
+    action_client_msg.path.slagpot_pose.pose.orientation.w = q.getW();
+
+        // ac_.sendGoal(action_client_msg);    //发送目标轨迹
+    ac_.sendGoal(action_client_msg, NULL, NULL, boost::bind(&feedback_cb, _1));
+
+    ac_.waitForResult();
+
+    if (ac_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
+        ROS_INFO("回到E点完成");   
+        hinge_car_status_msg.data = 0;
+        hinge_car_current_status_pub.publish(hinge_car_status_msg);
+    }  
+}
+
+
+
+// // 函数用于处理解析后的数据   json对应cmd1001版本
+void processReceivedData(const json& received_data) {
+
+    ros::NodeHandle nh;
+    cancel_task_pub = nh.advertise<std_msgs::Int32>("/task_control",1);  //取消任务
+    op_fire_pub = nh.advertise<std_msgs::Int8>("/op_fire",1);            //远程打/熄火
+    op_mode_pub = nh.advertise<std_msgs::Int8>("/op_mode",1);            //申请远程驾驶/自动驾驶
+    duibao_pub = nh.advertise<std_msgs::Int8>("/duibao_cmd",1);          //对包
+    fanbao_pub = nh.advertise<std_msgs::Int8>("/fanbao_cmd",1);          //翻包
+    fangbao_pub = nh.advertise<std_msgs::Int8>("/fangbao_cmd",1);        //放包
+    taibao_pub = nh.advertise<std_msgs::Int8>("/taibao_cmd",1);        //放包
+
+    forward_max_speed_pub = nh.advertise<std_msgs::Int8>("/forward_max_speed",1);
+    back_max_speed_pub = nh.advertise<std_msgs::Int8>("/back_max_speed",1);
+    autoenable_pub = nh.advertise<std_msgs::Int8>("/automatic_enable",1);
+    brake_pub =nh.advertise<std_msgs::Int8>("/brake_status",1);
+
+    pot_id_pub = nh.advertise <std_msgs::String>("/pot_id",1);  //渣灌的id
+    current_hinge_car_posiotion_pub = nh.advertise<geometry_msgs::PoseStamped>("/current_feedback_pose",1);  //反馈出当前车的位置，供mqtt使用
+    hinge_car_current_status_pub = nh.advertise<std_msgs::Int8>("/hinge_car_current_status",1);    //反馈前量当前状态 0- 空闲
+
+    duibao_status_feedback_sub_ = nh.subscribe("/duibao_status",1,duibao_status_feedback_); 
+    fanbao_status_feedback_sub_ = nh.subscribe("/fanbao_status",1,fanbao_status_feedback_); 
+    fangbao_status_feedback_sub_ = nh.subscribe("/fangbao_status",1,fangbao_status_feedback_); 
+    taibao_status_feedback_sub_ = nh.subscribe("/taibao_status",1,taibao_status_feedback_); 
+
+    HingeCarNavClient ac_("hinge_car_nav", true);
+    ac_.waitForServer();
+    
+    //cmdid -> 1001  自动驾驶
+    if (received_data["cmdid"] == 1001){
+        auto navigation_array = received_data["param"]["navigation"];
+        auto nav_type_num = navigation_array.size();     //判断正向反向的方式有几种
+
+        for(int i =0; i<nav_type_num;i++){
+            goal_path.target_path.clear();  // 清空目标路径
+            auto nav_coordinates_num = navigation_array[i]["nav_coordinates"].size();  // 正向/反向方式中，有多少组点
+            auto nav_type = navigation_array[i]["type"];   //正向还是反向；
+         
+            if (nav_type=="forward"){
+                hinge_car_status_msg.data = 1; //发布车辆状态
+                hinge_car_current_status_pub.publish(hinge_car_status_msg);
+
+                action_client_msg.goal_type=1;
+                float str_max_vel = 1.0;  //直线
+                float turn_max_vel = 0.4;  //转弯
+  
+                auto navCoordinates = navigation_array[i]["nav_coordinates"]; 
+
+                for(int j=0; j<nav_coordinates_num;j++){
+                    int coordinates_num = navCoordinates[j].size(); // 每组点，坐标是几位，4-直线  6-转弯
+                    
+                    if (coordinates_num == 4){
+                        seg_path.start_point.x = navCoordinates[j]["start_point_x"];
+                        seg_path.start_point.y = navCoordinates[j]["start_point_y"];
+                        seg_path.end_point.x = navCoordinates[j]["end_point_x"];
+                        seg_path.end_point.y = navCoordinates[j]["end_point_y"];
+                        seg_path.center_point.x = 0;
+                        seg_path.center_point.y = 0;
+                        seg_path.max_vel = str_max_vel;
+                        seg_path.segment_type = 1;   //直线
+                    }
+                    else if (coordinates_num == 6){
+                        seg_path.start_point.x = navCoordinates[j]["start_point_x"];
+                        seg_path.start_point.y = navCoordinates[j]["start_point_y"];
+                        seg_path.end_point.x = navCoordinates[j]["end_point_x"];
+                        seg_path.end_point.y = navCoordinates[j]["end_point_y"];
+                        seg_path.center_point.x = navCoordinates[j]["center_point_x"];
+                        seg_path.center_point.y = navCoordinates[j]["center_point_y"];
+                        seg_path.max_vel = turn_max_vel;
+                        seg_path.segment_type = 2;   //圆弧 
+                    }
+                    goal_path.target_path.push_back(seg_path);  
+                } 
+                action_client_msg.path= goal_path;
+
+                // ac_.sendGoal(action_client_msg);    //发送目标轨迹
+                ac_.sendGoal(action_client_msg, NULL, NULL, boost::bind(&feedback_cb, _1));
+                ROS_INFO("发送前向运行轨迹");
+
+                ac_.waitForResult();
+        
+                if (ac_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
+                    ROS_INFO("前向自动驾驶完成");   
+                    hinge_car_status_msg.data = 0;
+                    hinge_car_current_status_pub.publish(hinge_car_status_msg);         
+                }  
+            }
+
+            else if (nav_type=="backward"){
+                hinge_car_status_msg.data = 1; //发布车辆状态
+                hinge_car_current_status_pub.publish(hinge_car_status_msg);
+
+                action_client_msg.goal_type=2;
+                float max_vel = -0.38;  //直线
+                // auto navCoordinates = received_data["param"]["navigation"]["nav_coordinates"]; 
+                auto navCoordinates = navigation_array[i]["nav_coordinates"]; 
+
+                for(int j=0; j<nav_coordinates_num;j++){
+                    int coordinates_num = navCoordinates[j].size(); // 每组点，坐标是几位，4-直线  6-转弯
+                    
+                    if (coordinates_num == 4){  
+                        seg_path.start_point.x = navCoordinates[j]["start_point_x"];
+                        seg_path.start_point.y = navCoordinates[j]["start_point_y"];
+                        seg_path.end_point.x = navCoordinates[j]["end_point_x"];
+                        seg_path.end_point.y = navCoordinates[j]["end_point_y"];
+                        seg_path.center_point.x = 0;
+                        seg_path.center_point.y = 0;
+                        seg_path.max_vel = max_vel;
+                        seg_path.segment_type = 1;   //直线
+
+                    }
+                    else if (coordinates_num == 6){
+                        seg_path.start_point.x = navCoordinates[j]["start_point_x"];
+                        seg_path.start_point.y = navCoordinates[j]["start_point_y"];
+                        seg_path.end_point.x = navCoordinates[j]["end_point_x"];
+                        seg_path.end_point.y = navCoordinates[j]["end_point_y"];
+                        seg_path.center_point.x = navCoordinates[j]["center_point_x"];
+                        seg_path.center_point.y = navCoordinates[j]["center_point_y"];
+                        seg_path.max_vel = max_vel;
+                        seg_path.segment_type = 2;   //圆弧
+                    }
+                    goal_path.target_path.push_back(seg_path);  
+                } 
+
+                action_client_msg.path= goal_path;
+
+                // ac_.sendGoal(action_client_msg);    //发送目标轨迹
+                ac_.sendGoal(action_client_msg, NULL, NULL, boost::bind(&feedback_cb, _1));
+                ROS_INFO("发送后向运动轨迹");
+
+                ac_.waitForResult();
+
+                if (ac_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
+                    ROS_INFO("后向运动完成"); 
+                    hinge_car_status_msg.data = 0;
+                    hinge_car_current_status_pub.publish(hinge_car_status_msg);           
+                }  
+            }         
+        }
+    }
+
+    //cmdid -> 1002  中止任务  std_msgs/Int32   /task_control   ->1  任务取消
+    else if(received_data["cmdid"] == 1002){
+        // std_msgs::Int32 cancel_task_msg;
+        cancel_task_msg.data = 1;
+        cancel_task_pub.publish(cancel_task_msg);
+    }
+
+    //远程打火
+    else if(received_data["cmdid"] == 1003){
+        // std_msgs::Int8 open_fire_msg;
+        open_fire_msg.data = 1;
+        op_fire_pub.publish(open_fire_msg);
+    }
+
+        //远程熄火
+    else if(received_data["cmdid"] == 1004){
+        // std_msgs::Int8 close_fire_msg;
+        close_fire_msg.data = 2;
+        op_fire_pub.publish(close_fire_msg);
+    }
+    //远程驾驶权限
+    else if(received_data["cmdid"] == 1005){
+        // std_msgs::Int8 op_mode_remote_msg;
+        op_mode_remote_msg.data = 1;
+        op_mode_pub.publish(op_mode_remote_msg);
+    }
+    
+    //无人驾驶权限，并使能
+    else if(received_data["cmdid"] == 1006){
+        // std_msgs::Int8 op_mode_msg, forward_max_speed_msg, back_max_speed_msg, automatic_enable_msg, brake_status_msg;
+        op_mode_msg.data = 2;
+        op_mode_pub.publish(op_mode_msg);
+        std::this_thread::sleep_for(std::chrono::milliseconds(100));  
+        forward_max_speed_msg.data = forward_max_speed;
+        back_max_speed_msg.data = back_max_speed;
+        automatic_enable_msg.data = automatic_enable;
+        brake_status_msg.data = brake_status;
+
+        forward_max_speed_pub.publish(forward_max_speed_msg);
+        back_max_speed_pub.publish(back_max_speed_msg);
+        autoenable_pub.publish(automatic_enable_msg);
+        brake_pub.publish(brake_status_msg);
+    }
+    //自动作业任务
+    else if(received_data["cmdid"] == 100101){
+        std::cout << "new  start" << std::endl;
+        auto task_type = received_data["param"]["task_type"];
+        std:: cout << "task_type:" << task_type << std::endl;
+
+        //叉包 -102   叉包--需要先进行对包操作，对包操作完成后，再发渣灌位姿，车辆控制完成后，进行抬包操作
+        if (task_type==102){
+
+            hinge_car_status_msg.data = 1; //发布车辆状态
+            hinge_car_current_status_pub.publish(hinge_car_status_msg);
+
+            auto task_coordinates = received_data["param"]["task_coordinates"];
+            duibao_cmd_msg.data =1;   
+            duibao_pub.publish(duibao_cmd_msg);
+
+            while (true) {
+               if (duibao_status_feedback == 3){
+                    break;
+                }
+            }
+            duibao_status_feedback =0; // 对包完成 赋值  0
+            ROS_INFO("---对包完成  ->  发送叉包轨迹---");
+            action_client_msg.goal_type = 3;    //设置目标类型为叉包
+            action_client_msg.path.slagpot_pose.pose.position.x = task_coordinates["pot_x"];
+            action_client_msg.path.slagpot_pose.pose.position.y = task_coordinates["pot_y"];
+            tf::Quaternion q = tf::createQuaternionFromRPY(0, 0,task_coordinates["mark_pot_pose"] );
+            action_client_msg.path.slagpot_pose.pose.orientation.x = q.getX();
+            action_client_msg.path.slagpot_pose.pose.orientation.y = q.getY();
+            action_client_msg.path.slagpot_pose.pose.orientation.z = q.getZ();
+            action_client_msg.path.slagpot_pose.pose.orientation.w = q.getW();
+
+            ac_.sendGoal(action_client_msg, NULL, NULL, boost::bind(&feedback_cb, _1));
+            ROS_INFO("---已经发送叉包轨迹，等待车辆控制---");
+
+            ac_.waitForResult();
+            //车辆控制完成，即开始进行抬包动作
+            if (ac_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
+                ROS_INFO("叉包车辆控制完成 -> 进行抬包动作");  
+                taibao_cmd_msg.data =1;
+                taibao_pub.publish(taibao_cmd_msg);
+                while (true){
+                    if (taibao_status_feedback == 3){
+                        ROS_INFO("抬包动作完成 -> 回到E点");
+                        loadSuccessNavtoE(task_coordinates["e_point_x"],task_coordinates["e_point_y"],task_coordinates["e_pose"]);      //叉包/放包成功后才执行发送返回E点
+                        break;
+                    }
+                }
+                taibao_status_feedback =0;
+                std::cout << "叉包完成" << std::endl;
+            }  
+        }
+
+        //放包-103   放包时候，需要记录pot_id   放包--需要发放包位姿，车辆控制完成后，进行放包操作，放包完成后发送E点坐标
+        else if (task_type == 103){
+            auto task_coordinates = received_data["param"]["task_coordinates"];
+            // auto pot_id = task_coordinates["pot_name"];
+            std::string pot_id = task_coordinates["pot_name"];
+
+            pot_id_msg.data = pot_id;
+            pot_id_pub.publish(pot_id_msg);
+
+            hinge_car_status_msg.data = 1; //发布车辆状态
+            hinge_car_current_status_pub.publish(hinge_car_status_msg);
+
+            action_client_msg.goal_type = 4;    //设置目标类型为放包
+            action_client_msg.path.slagpot_pose.pose.position.x = task_coordinates["pot_x"];
+            action_client_msg.path.slagpot_pose.pose.position.y = task_coordinates["pot_y"];
+            tf::Quaternion q = tf::createQuaternionFromRPY(0, 0,task_coordinates["mark_pot_pose"] );  //注意角度为弧度，需要对应
+            action_client_msg.path.slagpot_pose.pose.orientation.x = q.getX();
+            action_client_msg.path.slagpot_pose.pose.orientation.y = q.getY();
+            action_client_msg.path.slagpot_pose.pose.orientation.z = q.getZ();
+            action_client_msg.path.slagpot_pose.pose.orientation.w = q.getW();
+
+                // ac_.sendGoal(action_client_msg);    //发送目标轨迹
+            ac_.sendGoal(action_client_msg, NULL, NULL, boost::bind(&feedback_cb, _1));
+            ROS_INFO("fangbao -- send fangbao pot position");
+
+            ac_.waitForResult();
+
+            if (ac_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
+                ROS_INFO("success");  
+                fangbao_cmd_msg.data = 1;
+                fangbao_pub.publish(fangbao_cmd_msg);
+
+                while(true){
+                    ROS_INFO("FANGBAO_STATUS:%d",fangbao_status_feedback);
+                    if(fangbao_status_feedback == 3){
+                        // loadSuccessNavtoE(task_coordinates["e_point_x"],task_coordinates["e_point_y"]); //叉包/放包成功后才执行发送返回E点
+                        loadSuccessNavtoE(task_coordinates["e_point_x"],task_coordinates["e_point_y"],task_coordinates["e_pose"]); //叉包/放包成功后才执行发送返回E点
+                        break;
+                    }
+                    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+                }
+                fangbao_status_feedback = 0;
+                std::cout << "放包完成" << std::endl;
+                
+            }  
+        }
+        //倒渣
+        else if (task_type == 104){
+            std::cout << "进入104 "<< std::endl;
+            fanbao_cmd_msg.data = 1;
+            fanbao_pub.publish(fanbao_cmd_msg);
+
+            std::this_thread::sleep_for(std::chrono::milliseconds(10));
+            while (true) {
+                if (fanbao_status_feedback == 3) {
+
+                    ROS_INFO("daozha success");
+                    break;  // 跳出循环
+                }
+                // 休眠时间
+                std::this_thread::sleep_for(std::chrono::milliseconds(10));  // 延时10毫秒
+            }
+            std::this_thread::sleep_for(std::chrono::milliseconds(10));
+            hinge_car_status_msg.data = 1;
+            hinge_car_current_status_pub.publish(hinge_car_status_msg);
+            std::cout << "倒渣完成"<< std::endl;
+            // std::cout <<"hinge status " <<hinge_car_status_msg<<std::endl;
+            fanbao_status_feedback =0;
+        }  
+
+    }
+}
+
+
+
+
+// 函数用于接收HTTP请求并解析数据
+void handleHttpRequest(const Request& req, Response& res) {
+    auto received_data = json::parse(req.body);
+    std::cout << "Received data: " << received_data["cmdid"] << "\n";
+    // std::cout << "Received data: " << received_data.dump(4) << "\n";
+
+    // 在新线程中运行处理数据的函数
+    std::thread process_thread(processReceivedData, received_data);
+
+    // 分离线程，使其在后台运行l
+    process_thread.detach();
+
+}
+
+
+
+int main(int argc, char** argv) {
+
+    setlocale(LC_CTYPE, "zh_CN.utf8");
+
+    ros::init(argc, argv, "decision_node");
+    // ros::NodeHandle nh;
+
+    // cancel_task_pub = nh.advertise<std_msgs::Int32>("/task_control",1);  //取消任务
+    // op_fire_pub = nh.advertise<std_msgs::Int8>("/op_fire",1);            //远程打/熄火
+    // op_mode_pub = nh.advertise<std_msgs::Int8>("/op_mode",1);            //申请远程驾驶/自动驾驶
+    // duibao_pub = nh.advertise<std_msgs::Int8>("/duibao_cmd",1);          //对包
+    // fanbao_pub = nh.advertise<std_msgs::Int8>("/fanbao_cmd",1);          //翻包
+    // fangbao_pub = nh.advertise<std_msgs::Int8>("/fangbao_cmd",1);        //放包
+
+    // forward_max_speed_pub = nh.advertise<std_msgs::Int8>("/forward_max_speed",1);
+    // back_max_speed_pub = nh.advertise<std_msgs::Int8>("/back_max_speed",1);
+    // autoenable_pub = nh.advertise<std_msgs::Int8>("/automatic_enable",1);
+    // brake_pub =nh.advertise<std_msgs::Int8>("/brake_status",1);
+
+    // current_hinge_car_posiotion_pub = nh.advertise<geometry_msgs::PoseStamped>("/current_feedback_pose",1);  //反馈出当前车的位置，供mqtt使用
+    // hinge_car_current_status_pub = nh.advertise<std_msgs::Int8>("/hinge_car_current_status",1);    //反馈前量当前状态 0- 空闲
+
+
+    // duibao_status_feedback_sub_ = nh.subscribe("/duibao_status",1,duibao_status_feedback_); 
+    // fanbao_status_feedback_sub_ = nh.subscribe("/fanbao_status",1,fanbao_status_feedback_); 
+    // fangbao_status_feedback_sub_ = nh.subscribe("/fangbao_status",1,fangbao_status_feedback_); 
+    // taibao_status_feedback_sub_ = nh.subscribe("/taibao_status",1,taibao_status_feedback_); 
+
+
+    signal(SIGINT, sigintHandler);
+    Server svr;
+    svr.Post("/", handleHttpRequest);
+
+    std::thread serverThread([&]() {
+        svr.listen("192.168.191.91", 8000);
+    });
+
+    ros::spin();
+    
+    svr.stop();
+
+    serverThread.join();
+
+    return 0;
+}

hinge_car_decision_nav_task_sensor/src/hinge_car_decision/src/http_node.cpp

@@ -0,0 +1,107 @@
+#include <ros/ros.h>
+#include <httplib.h>
+#include <nlohmann/json.hpp>
+#include <thread>
+#include <std_msgs/Int8.h>
+#include "http_connect/HingeCarPath.h"
+#include "http_connect/Segment.h"
+
+using namespace httplib;
+using json = nlohmann::json;
+
+ros::Publisher target_path_nav_pub, target_point_load_pub, mission_pub,nav_type_pub;
+
+http_connect::HingeCarPath goal_path;
+http_connect::Segment seg_path;
+
+std_msgs::Int8 mission_msg, nav_type_msg;
+
+
+void handleHttpRequest(const Request& req, Response& res) {
+    auto received_data = json::parse(req.body);
+    std::cout << "Received data: " << received_data.dump(4) << "\n";
+    std::cout << "Received data: " << received_data["cmdid"] << "\n";
+
+    if (received_data["cmdid"] == 1001 && received_data["nav_type"] == 1) {
+        mission_msg.data = 1;
+        // std::cout << received_data["param"]["nav_type"]<<"99" <<std::endl;;
+        if (received_data["param"]["nav_type"]==1){
+            nav_type_msg.data= 1;
+        }
+        else{
+            nav_type_msg.data= 2;
+
+        }
+        
+        auto navCoordinates = received_data["param"]["nav_coordinates"];
+        goal_path.target_path.clear();  // 清空目标路径
+        if (navCoordinates.is_array()) {
+            int size = navCoordinates.size();
+            std::cout << "Size of nav_coordinates: " << size << std::endl;
+        }
+
+        goal_path.size = navCoordinates.size();
+
+        for (int i = 0; i < navCoordinates.size(); i++) {
+            ROS_INFO("1");
+            int numKeys = navCoordinates[i].size();
+            
+            if (numKeys == 4){
+                ROS_INFO("11");
+                seg_path.start_point.x = navCoordinates[i]["start_point_x"];
+                seg_path.start_point.y = navCoordinates[i]["start_point_y"];
+                seg_path.end_point.x = navCoordinates[i]["end_point_x"];
+                seg_path.end_point.y = navCoordinates[i]["end_point_y"];
+                seg_path.max_vel = 1.0;
+                // seg_path.segment_type = http_connect::Segment::LINE;   //直线还是圆弧
+                seg_path.segment_type = 1;   //直线还是圆弧
+
+            }
+            else if (numKeys == 6){
+                seg_path.start_point.x = navCoordinates[i]["start_point_x"];
+                seg_path.start_point.y = navCoordinates[i]["start_point_y"];
+                seg_path.end_point.x = navCoordinates[i]["end_point_x"];
+                seg_path.end_point.y = navCoordinates[i]["end_point_y"];
+                seg_path.center_point.x = navCoordinates[i]["center_point_x"];
+                seg_path.center_point.y = navCoordinates[i]["center_point_y"];
+                seg_path.max_vel = 1.0;
+                seg_path.segment_type = 2;   //直线还是圆弧
+                // seg_path.segment_type = http_connect::Segment::CIRCLE;   //直线还是圆弧
+                ROS_INFO("----");
+            }
+            goal_path.target_path.push_back(seg_path);  
+        }
+    target_path_nav_pub.publish(goal_path);
+    mission_pub.publish(mission_msg);
+    nav_type_pub.publish(nav_type_msg);
+    }
+}
+
+int main(int argc, char** argv) {
+    ros::init(argc, argv, "json_parser_node");
+    ros::NodeHandle nh;
+
+    target_path_nav_pub = nh.advertise<http_connect::HingeCarPath>("/decision_target_path",10);
+    mission_pub = nh.advertise<std_msgs::Int8>("post_mission",10);
+    nav_type_pub = nh.advertise<std_msgs::Int8>("nav_type",10);
+
+    Server svr;
+    ROS_INFO("00000000000000");
+    svr.Post("/", handleHttpRequest);
+
+    std::thread serverThread([&]() {
+        svr.listen("192.168.191.91", 8000);
+    });
+
+    ros::spin();
+
+    // 程序退出前，关闭服务器对象并释放端口
+    svr.stop();
+
+    serverThread.join();
+    
+
+    return 0;
+}
+
+

hinge_car_decision_nav_task_sensor/src/hinge_car_decision/src/mqtt_client.cpp

@@ -0,0 +1,196 @@
+#include <ros/ros.h>
+#include <std_msgs/String.h>
+#include <std_msgs/Int8.h>
+#include <stdio.h>
+#include <string.h>
+#include <ifaddrs.h>
+#include <netdb.h>
+#include <sys/socket.h>
+#include "MQTTClient.h"
+#include <nlohmann/json.hpp>
+#include "hinge_car_nav_msgs/getSlagpotPose.h"
+#include <tf/tf.h>
+
+using json = nlohmann::json;
+
+#define SERVER_ADDRESS "tcp://192.168.131.23:41883"
+#define TOPIC_VEHICLE_STATE "hs/vehicle/state"
+#define TOPIC_POT_DATA "hs/pot/data"
+
+#define CLIENT_ID ""
+
+volatile MQTTClient_deliveryToken deliveredtoken;
+
+ros::Subscriber pot_id_sub, pot_weight_sub,current_hinge_car_status_sub,op_mode_current_sub;
+
+const char* interface = "eth0"; // 选择你想要查询的网络接口，例如 "eth0" 或 "wlan0"
+double roll, pitch, yaw, robot_roll, robot_pitch, robot_yaw;
+std::string pot_name, ip_address;
+int pot_weight, rc, hinge_car_current_status,op_mode_current_status;
+json vehicle_state_json, pot_data_json;    
+hinge_car_nav_msgs::getSlagpotPose srv;    
+
+void connlost(void* context, char* cause)
+{
+    printf("\nConnection lost\n");
+    printf("     cause: %s\n", cause);
+}
+
+void publishMQTTMessage(const std::string& topic, const std::string& message)
+{
+    MQTTClient client;
+    MQTTClient_connectOptions conn_opts = MQTTClient_connectOptions_initializer;
+
+
+    MQTTClient_create(&client, SERVER_ADDRESS, CLIENT_ID, MQTTCLIENT_PERSISTENCE_NONE, NULL);
+    conn_opts.keepAliveInterval = 20;
+    conn_opts.cleansession = 1;
+    MQTTClient_setCallbacks(client, NULL, connlost, NULL, NULL);
+
+    // printf("Connecting to MQTT server...\n");
+    if ((rc = MQTTClient_connect(client, &conn_opts)) != MQTTCLIENT_SUCCESS)
+    {
+        printf("Failed to connect, return code %d\n", rc);
+        return;
+    }
+    // printf("Connected\n");
+
+    MQTTClient_message pubmsg = MQTTClient_message_initializer;
+    MQTTClient_deliveryToken token;
+    pubmsg.payload = const_cast<char*>(message.c_str());
+    pubmsg.payloadlen = message.length();
+    pubmsg.qos = 0;
+    pubmsg.retained = 0;
+    MQTTClient_publishMessage(client, topic.c_str(), &pubmsg, &token);
+    rc = MQTTClient_waitForCompletion(client, token, 10000);
+
+    // printf("Message published to topic: %s\n", topic.c_str());
+
+    MQTTClient_disconnect(client, 10000);
+    MQTTClient_destroy(&client);
+}
+
+void pot_id_callback(const std_msgs::String::ConstPtr &msg){
+    pot_name = msg ->data;
+}
+
+void pot_weight_callback(const std_msgs::Int8::ConstPtr &msg){
+    pot_weight = msg ->data;
+}
+
+void hinge_car_current_callback(const std_msgs::Int8::ConstPtr &msg){
+    hinge_car_current_status = msg -> data;
+}
+
+void op_mode_callback(const std_msgs::Int8::ConstPtr &msg){
+    op_mode_current_status = msg ->data;
+}
+
+
+std::string getIPAddress(const char* interface) 
+{
+    struct ifaddrs *ifap, *ifa;
+    char host[NI_MAXHOST] = "";
+
+    if (getifaddrs(&ifap) == -1) 
+    {
+        perror("getifaddrs");
+        exit(EXIT_FAILURE);
+    }
+
+    for (ifa = ifap; ifa != NULL; ifa = ifa->ifa_next) 
+    {
+        if (ifa->ifa_addr != NULL && ifa->ifa_name != NULL && strcmp(ifa->ifa_name, interface) == 0) 
+        {
+            int s = getnameinfo(ifa->ifa_addr, sizeof(struct sockaddr_in), host, NI_MAXHOST, NULL, 0, NI_NUMERICHOST);
+            if (s == 0) 
+            {
+                freeifaddrs(ifap);
+                return host;
+            }
+        }
+    }
+
+    freeifaddrs(ifap);
+    return "";
+}
+
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "mqtt_client_node");
+    ros::NodeHandle nh;
+
+    ip_address = getIPAddress(interface);
+
+    pot_id_sub = nh.subscribe<std_msgs::String>("/pot_id",1,pot_id_callback);                    //罐的id
+    pot_weight_sub = nh.subscribe<std_msgs::Int8>("/pot_weight",1,pot_weight_callback);          //车辆的载重
+    op_mode_current_sub = nh.subscribe<std_msgs::Int8>("/op_mode_current_status",1,op_mode_callback);          //操作模式
+
+    //  1 离线 2 在线空闲 3 人工驾驶中 4 远程驾驶中 5 自动驾驶中
+    current_hinge_car_status_sub = nh.subscribe<std_msgs::Int8>("/hinge_car_current_status",1,hinge_car_current_callback); //车辆状态                               //车辆当前状态
+
+    //Declare the service client
+    ros::ServiceClient client = nh.serviceClient<hinge_car_nav_msgs::getSlagpotPose>("get_slagpot_pose");
+
+
+    ros::Rate loop_rate(1);
+
+    while (ros::ok()) {
+        while (!client.waitForExistence(ros::Duration(1.0))) {
+            ROS_INFO("Waiting for service to be available...");
+        }
+
+        if (client.call(srv))
+        {
+            ROS_INFO("Service called successfully. Response: %f, %f", srv.response.slagpotPose.pose.position.x, srv.response.slagpotPose.pose.position.y);
+            ROS_INFO("robot: %f, %f", srv.response.robotPose.pose.position.x, srv.response.robotPose.pose.position.y);
+
+            pot_data_json["pot_name"] = pot_name;
+            pot_data_json["pot_x"] = srv.response.slagpotPose.pose.position.x;
+            pot_data_json["pot_y"] = srv.response.slagpotPose.pose.position.y;
+
+            
+            tf::Quaternion robot_q;
+            tf::quaternionMsgToTF(srv.response.robotPose.pose.orientation, robot_q);    
+            tf::Matrix3x3(robot_q).getRPY(robot_roll, robot_pitch, robot_yaw);
+        
+            tf::Quaternion slag_q;
+            tf::quaternionMsgToTF(srv.response.slagpotPose.pose.orientation, slag_q);    
+            tf::Matrix3x3(slag_q).getRPY(roll, pitch, yaw);
+            pot_data_json["mark_pot_pose"] = yaw;
+            ROS_INFO("SLAG: %f",yaw);
+
+
+            std::string pot_data = pot_data_json.dump();
+            publishMQTTMessage(TOPIC_POT_DATA, pot_data);
+        }
+        else
+        {
+            ROS_ERROR("Failed to call service");
+            return 1;
+        }
+
+        vehicle_state_json["address"] = ip_address;
+     
+        if (op_mode_current_status == 0)  //人工
+            vehicle_state_json["state"] = 3 ;
+        else if (hinge_car_current_status==1 && op_mode_current_status ==2)
+            vehicle_state_json["state"] = 5 ;   //自动驾驶中
+        else if (hinge_car_current_status==0 && op_mode_current_status ==2)
+            vehicle_state_json["state"] = 2 ;   //在线空闲
+        else if(op_mode_current_status == 1)  //远程驾驶
+            vehicle_state_json["state"] = 4 ;
+
+        vehicle_state_json["direction"] = robot_yaw;
+        vehicle_state_json ["position_x"] = srv.response.robotPose.pose.position.x ;
+        vehicle_state_json ["position_y"] = srv.response.robotPose.pose.position.y;
+        vehicle_state_json["weight"] = pot_weight;
+        std::string vehicle_state = vehicle_state_json.dump();
+        publishMQTTMessage(TOPIC_VEHICLE_STATE, vehicle_state);
+
+        ros::spinOnce();
+        loop_rate.sleep();
+    }
+
+    return 0;
+}

hinge_car_decision_nav_task_sensor/src/hinge_car_decision/src/new_slag_task.cpp

@@ -0,0 +1,505 @@
+#include <ros/ros.h>
+#include <httplib.h>
+#include <nlohmann/json.hpp>
+#include <thread>
+#include <chrono>
+#include <std_msgs/Int8.h>
+#include <std_msgs/Int32.h>
+#include <std_msgs/String.h>
+#include <actionlib/client/simple_action_client.h>
+#include <tf/tf.h>
+#include "geometry_msgs/PoseStamped.h"
+
+
+#include "hinge_car_nav_msgs/HingeCarPath.h"
+#include "hinge_car_nav_msgs/Segment.h"
+#include "hinge_car_nav_msgs/HingeCarNavAction.h"
+#include "hinge_car_nav_msgs/HingeCarNavActionFeedback.h"
+#include "hinge_car_nav_msgs/HingeCarNavFeedback.h"
+
+using namespace httplib;
+using json = nlohmann::json;
+
+//current_hinge_car_position_pub 发布反馈的当前车辆的位置
+ros::Publisher cancel_task_pub, op_fire_pub, op_mode_pub, duibao_pub, fangbao_pub, taibao_pub,fanbao_pub,
+              forward_max_speed_pub, back_max_speed_pub, autoenable_pub, brake_pub, current_hinge_car_posiotion_pub,
+              hinge_car_current_status_pub, pot_id_pub; 
+
+ros::Subscriber fangbao_status_feedback_sub_, taibao_status_feedback_sub_, fanbao_status_feedback_sub_, duibao_status_feedback_sub_;
+
+std_msgs::Int8 op_mode_msg, forward_max_speed_msg, back_max_speed_msg, automatic_enable_msg, brake_status_msg,open_fire_msg,
+               close_fire_msg, op_mode_remote_msg,duibao_cmd_msg,taibao_cmd_msg,fangbao_cmd_msg,fanbao_cmd_msg, hinge_car_status_msg;
+std_msgs::Int32 cancel_task_msg;
+std_msgs::String pot_id_msg;
+
+hinge_car_nav_msgs::HingeCarPath goal_path;
+hinge_car_nav_msgs::Segment seg_path;
+hinge_car_nav_msgs::HingeCarNavGoal action_client_msg;
+
+int forward_max_speed = 120;
+int back_max_speed = -120;
+int automatic_enable = 1;
+int brake_status = 1;
+int fangbao_status_feedback,fanbao_status_feedback,taibao_status_feedback,duibao_status_feedback;
+
+
+typedef actionlib::SimpleActionClient<hinge_car_nav_msgs::HingeCarNavAction> HingeCarNavClient;
+
+// HingeCarNavClient ac_("hinge_car_nav", true);
+
+void sigintHandler(int sig)
+{ 
+    ros::shutdown();
+}
+
+
+void fangbao_status_feedback_(const std_msgs::Int8::ConstPtr & msg){
+    fangbao_status_feedback = msg ->data;
+}
+
+void fanbao_status_feedback_(const std_msgs::Int8::ConstPtr & msg){
+    fanbao_status_feedback = msg ->data;
+}
+
+void taibao_status_feedback_(const std_msgs::Int8::ConstPtr & msg){
+    taibao_status_feedback = msg ->data;
+}
+
+void duibao_status_feedback_(const std_msgs::Int8::ConstPtr & msg){
+    duibao_status_feedback = msg ->data;
+}
+
+//接收反馈渣包车的位置和姿态
+void feedback_cb(const hinge_car_nav_msgs::HingeCarNavFeedbackConstPtr &feedback) {
+
+    geometry_msgs::PoseStamped pose = feedback->robot_pose;
+    current_hinge_car_posiotion_pub.publish(pose);
+
+    hinge_car_status_msg.data = 1;
+    hinge_car_current_status_pub.publish(hinge_car_status_msg);
+    ROS_INFO("feedback ROBET POSE: %f, %f", pose.pose.position.x,pose.pose.position.y);
+
+}
+
+void loadSuccessNavtoE(double e_point_x, double e_point_y)
+{
+    ROS_INFO("叉包/放包完成,发送E点坐标");
+    HingeCarNavClient ac_("hinge_car_nav", true);
+    action_client_msg.goal_type = 5;    //设置目标类型为叉包
+    action_client_msg.path.slagpot_pose.pose.position.x = e_point_x;
+    action_client_msg.path.slagpot_pose.pose.position.y = e_point_y;
+
+        // ac_.sendGoal(action_client_msg);    //发送目标轨迹
+    ac_.sendGoal(action_client_msg, NULL, NULL, boost::bind(&feedback_cb, _1));
+
+    ac_.waitForResult();
+
+    if (ac_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
+        ROS_INFO("back to E success");   
+        hinge_car_status_msg.data = 0;
+        hinge_car_current_status_pub.publish(hinge_car_status_msg);
+    }  
+}
+
+// // 函数用于处理解析后的数据   json对应cmd1001版本
+void processReceivedData(const json& received_data) {
+
+    ros::NodeHandle nh;
+    cancel_task_pub = nh.advertise<std_msgs::Int32>("/task_control",1);  //取消任务
+    op_fire_pub = nh.advertise<std_msgs::Int8>("/op_fire",1);            //远程打/熄火
+    op_mode_pub = nh.advertise<std_msgs::Int8>("/op_mode",1);            //申请远程驾驶/自动驾驶
+    duibao_pub = nh.advertise<std_msgs::Int8>("/duibao_cmd",1);          //对包
+    fanbao_pub = nh.advertise<std_msgs::Int8>("/fanbao_cmd",1);          //翻包
+    fangbao_pub = nh.advertise<std_msgs::Int8>("/fangbao_cmd",1);        //放包
+    taibao_pub = nh.advertise<std_msgs::Int8>("/taibao_cmd",1);        //放包
+
+    forward_max_speed_pub = nh.advertise<std_msgs::Int8>("/forward_max_speed",1);
+    back_max_speed_pub = nh.advertise<std_msgs::Int8>("/back_max_speed",1);
+    autoenable_pub = nh.advertise<std_msgs::Int8>("/automatic_enable",1);
+    brake_pub =nh.advertise<std_msgs::Int8>("/brake_status",1);
+
+    current_hinge_car_posiotion_pub = nh.advertise<geometry_msgs::PoseStamped>("/current_feedback_pose",1);  //反馈出当前车的位置，供mqtt使用
+    hinge_car_current_status_pub = nh.advertise<std_msgs::Int8>("/hinge_car_current_status",1);    //反馈前量当前状态 0- 空闲
+
+    duibao_status_feedback_sub_ = nh.subscribe("/duibao_status",1,duibao_status_feedback_); 
+    fanbao_status_feedback_sub_ = nh.subscribe("/fanbao_status",1,fanbao_status_feedback_); 
+    fangbao_status_feedback_sub_ = nh.subscribe("/fangbao_status",1,fangbao_status_feedback_); 
+    taibao_status_feedback_sub_ = nh.subscribe("/taibao_status",1,taibao_status_feedback_); 
+
+    HingeCarNavClient ac_("hinge_car_nav", true);
+    ac_.waitForServer();
+    
+    //cmdid -> 1001  自动驾驶
+    if (received_data["cmdid"] == 1001){
+        auto navigation_array = received_data["param"]["navigation"];
+        auto nav_type_num = navigation_array.size();     //判断正向反向的方式有几种
+
+        for(int i =0; i<nav_type_num;i++){
+  
+            ROS_INFO("00000");
+            goal_path.target_path.clear();  // 清空目标路径
+            auto nav_coordinates_num = navigation_array[i]["nav_coordinates"].size();  // 正向/反向方式中，有多少组点
+            auto nav_type = navigation_array[i]["type"];   //正向还是反向；
+         
+            // auto nav_coordinates_array = received_data[][]
+            if (nav_type=="forward"){
+                std:: cout << "forward"<< std::endl;
+                action_client_msg.goal_type=1;
+                float str_max_vel = 1.0;  //直线
+                float turn_max_vel = 0.4;  //转弯
+                // auto navCoordinates = received_data["param"]["navigation"]["nav_coordinates"]; 
+                auto navCoordinates = navigation_array[i]["nav_coordinates"]; 
+
+                for(int j=0; j<nav_coordinates_num;j++){
+                    int coordinates_num = navCoordinates[j].size(); // 每组点，坐标是几位，4-直线  6-转弯
+                    
+                    if (coordinates_num == 4){
+                        seg_path.start_point.x = navCoordinates[j]["start_point_x"];
+                        seg_path.start_point.y = navCoordinates[j]["start_point_y"];
+                        seg_path.end_point.x = navCoordinates[j]["end_point_x"];
+                        seg_path.end_point.y = navCoordinates[j]["end_point_y"];
+                        seg_path.center_point.x = 0;
+                        seg_path.center_point.y = 0;
+                        seg_path.max_vel = str_max_vel;
+                        seg_path.segment_type = 1;   //直线
+                        std::cout<< seg_path.start_point.x<<" " << seg_path.start_point.y << " " << seg_path.end_point.x  << " " <<  seg_path.end_point.y;
+                    }
+                    else if (coordinates_num == 6){
+                        seg_path.start_point.x = navCoordinates[j]["start_point_x"];
+                        seg_path.start_point.y = navCoordinates[j]["start_point_y"];
+                        seg_path.end_point.x = navCoordinates[j]["end_point_x"];
+                        seg_path.end_point.y = navCoordinates[j]["end_point_y"];
+                        seg_path.center_point.x = navCoordinates[j]["center_point_x"];
+                        seg_path.center_point.y = navCoordinates[j]["center_point_y"];
+                        seg_path.max_vel = turn_max_vel;
+                        seg_path.segment_type = 2;   //圆弧 
+                        std::cout<< seg_path.start_point.x<<" " << seg_path.start_point.y << " " << seg_path.end_point.x  << " " <<  seg_path.end_point.y;
+
+                    }
+                    goal_path.target_path.push_back(seg_path);  
+                } 
+                action_client_msg.path= goal_path;
+            
+                // ac_.sendGoal(action_client_msg);    //发送目标轨迹
+                ac_.sendGoal(action_client_msg, NULL, NULL, boost::bind(&feedback_cb, _1));
+                ROS_INFO("send path");
+
+                ac_.waitForResult();
+        
+                if (ac_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
+                    ROS_INFO("success--forward");   
+                    hinge_car_status_msg.data = 0;
+                    hinge_car_current_status_pub.publish(hinge_car_status_msg);         
+                }  
+            }
+
+            else if (nav_type=="backward"){
+                std:: cout << "backward"<< std::endl;
+                action_client_msg.goal_type=2;
+                float max_vel = -0.40;  //直线
+                // auto navCoordinates = received_data["param"]["navigation"]["nav_coordinates"]; 
+                auto navCoordinates = navigation_array[i]["nav_coordinates"]; 
+
+                for(int j=0; j<nav_coordinates_num;j++){
+                    int coordinates_num = navCoordinates[j].size(); // 每组点，坐标是几位，4-直线  6-转弯
+                    
+                    if (coordinates_num == 4){  
+                        seg_path.start_point.x = navCoordinates[j]["start_point_x"];
+                        seg_path.start_point.y = navCoordinates[j]["start_point_y"];
+                        seg_path.end_point.x = navCoordinates[j]["end_point_x"];
+                        seg_path.end_point.y = navCoordinates[j]["end_point_y"];
+                        seg_path.center_point.x = 0;
+                        seg_path.center_point.y = 0;
+                        seg_path.max_vel = max_vel;
+                        seg_path.segment_type = 1;   //直线
+
+                    }
+                    else if (coordinates_num == 6){
+                        seg_path.start_point.x = navCoordinates[j]["start_point_x"];
+                        seg_path.start_point.y = navCoordinates[j]["start_point_y"];
+                        seg_path.end_point.x = navCoordinates[j]["end_point_x"];
+                        seg_path.end_point.y = navCoordinates[j]["end_point_y"];
+                        seg_path.center_point.x = navCoordinates[j]["center_point_x"];
+                        seg_path.center_point.y = navCoordinates[j]["center_point_y"];
+                        seg_path.max_vel = max_vel;
+                        seg_path.segment_type = 2;   //圆弧
+                    }
+                    goal_path.target_path.push_back(seg_path);  
+                } 
+
+                action_client_msg.path= goal_path;
+
+                // ac_.sendGoal(action_client_msg);    //发送目标轨迹
+                ac_.sendGoal(action_client_msg, NULL, NULL, boost::bind(&feedback_cb, _1));
+                ROS_INFO("send path");
+
+                ac_.waitForResult();
+
+                if (ac_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
+                    ROS_INFO("success"); 
+                    hinge_car_status_msg.data = 0;
+                    hinge_car_current_status_pub.publish(hinge_car_status_msg);           
+                }  
+            }         
+        }
+    }
+
+    //cmdid -> 1002  中止任务  std_msgs/Int32   /task_control   ->1  任务取消
+    else if(received_data["cmdid"] == 1002){
+        // std_msgs::Int32 cancel_task_msg;
+        cancel_task_msg.data = 1;
+        cancel_task_pub.publish(cancel_task_msg);
+    }
+
+    //远程打火
+    else if(received_data["cmdid"] == 1003){
+        // std_msgs::Int8 open_fire_msg;
+        open_fire_msg.data = 1;
+        op_fire_pub.publish(open_fire_msg);
+    }
+
+        //远程熄火
+    else if(received_data["cmdid"] == 1004){
+        // std_msgs::Int8 close_fire_msg;
+        close_fire_msg.data = 2;
+        op_fire_pub.publish(close_fire_msg);
+    }
+    //远程驾驶权限
+    else if(received_data["cmdid"] == 1005){
+        // std_msgs::Int8 op_mode_remote_msg;
+        op_mode_remote_msg.data = 1;
+        op_mode_pub.publish(op_mode_remote_msg);
+    }
+    
+    //无人驾驶权限，并使能
+    else if(received_data["cmdid"] == 1006){
+        // std_msgs::Int8 op_mode_msg, forward_max_speed_msg, back_max_speed_msg, automatic_enable_msg, brake_status_msg;
+        op_mode_msg.data = 2;
+        op_mode_pub.publish(op_mode_msg);
+        std::this_thread::sleep_for(std::chrono::milliseconds(100));  
+        forward_max_speed_msg.data = forward_max_speed;
+        back_max_speed_msg.data = back_max_speed;
+        automatic_enable_msg.data = automatic_enable;
+        brake_status_msg.data = brake_status;
+
+        forward_max_speed_pub.publish(forward_max_speed_msg);
+        back_max_speed_pub.publish(back_max_speed_msg);
+        autoenable_pub.publish(automatic_enable_msg);
+        brake_pub.publish(brake_status_msg);
+    }
+    //自动作业任务
+    else if(received_data["cmdid"] == 100101){
+        std::cout << "new  start" << std::endl;
+        auto task_type = received_data["param"]["task_type"];
+        std:: cout << "task_type:" << task_type << std::endl;
+
+        //叉包 -102   叉包--需要先进行对包操作，对包操作完成后，再发渣灌位姿，车辆控制完成后，进行抬包操作
+        if (task_type==102){
+            std::cout << "jinru  102" << std::endl;
+            
+            auto task_coordinates = received_data["param"]["task_coordinates"];
+            std::cout << "9999" << std::endl;
+            // duibao_cmd_msg.data =1;   //dui
+            // duibao_pub.publish(duibao_cmd_msg);
+            // std::cout << " have question" << std::endl;
+            // while (true) {
+            //    if (duibao_status_feedback == 3){
+            //         break;
+            //     }
+            // }
+            // ac_.waitForResult();
+
+            // duibao_status_feedback =0; // 对包完成 赋值  0
+            ROS_INFO("duibao success  -> 发送叉包轨迹");
+            action_client_msg.goal_type = 3;    //设置目标类型为叉包
+            action_client_msg.path.slagpot_pose.pose.position.x = task_coordinates["pot_x"];
+            action_client_msg.path.slagpot_pose.pose.position.y = task_coordinates["pot_y"];
+            tf::Quaternion q = tf::createQuaternionFromRPY(0, 0,task_coordinates["mark_pot_pose"] );
+            action_client_msg.path.slagpot_pose.pose.orientation.x = q.getX();
+            action_client_msg.path.slagpot_pose.pose.orientation.y = q.getY();
+            action_client_msg.path.slagpot_pose.pose.orientation.z = q.getZ();
+            action_client_msg.path.slagpot_pose.pose.orientation.w = q.getW();
+
+            // ac_.sendGoal(action_client_msg);    //发送目标轨迹
+            ac_.sendGoal(action_client_msg, NULL, NULL, boost::bind(&feedback_cb, _1));
+            ROS_INFO("chabao -- send pot position");
+
+            ac_.waitForResult();
+
+            //车辆控制完成，即开始进行抬包动作
+            if (ac_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
+                ROS_INFO("对包后,车辆控制success");  
+                // taibao_cmd_msg.data =1;
+                // taibao_pub.publish(taibao_cmd_msg);
+                // while (true){
+                //     if (taibao_status_feedback == 3){
+
+                //         ac_.waitForResult();
+                //         ROS_INFO("抬包完成,下一步发送到E点的坐标");
+                //         break;
+                //     }
+                }
+                // 发送E点轨迹
+                // loadSuccessNavtoE(task_coordinates["e_point_x"],task_coordinates["e_point_y"]);      //叉包/放包成功后才执行发送返回E点
+                
+                // taibao_status_feedback =0;
+                // ROS_INFO("叉包/放包完成,发送E点坐标");
+                // float e_point_x = task_coordinates["e_point_x"];
+                // float e_point_y = task_coordinates["e_point_y"];
+                // // HingeCarNavClient ac_("hinge_car_nav", true);
+                // action_client_msg.goal_type = 5;    //设置目标类型为叉包
+                // action_client_msg.path.slagpot_pose.pose.position.x = e_point_x;
+                // action_client_msg.path.slagpot_pose.pose.position.y = e_point_y;
+
+                    // ac_.sendGoal(action_client_msg);    //发送目标轨迹
+                // ac_.sendGoal(action_client_msg, NULL, NULL, boost::bind(&feedback_cb, _1));
+
+
+                // if (ac_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
+                //     ROS_INFO("back to E success");   
+                //     hinge_car_status_msg.data = 0;
+                //     hinge_car_current_status_pub.publish(hinge_car_status_msg);
+                // }  
+
+            // }  
+
+        }
+        //放包-103   放包时候，需要记录pot_id   放包--需要发放包位姿，车辆控制完成后，进行放包操作，放包完成后发送E点坐标
+        else if (task_type == 103){
+            auto task_coordinates = received_data["param"]["task_coordinates"];
+            // auto pot_id = task_coordinates["pot_name"];
+        
+
+            // pot_id_msg.data = pot_id;
+            // pot_id_pub.publish(pot_id_msg);
+
+            action_client_msg.goal_type = 4;    //设置目标类型为放包
+            action_client_msg.path.slagpot_pose.pose.position.x = task_coordinates["pot_x"];
+            action_client_msg.path.slagpot_pose.pose.position.y = task_coordinates["pot_y"];
+            tf::Quaternion q = tf::createQuaternionFromRPY(0, 0,task_coordinates["mark_pot_pose"] );  //注意角度为弧度，需要对应
+            action_client_msg.path.slagpot_pose.pose.orientation.x = q.getX();
+            action_client_msg.path.slagpot_pose.pose.orientation.y = q.getY();
+            action_client_msg.path.slagpot_pose.pose.orientation.z = q.getZ();
+            action_client_msg.path.slagpot_pose.pose.orientation.w = q.getW();
+
+                // ac_.sendGoal(action_client_msg);    //发送目标轨迹
+            ac_.sendGoal(action_client_msg, NULL, NULL, boost::bind(&feedback_cb, _1));
+            ROS_INFO("fangbao -- send fangbao pot position");
+
+            ac_.waitForResult();
+
+            if (ac_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
+                ROS_INFO("success");  
+                // fangbao_cmd_msg.data = 1;
+                // fangbao_pub.publish(fangbao_cmd_msg);
+
+                // while(true){
+                //     if(fangbao_status_feedback == 3){
+                //         loadSuccessNavtoE(task_coordinates["e_point_x"],task_coordinates["e_point_y"]); //叉包/放包成功后才执行发送返回E点
+                //         break;
+                //     }
+                //     std::this_thread::sleep_for(std::chrono::milliseconds(10));
+                // }
+                // std::cout << "放包完成" << std::endl;
+                // fangbao_status_feedback == 0;
+            }  
+        }
+
+        //倒渣
+        else if (task_type == 104){
+            std::cout << "进入104 "<< std::endl;
+            // fanbao_cmd_msg.data = 1;
+            // fanbao_pub.publish(fanbao_cmd_msg);
+            // std::cout << "pub data "<< fanbao_cmd_msg;
+            // std::this_thread::sleep_for(std::chrono::milliseconds(10));
+            // while (true) {
+            //     if (fanbao_status_feedback == 3) {
+
+            //         ROS_INFO("daozha success");
+            //         break;  // 跳出循环
+            //     }
+            //     // 休眠时间
+            //     std::this_thread::sleep_for(std::chrono::milliseconds(10));  // 延时10毫秒
+            // }
+            // std::this_thread::sleep_for(std::chrono::milliseconds(10));
+            // hinge_car_status_msg.data = 1;
+            // hinge_car_current_status_pub.publish(hinge_car_status_msg);
+            // std::cout << "倒渣完成"<< std::endl;
+            // std::cout <<"hinge status " <<hinge_car_status_msg<<std::endl;
+            // fanbao_status_feedback =0;
+        }  
+        else if (task_type == 105){
+            std::cout << "回到 E点"<< std::endl;
+            auto task_coordinates = received_data["param"]["task_coordinates"];
+
+            action_client_msg.goal_type = 5;    //设置目标类型为回E包
+            action_client_msg.path.slagpot_pose.pose.position.x = task_coordinates["e_point_x"];
+            action_client_msg.path.slagpot_pose.pose.position.y = task_coordinates["e_point_y"];
+            tf::Quaternion q = tf::createQuaternionFromRPY(0, 0,task_coordinates["e_pose"] );  //注意角度为弧度，需要对应
+            std::cout << "-------"<<task_coordinates["e_pose"] << std::endl;
+            action_client_msg.path.slagpot_pose.pose.orientation.x = q.getX();
+            action_client_msg.path.slagpot_pose.pose.orientation.y = q.getY();
+            action_client_msg.path.slagpot_pose.pose.orientation.z = q.getZ();
+            action_client_msg.path.slagpot_pose.pose.orientation.w = q.getW();
+
+
+
+                // ac_.sendGoal(action_client_msg);    //发送目标轨迹
+            ac_.sendGoal(action_client_msg, NULL, NULL, boost::bind(&feedback_cb, _1));
+
+            ac_.waitForResult();
+
+            if (ac_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
+                ROS_INFO("back to E success");   
+                // hinge_car_status_msg.data = 0;
+                // hinge_car_current_status_pub.publish(hinge_car_status_msg);
+            }  
+
+
+        }
+
+    }
+}
+
+
+
+
+// 函数用于接收HTTP请求并解析数据
+void handleHttpRequest(const Request& req, Response& res) {
+    auto received_data = json::parse(req.body);
+    std::cout << "Received data: " << received_data["cmdid"] << "\n";
+    // std::cout << "Received data: " << received_data.dump(4) << "\n";
+
+    // 在新线程中运行处理数据的函数
+    std::thread process_thread(processReceivedData, received_data);
+
+    // 分离线程，使其在后台运行l
+    process_thread.detach();
+
+}
+
+
+
+int main(int argc, char** argv) {
+
+    setlocale(LC_CTYPE, "zh_CN.utf8");
+
+    ros::init(argc, argv, "decision_node");
+
+
+    signal(SIGINT, sigintHandler);
+    Server svr;
+    svr.Post("/", handleHttpRequest);
+
+    std::thread serverThread([&]() {
+        svr.listen("192.168.131.180", 8000);
+    });
+
+    ros::spin();
+    
+    svr.stop();
+
+    serverThread.join();
+
+    return 0;
+}

hinge_car_decision_nav_task_sensor/src/hinge_car_decision/src/server.cpp

@@ -0,0 +1,60 @@
+#include <ros/ros.h>
+#include <geometry_msgs/Point.h>
+#include <actionlib/server/simple_action_server.h>
+// #include <actionlib/client/simple_action_client.h>
+#include "hinge_car_nav_msgs/HingeCarNavAction.h"
+#include "hinge_car_nav_msgs/HingeCarPath.h"
+#include "hinge_car_nav_msgs/Segment.h"
+
+#include <chrono>
+#include <thread>
+
+
+void execute(const hinge_car_nav_msgs::HingeCarNavGoalConstPtr &goal, actionlib::SimpleActionServer<hinge_car_nav_msgs::HingeCarNavAction> *as)
+{
+    // 在这里编写处理目标的逻辑
+
+    // 获取目标类型
+    uint8_t goal_type = goal->goal_type;
+
+    // 获取路径信息
+    hinge_car_nav_msgs::HingeCarPath path = goal->path;
+    
+    ROS_INFO("SERVER SR");
+
+    // // 处理路径中的段
+    // for (const action_path_client::Segment &segment : path.target_path)
+    // {
+    //     // 处理每个段的逻辑
+    //     // 可以使用segment.segment_type等字段来执行特定操作
+    //     // segment.start_point包含段的起点，segment.end_point包含段的终点
+    //     // segment.max_vel包含最大速度等信息
+    // }
+std::this_thread::sleep_for(std::chrono::milliseconds(20000));  
+//    for(int i =0; i<10000;++i){
+//         std::cout << "mm"<< std::endl;
+//    }
+    // 完成目标并返回结果
+    // hinge_car_nav_msgs::HingeCarNavResult result;
+    // // 设置结果字段
+    // as->setSucceeded(result);
+    as->setSucceeded(hinge_car_nav_msgs::HingeCarNavResult(),"task exit");
+}
+
+int main(int argc, char **argv)
+{
+    // 初始化ROS节点
+    ros::init(argc, argv, "trajectory_server");
+
+    // 创建节点句柄
+    ros::NodeHandle nh;
+
+    // 创建简单行为服务器
+    actionlib::SimpleActionServer<hinge_car_nav_msgs::HingeCarNavAction> server(nh, "hinge_car_nav_test", boost::bind(&execute, _1, &server), false);
+    server.start();
+
+    // 循环等待
+    ros::spin();
+
+    return 0;
+}

hinge_car_decision_nav_task_sensor/src/hinge_car_nav_msgs/CMakeLists.txt

@@ -0,0 +1,46 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(hinge_car_nav_msgs)
+
+find_package(catkin REQUIRED
+  COMPONENTS
+    actionlib_msgs
+    geometry_msgs
+    message_generation
+)
+
+# msgs
+add_message_files(
+  DIRECTORY
+    msg
+  FILES
+    Segment.msg
+    SwitchLocation.msg
+    HingeCarPath.msg
+)
+
+# actions
+add_action_files(
+  DIRECTORY
+    action
+  FILES
+  HingeCarNav.action
+)
+
+add_service_files(
+  DIRECTORY
+      srv
+  FILES
+  getSlagpotPose.srv
+)
+
+
+
+generate_messages(
+  DEPENDENCIES
+    actionlib_msgs
+    geometry_msgs
+)
+
+catkin_package(
+    CATKIN_DEPENDS actionlib_msgs geometry_msgs message_runtime
+)

hinge_car_decision_nav_task_sensor/src/hinge_car_nav_msgs/action/HingeCarNav.action

@@ -0,0 +1,12 @@
+HingeCarPath path 
+####goal_type enum
+uint8 FORWARD = 1             #正向   path中target_path字段必须赋值
+uint8 BACKWARD = 2            #倒车   path中target_path字段必须赋值
+uint8 LOAD_SLAGPOT_IN = 3     #叉包   path中slagpot_pose字段必须赋值 包的位姿(必须包含朝向)
+uint8 UNLOAD_SLAGPOT_IN = 4   #放包   path中slagpot_pose字段必须赋值 包的位姿(必须包含朝向)
+uint8 SLAGPOT_OUT = 5         #返回   path中slagpot_pose字段必须赋值 包的位置(不需要朝向)，对应路中间点的位置
+####
+uint8 goal_type
+---
+---
+geometry_msgs/PoseStamped robot_pose

hinge_car_decision_nav_task_sensor/src/hinge_car_nav_msgs/msg/HingeCarPath.msg

@@ -0,0 +1,3 @@
+uint32 size
+Segment[] target_path
+geometry_msgs/PoseStamped slagpot_pose

hinge_car_decision_nav_task_sensor/src/hinge_car_nav_msgs/msg/Segment.msg

@@ -0,0 +1,9 @@
+geometry_msgs/Point start_point  # 
+geometry_msgs/Point end_point
+geometry_msgs/Point center_point
+float32 max_vel
+uint8 LINE = 1            # LINE target
+uint8 CIRCLE = 2          # CIRCLE target
+####
+uint8 segment_type
+SwitchLocation switch_location

hinge_car_decision_nav_task_sensor/src/hinge_car_nav_msgs/msg/SwitchLocation.msg

@@ -0,0 +1,5 @@
+uint8 RTK2LIDAR = 1            # LINE target
+uint8 LIDAR2RTK = 2          # CIRCLE target
+####
+bool need_swtich_location
+uint8 swtich_action

hinge_car_decision_nav_task_sensor/src/hinge_car_nav_msgs/package.xml

@@ -0,0 +1,25 @@
+<package>
+    <name>hinge_car_nav_msgs</name>
+    <version>1.0.0</version>
+    <description>
+
+        Holds the action description and relevant messages for the hinge_car_nav package.
+
+    </description>
+    <author>gan feng</author>
+    <author email="ganf@seri.com">ganf@seri.com</author>
+    <maintainer email="romeroy@gmail.com">ganfeng</maintainer>
+    <license>BSD</license>
+    <buildtool_depend>catkin</buildtool_depend>
+
+    <build_depend>actionlib_msgs</build_depend>
+    <build_depend>geometry_msgs</build_depend>
+    <build_depend>message_generation</build_depend>
+  
+
+    <run_depend>actionlib_msgs</run_depend>
+    <run_depend>geometry_msgs</run_depend>
+    <run_depend>message_runtime</run_depend>
+ 
+</package>
+

hinge_car_decision_nav_task_sensor/src/hinge_car_nav_msgs/srv/getSlagpotPose.srv

@@ -0,0 +1,4 @@
+std_msgs/Empty empty
+---
+geometry_msgs/PoseStamped slagpotPose
+geometry_msgs/PoseStamped robotPose

hinge_car_decision_nav_task_sensor/src/hinge_car_nav_msgs/srv/taskControl.srv

@@ -0,0 +1,3 @@
+std_msgs/Empty empty
+---
+geometry_msgs/PoseStamped slagpotPose

hinge_car_decision_nav_task_sensor/src/plc_modbus/CMakeLists.txt

@@ -0,0 +1,219 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(plc_modbus)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## 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
+  std_msgs
+  geometry_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## 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 also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES plc_modbus
+#  CATKIN_DEPENDS roscpp std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+set(LIBMODBUS_INCLUDE_DIRS /usr/local/include/modbus)
+set(LIBMODBUS_LIBRARIES /usr/local/lib/libmodbus.so)
+
+include_directories(${LIBMODBUS_INCLUDE_DIRS})
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/plc_modbus.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+
+add_executable(${PROJECT_NAME} src/plc_modbus_node.cpp)
+
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+  ${LIBMODBUS_LIBRARIES}
+)
+
+# target_link_libraries(remote
+#   ${catkin_LIBRARIES}
+#   ${LIBMODBUS_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_plc_modbus.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

hinge_car_decision_nav_task_sensor/src/plc_modbus/REAME.md

@@ -0,0 +1,11 @@
+#  从plc的寄存器通过 modbus-tcp  读取变速箱增量编码器以及铰链转向编码器的值
+
+rostopic pub -1 /cmd_vel /geometry_msgs/Twist "[0,0,0],[0,0,0]"     #发送一次话题以及值
+
+rostopic pub -r 50 /op_mode std_msgs/Int8 'data: 2'
+rostopic pub -r 50 /op_fire std_msgs/Int8 'data: 1'
+rostopic pub -r 50 /automatic_enable std_msgs/Int8 'data: 1'
+rostopic pub -r 50 /forward_max_speed std_msgs/Int8 'data: 100'
+rostopic pub -r 50 /back_max_speed std_msgs/Int8 'data: 100'
+
+rostopic pub -1 /cmd_vel geometry_msgs/Twist "{linear: {x: 0, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0}}"

hinge_car_decision_nav_task_sensor/src/plc_modbus/launch/command.sh

@@ -0,0 +1,9 @@
+#!/bin/bash
+
+while true; do
+  rostopic pub -r 10 /op_mode std_msgs/Int8 'data: 2'
+  rostopic pub -r 10 /op_fire std_msgs/Int8 'data: 1'
+  rostopic pub -r 10 /automatic_enable std_msgs/Int8 'data: 1'
+  rostopic pub -r 10 /forward_max_speed std_msgs/Int8 'data: 10'
+  rostopic pub -r 10 /back_max_speed std_msgs/Int8 'data: 10'
+done

hinge_car_decision_nav_task_sensor/src/plc_modbus/launch/param.launch

@@ -0,0 +1,13 @@
+<launch>
+   
+  <node pkg="rostopic" type="rostopic" name="pub_op_mode" args="pub -1 /op_mode std_msgs/Int8 'data: 0'" output="screen" />
+  <!-- <node pkg="rostopic" type="rostopic" name="pub_op_fire" args="pub -1 /op_fire std_msgs/Int8 'data: 1'" output="screen" /> -->
+  <!-- <node pkg="rostopic" type="rostopic" name="pub_automatic_enable" args="pub -1 /automatic_enable std_msgs/Int8 'data: 1'" output="screen" /> -->
+  <!-- <node pkg="rostopic" type="rostopic" name="pub_brake_status" args="pub -1 /brake_status std_msgs/Int8 'data: 1'" output="screen" /> -->
+  <!-- <node pkg="rostopic" type="rostopic" name="pub_forward_max_speed" args="pub -1 /forward_max_speed std_msgs/Int8 'data: 10'" output="screen" /> -->
+  <!-- <node pkg="rostopic" type="rostopic" name="pub_back_max_speed" args="pub -1 /back_max_speed std_msgs/Int8 'data: -10'" output="screen" /> -->
+
+  <!-- <node pkg="rostopic" type="rostopic" name="pub_cmd_vel" args='pub -1 /cmd_vel geometry_msgs/Twist "{linear: {x: 0, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0}}"' output="screen" /> -->
+  <!-- <node pkg="rostopic" type="rostopic" name="pub_cmd_vel" args='pub -1 /cmd_vel geometry_msgs/Twist "{linear: {x: 0, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0}}"' output="screen" /> -->
+
+</launch>

hinge_car_decision_nav_task_sensor/src/plc_modbus/launch/plc.launch

@@ -0,0 +1,42 @@
+<launch>
+  <!-- <rosparam command="load" file="$(find plc_modbus)/config/delay.yaml" />  -->
+   <arg name="delay_time_1" default="30000" />  
+   <arg name="delay_time_2" default="60000" />  
+   <arg name="delay_time_3" default="90000" />  
+   <arg name="delay_time_4" default="120000" />  
+   <arg name="delay_time_5" default="150000" />  
+
+  <group ns="op_mode">
+      <node pkg="rostopic" type="rostopic" name="pub_op_mode" args="pub -1 /op_mode std_msgs/Int8 'data: 2'" output="screen" />
+  </group>
+
+  <group ns="fire" >
+    <node pkg="rostopic" type="rostopic" name="pub_op_fire" args="pub -1 /op_fire std_msgs/Int8 'data: 1'" output="screen" />
+      <param name="delay" value="$(arg delay_time_1)" />
+    <!-- </node> -->
+  </group>
+
+  <group ns="forward_max_speed" >
+    <node pkg="rostopic" type="rostopic" name="pub_forward_max_speed" args="pub -1 /forward_max_speed std_msgs/Int8 'data: 10'" output="screen" />
+      <param name="delay" value="$(arg delay_time_2)" />
+    <!-- </node> -->
+  </group>
+
+  <group ns="back_max_speed" >
+    <node pkg="rostopic" type="rostopic" name="pub_back_max_speed" args="pub -1 /back_max_speed std_msgs/Int8 'data: -10'" output="screen" /> output="screen" />
+      <param name="delay" value="$(arg delay_time_3)" />
+    <!-- </node> -->
+  </group>
+
+  <group ns="automatic_enable" >
+      <node pkg="rostopic" type="rostopic" name="pub_automatic_enable" args="pub -1 /automatic_enable std_msgs/Int8 'data: 1'" output="screen" />
+      <param name="delay" value="$(arg delay_time_4)" />
+    <!-- </node> -->
+  </group>
+
+  <group ns="brake" >
+      <node pkg="rostopic" type="rostopic" name="pub_brake_status" args="pub -1 /brake_status std_msgs/Int8 'data: 1'" output="screen" />
+      <param name="delay" value="$(arg delay_time_5)" />
+    <!-- </node> -->
+  </group>
+</launch>

hinge_car_decision_nav_task_sensor/src/plc_modbus/launch/plc_modbus.launch

@@ -0,0 +1,20 @@
+<launch>
+  <arg name="plc_ip" default="192.168.3.21 " />    
+  <arg name="dev_port" default=" 502" />    
+  <arg name="working_hz" default=" 50" />    
+  <arg name="modbus_debug" default=" false" />    
+
+  <node pkg="plc_modbus" type="plc_modbus" name="plc_modbus" output="screen">
+    <param name="plc_ip" value="$(arg plc_ip)" />
+    <param name="dev_port" value="$(arg dev_port)" />
+    <param name="working_hz" value="$(arg working_hz)" />
+    <param name="modbus_debug" value="$(arg modbus_debug)" />
+  </node>
+
+  <!-- <node pkg="rostopic" type="rostopic" name="pub_op_mode" args="pub -1 /op_mode std_msgs/Int8 'data: 0'" output="screen" />  -->
+  <!-- <node pkg="rostopic" type="rostopic" name="pub_op_fire" args="pub -1 /op_fire std_msgs/Int8 'data: 1'" output="screen" /> -->
+  <!-- <node pkg="rostopic" type="rostopic" name="pub_automatic_enable" args="pub -1 /automatic_enable std_msgs/Int8 'data: 1'" output="screen" /> -->
+  <!-- <node pkg="rostopic" type="rostopic" name="pub_forward_max_speed" args="pub -r 2 /forward_max_speed std_msgs/Int8 'data: 10'" output="screen" /> -->
+  <!-- <node pkg="rostopic" type="rostopic" name="pub_back_max_speed" args="pub -r 1 /back_max_speed std_msgs/Int8 'data: 10'" output="screen" /> -->
+  <!-- <node pkg="rostopic" type="rostopic" name="pub_cmd_vel" args='pub -1 /cmd_vel geometry_msgs/Twist "{linear: {x: 0, y: 0, z: 0}, angular: {x: 0, y: 0, z: -100}}"' output="screen" /> -->
+</launch>

hinge_car_decision_nav_task_sensor/src/plc_modbus/launch/remote.launch

@@ -0,0 +1,20 @@
+<launch>
+  <arg name="plc_ip" default="192.168.3.21 " />    
+  <arg name="dev_port" default=" 502" />    
+  <arg name="working_hz" default=" 50" />    
+  <arg name="modbus_debug" default=" false" />    
+
+  <node pkg="plc_modbus" type="remote" name="remote" output="screen">
+    <param name="plc_ip" value="$(arg plc_ip)" />
+    <param name="dev_port" value="$(arg dev_port)" />
+    <param name="working_hz" value="$(arg working_hz)" />
+    <param name="modbus_debug" value="$(arg modbus_debug)" />
+  </node>
+
+  <!-- <node pkg="rostopic" type="rostopic" name="pub_op_mode" args="pub -1 /op_mode std_msgs/Int8 'data: 0'" output="screen" />  -->
+  <!-- <node pkg="rostopic" type="rostopic" name="pub_op_fire" args="pub -1 /op_fire std_msgs/Int8 'data: 1'" output="screen" /> -->
+  <!-- <node pkg="rostopic" type="rostopic" name="pub_automatic_enable" args="pub -1 /automatic_enable std_msgs/Int8 'data: 1'" output="screen" /> -->
+  <!-- <node pkg="rostopic" type="rostopic" name="pub_forward_max_speed" args="pub -r 2 /forward_max_speed std_msgs/Int8 'data: 10'" output="screen" /> -->
+  <!-- <node pkg="rostopic" type="rostopic" name="pub_back_max_speed" args="pub -r 1 /back_max_speed std_msgs/Int8 'data: 10'" output="screen" /> -->
+  <!-- <node pkg="rostopic" type="rostopic" name="pub_cmd_vel" args='pub -1 /cmd_vel geometry_msgs/Twist "{linear: {x: 0, y: 0, z: 0}, angular: {x: 0, y: 0, z: -100}}"' output="screen" /> -->
+</launch>

hinge_car_decision_nav_task_sensor/src/plc_modbus/launch/remote_start.sh

@@ -0,0 +1,7 @@
+#!/bin/bash
+
+rostopic pub -1 /op_mode std_msgs/Int8 'data: 1' &
+sleep 5
+
+wait
+exit 0

hinge_car_decision_nav_task_sensor/src/plc_modbus/launch/start.sh

@@ -0,0 +1,21 @@
+#!/bin/bash
+
+rostopic pub -1 /op_mode std_msgs/Int8 'data: 2' &
+sleep 5
+
+# rostopic pub -1 /op_fire std_msgs/Int8 'data: 1' &
+# sleep 5
+
+rostopic pub -1  /forward_max_speed std_msgs/Int8 'data: 100' &
+sleep 5
+
+rostopic pub -1 /back_max_speed std_msgs/Int8 'data: -100' &
+sleep 5
+
+rostopic pub -1 /automatic_enable std_msgs/Int8 'data: 1' &
+sleep 5
+
+rostopic pub -1 /brake_status std_msgs/Int8 'data: 1' &
+sleep 0.1
+wait
+exit 0

hinge_car_decision_nav_task_sensor/src/plc_modbus/package.xml

@@ -0,0 +1,68 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>plc_modbus</name>
+  <version>0.0.0</version>
+  <description>The plc_modbus package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="user@todo.todo">user</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/plc_modbus</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>geometry_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

hinge_car_decision_nav_task_sensor/src/plc_modbus/src/main.cpp

@@ -0,0 +1,10 @@
+#include "plc_modbus.h"
+#include <ros/ros.h>
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "plc_");
+  plc_control::PlcControlNode obj;
+  obj.run();
+  return 0;
+};

hinge_car_decision_nav_task_sensor/src/plc_modbus/src/plc_modbus.cpp

@@ -0,0 +1,361 @@
+#include "plc_modbus.h"
+
+double cmdvel_rcv_time = 0;
+
+string plc_ip;
+int working_hz = 0;
+int dev_port;
+bool modbus_debug = false;
+
+modbus_t *mb;
+int rc;
+
+//  D500 操作模式反愦  D501-点火熄火状态  D502-当前角度  D503-当前速度    D515 心跳信号
+#define READ_REGISTER_SIZE 20
+uint16_t fb_registers[READ_REGISTER_SIZE];
+#define op_mode_plc_callback (short)fb_registers[0]
+#define fire_status_plc_callback (short)fb_registers[1]
+#define fb_steerangle_plc_callback (short)fb_registers[2]
+#define fb_speed_plc_callback (short)fb_registers[3]
+
+#define duibao_status (short)fb_registers[9]   // 对包状态
+#define taibao_status (short)fb_registers[10]  // 抬包状态
+#define fangbao_status (short)fb_registers[11] // 放包状态
+#define fanbao_status (short)fb_registers[12]  // 翻包状态
+
+#define heart_plc_callback (short)fb_registersp[15]
+#define left_outrigger (short)fb_registers[16]  // 左边支腿高度
+#define right_outrigger (short)fb_registers[17] // 右边支腿高度
+#define weight (short)fb_registers[19]          //载重量
+ 
+// D400 操作模式 0-人工   1-遥操   2-无人     D401-点火-1  2-熄火   D402-角度  * 100   D403-目标速度*
+// D404-驻车：1-驻车打开，不驻车  0-无效  2-驻车关闭，驻车  D413 急停
+// D418 自动模式使能 -1    D421-遥控转向频率  D433-向前 最大速度  D434-向后 最大速度
+#define WRITE_REGISTER_SIZE 37
+uint16_t wb_registers[WRITE_REGISTER_SIZE];
+#define op_mode wb_registers[0]        // 操作模式切换控制
+#define fire_cmd wb_registers[1]    // 点火-熄火控制
+#define target_angular wb_registers[2] // 目标角度对应402
+#define target_speed wb_registers[3]   // 目标速度对应403
+#define brake_status wb_registers[4]   // 驻车对应404
+
+#define duibao wb_registers[9]   // 对包控制
+#define taibao wb_registers[10]  // 抬包控制
+#define fangbao wb_registers[11] // 放包控制
+#define fanbao wb_registers[12]  // 翻包控制
+
+#define emergency_stop wb_registers[13]    // 急停 D413
+#define auto_enable wb_registers[18]       // 自动使能
+#define steer_hz wb_registers[21]          // 遥控转向频率
+#define forward_max_speed wb_registers[33] // 向前最大速度
+#define back_max_speed wb_registers[34]    // 向后最大速度
+
+std::map<int, int> statusMap = {
+    {duibao_status, duibao},
+    {taibao_status, taibao},
+    {fangbao_status, fangbao},
+    {fanbao_status, fanbao}};
+
+namespace plc_control
+{
+    PlcControlNode::PlcControlNode()
+        : nh_(""), pnh_("~")
+    {
+        pnh_.getParam("plc_ip", plc_ip);
+        pnh_.getParam("working_hz", working_hz);
+        pnh_.getParam("modbus_debug", modbus_debug);
+        pnh_.getParam("dev_port", dev_port);
+
+        op_mode_sub = nh_.subscribe("/op_mode", 1, &PlcControlNode::op_mode_callback, this);                               // 操作模式
+        fire_mode_sub = nh_.subscribe("/op_fire", 1, &PlcControlNode::fire_mode_callback, this);                           // 点火指令
+        target_val_sub = nh_.subscribe("/cmd_vel", 1, &PlcControlNode::target_vel_back, this);                             // 导航规划的速度   当前无人中角速度为  rad/s   ->  度/s
+        brake_sub = nh_.subscribe("/brake_status", 1, &PlcControlNode::brake_status_back, this);                           // 驻车状态
+        emergency_stop_sub = nh_.subscribe("/emergency_stop", 1, &PlcControlNode::emergency_stop_callback, this);           //  紧急停止  D413
+        control_status_sub = nh_.subscribe("/automatic_enable", 1, &PlcControlNode::auto_enable_back, this);                // 自动使能
+        steer_hz_sub = nh_.subscribe("/steer_hz", 1, &PlcControlNode::steer_hz_callback, this);                            // 遥控转向频率  --涉及遥操
+        forward_max_speed_sub = nh_.subscribe("/forward_max_speed", 1, &PlcControlNode::forward_max_speed_callback, this); // 前向最大速度
+        back_max_speed_sub = nh_.subscribe("/back_max_speed", 1, &PlcControlNode::back_max_speed_callback, this);          // 后向最大速度
+        duibao_sub = nh_.subscribe("/duibao_cmd", 1, &PlcControlNode::duibao_callback, this);                              // 抬包
+        taibao_sub = nh_.subscribe("/taibao_cmd", 1, &PlcControlNode::taibao_callback, this);
+        fangbao_sub = nh_.subscribe("/fangbao_cmd", 1, &PlcControlNode::fangbao_callback, this);
+        fanbao_sub = nh_.subscribe("/fanbao_cmd", 1, &PlcControlNode::fanbao_callback, this);
+
+        fire_status_pub = nh_.advertise<std_msgs::Int8>("/fire_status",1);                                            //点火状态
+        encoder_pub = nh_.advertise<std_msgs::Float32>("/joint", 1);                               // 铰链转向编码器的值 即角度（单位：弧度）
+        encoder_debug_pub = nh_.advertise<std_msgs::Float32>("/joint_anger", 1);                   // 铰链转向编码器的值 即角度（单位：度）
+        fb_speed_pub = nh_.advertise<geometry_msgs::Twist>("/current_car_vel", 1);                 // 变速箱增量编码器的值  即速度
+        left_outrigger_height_pub = nh_.advertise<std_msgs::Int16>("/left_outrigger_height", 1);   // 左边支腿高度
+        right_outrigger_height_pub = nh_.advertise<std_msgs::Int16>("/right_outrigger_height", 1); // 右边支腿高度
+        weight_pub = nh_.advertise<std_msgs::Int16>("/pot_weight", 1);             // 载重量
+        op_mode_status_pub = nh_.advertise<std_msgs::Int8>("/op_mode_current_status",10);           //当前操作模式
+        duibao_pub = nh_.advertise<std_msgs::Int8>("/duibao_status", 10);                           // 对包状态 
+        taibao_pub = nh_.advertise<std_msgs::Int8>("/taibao_status", 10);                           // 抬包状态  
+        fangbao_pub = nh_.advertise<std_msgs::Int8>("/fangbao_status", 10);                         // 放包状态  
+        fanbao_pub = nh_.advertise<std_msgs::Int8>("/fanbao_status", 10);                           // 翻包状态  
+
+        monitor_vel_pub = nh_.advertise <std_msgs::Int16>("/monitor_vel",10);                       //监控写入plc的值 
+        monitor_op_mode_pub = nh_.advertise <std_msgs::Int16>("/monitor_op_mode",10);                       //监控写入plc的值 
+        monitor_enable_pub = nh_.advertise <std_msgs::Int16>("/monitor_enable",10);                       //监控写入plc的值 
+        monitor_for_max_speed = nh_.advertise <std_msgs::Int16>("/monitor_for_max_speed",10);                       //监控写入plc的值 
+        monitor_back_max_speed = nh_.advertise <std_msgs::Int16>("/monitor_back_max_speed",10);                       //监控写入plc的值 
+        monitor_brake = nh_.advertise <std_msgs::Int16>("/monitor_brake",10);                       //监控写入plc的值 
+        monitor_angular_pub = nh_.advertise <std_msgs::Int16>("/monitor_angular",10);  
+        
+    };
+
+    // Destructor
+    PlcControlNode::~PlcControlNode()
+    {
+    }
+
+    double PlcControlNode::degree2rad(const double &d)
+    {
+        return (double)d * M_PI / (double)180.0;
+    }
+
+    double PlcControlNode::rad2degree(const double &r)
+    {
+        return (double)r * 180.0 / (double)M_PI;
+    }
+
+    void PlcControlNode::op_mode_callback(const std_msgs::Int8::ConstPtr &op_msg)
+    {
+        op_mode = op_msg->data;
+    }
+
+    // 点火-熄火模式
+    void PlcControlNode::fire_mode_callback(const std_msgs::Int8::ConstPtr &fire_msg)
+    {
+        fire_cmd= fire_msg->data;
+        // ROS_INFO("XIHUO ");
+    }
+
+    // 目标速度和角度
+    void PlcControlNode::target_vel_back(const geometry_msgs::Twist::ConstPtr &targer_vel_msg)
+    {
+        target_anger = targer_vel_msg->angular.z;
+        if (target_anger < 1.5)
+            prev_anger = target_anger;
+        
+        if (target_anger > 1.5)
+            target_anger = prev_anger;
+
+        // target_angular = (short)(rad2degree(targer_vel_msg->angular.z) * 100);
+        target_angular = (short)(rad2degree(target_anger) * 100);
+        target_speed = (short)(targer_vel_msg->linear.x * 100);
+
+        cmdvel_rcv_time = ros::Time::now().toSec();
+
+        // ros::Time t1 = ros::Time::now() - ros::Duration(5.5); //t1是5.5s前的时刻，Time加减Duration返回都是Time
+        // ros::Time t2 = ros::Time::now() + ros::Duration(3.3);//t2是当前时刻往后推3.3s的时刻
+        // ros::Duration d1 = t2 - t1;
+        // std::cout << "target speed "<< target_speed << std::endl;
+    }
+
+    void PlcControlNode::brake_status_back(const std_msgs::Int8::ConstPtr &brake_msg)
+    {
+        brake_status = brake_msg->data;
+    }
+
+    // 自动使能
+    void PlcControlNode::auto_enable_back(const std_msgs::Int8::ConstPtr &auto_enable_msg)
+    {
+        auto_enable = auto_enable_msg->data;
+    }
+
+    void PlcControlNode::emergency_stop_callback(const std_msgs::Int8::ConstPtr &emergency_msg)
+    {
+
+        emergency_stop = emergency_msg->data;
+    }
+
+    // 遥控转向频率
+    void PlcControlNode::steer_hz_callback(const std_msgs::Int16::ConstPtr &steer_msg)
+    {
+        steer_hz = steer_msg->data;
+    }
+    // 向前最大速度
+    void PlcControlNode::forward_max_speed_callback(const std_msgs::Int8::ConstPtr &forward_max_speed_msg)
+    {
+        forward_max_speed = forward_max_speed_msg->data;
+    }
+
+    // 向后最大速度
+    void PlcControlNode::back_max_speed_callback(const std_msgs::Int8::ConstPtr &back_max_speed_msg)
+    {
+        back_max_speed = back_max_speed_msg->data;
+    }
+
+    void PlcControlNode::duibao_callback(const std_msgs::Int8::ConstPtr &duibao_msg)
+    {
+        duibao = duibao_msg->data;
+    }
+
+    void PlcControlNode::taibao_callback(const std_msgs::Int8::ConstPtr &dtaibao_msg)
+    {
+        taibao = dtaibao_msg->data;
+    }
+
+    void PlcControlNode::fangbao_callback(const std_msgs::Int8::ConstPtr &fangbao_msg)
+    {
+        fangbao = fangbao_msg->data;
+    }
+
+    void PlcControlNode::fanbao_callback(const std_msgs::Int8::ConstPtr &fanbao_msg)
+    {
+        fanbao = fanbao_msg->data;
+    }
+
+    void PlcControlNode::run()
+    {
+        ROS_INFO_STREAM("plc control start");
+        ros::Rate loop_rate(working_hz);
+
+        mb = modbus_new_tcp(plc_ip.c_str(), dev_port);
+        rc = modbus_connect(mb);
+        rc = modbus_set_debug(mb, modbus_debug);
+
+        while (ros::ok())
+        {
+            // mb = modbus_new_tcp(plc_ip.c_str(), dev_port);
+            // rc = modbus_connect(mb);
+            // rc = modbus_set_debug(mb, modbus_debug);
+            double rv_time = ros::Time::now().toSec();
+
+            // D400 操作模式 0-人工   1-遥操   2-无人     D401-点火-1  2-熄火   D402-角度  * 100   D403-目标速度*
+            // D418 自动模式使能 -1    D421-遥控转向频率  D433-向前 最大速度  D434-向后 最大速度
+            // D501-点火熄火状态  D500 操作模式反愦 D502-当前角度  D503-当前速度    D515 心跳信号
+            rc = modbus_read_registers(mb, 500, READ_REGISTER_SIZE, fb_registers);
+
+            // if (target_angular < 1.5)
+            //     prev_anger = target_angular;
+            
+            // if (target_angular > 1.5)
+            //     target_angular = prev_anger;
+            
+            if ((rv_time - cmdvel_rcv_time) > 0.2)
+            {
+                target_speed = 0;
+                // target_angular = prev_anger;
+            }
+            // if (op_mode_plc_callback == 0)
+
+
+
+            // 对包状态
+            if (duibao_status == 3){
+                duibao = 0;
+            }
+            if (taibao_status == 3){
+                taibao = 0;
+            }
+            if (fangbao_status == 3){
+                fangbao = 0;
+            }
+            if (fanbao_status == 3){
+                fanbao = 0;
+            }
+
+        
+            // for (auto &statusPair : statusMap)
+            // {
+            //     if (statusPair.first == 3)
+            //     {
+            //         statusPair.second = 0;
+            //     }
+            // }
+
+            if (fire_status_plc_callback == 1){
+                fire_cmd = 0;
+            }
+
+            rc = modbus_write_registers(mb, 400, WRITE_REGISTER_SIZE, wb_registers);
+
+		  
+            std_msgs::Int16 monitor_vel_msg;      //监控写入plc的速度
+            monitor_vel_msg.data = target_speed;
+            monitor_vel_pub.publish(monitor_vel_msg);
+
+            std_msgs::Int16 monitor_anger_msg;      //监控写入plc的角度
+            monitor_anger_msg.data = target_angular;
+            monitor_angular_pub.publish(monitor_anger_msg);
+
+            std_msgs::Int16 monitor_op_mode_msg;      //监控写入op模式
+            monitor_op_mode_msg.data = op_mode;
+            monitor_op_mode_pub.publish(monitor_op_mode_msg);
+
+            std_msgs::Int16 monitor_enable_msg;      //监控写入plc的是否自动使能
+            monitor_enable_msg.data = auto_enable;
+            monitor_enable_pub.publish(monitor_enable_msg);
+
+            std_msgs::Int16 monitor_for_max_speed_msg;      //监控写入plc的for  maxspeed
+            monitor_for_max_speed_msg.data = forward_max_speed;
+            monitor_for_max_speed.publish(monitor_for_max_speed_msg);
+
+            std_msgs::Int16 monitor_back_max_speed_msg;      //监控写入plc的back  maxspeed
+            monitor_back_max_speed_msg.data = back_max_speed;
+            monitor_back_max_speed.publish(monitor_back_max_speed_msg);
+
+            std_msgs::Int16 monitor_brake_msg;      //监控写入plc的brake 释放
+            monitor_brake_msg.data = back_max_speed;
+            monitor_brake.publish(monitor_brake_msg);
+
+            std_msgs::Int8 op_status_msg;      //操作模式
+            op_status_msg.data = op_mode_plc_callback;
+            op_mode_status_pub.publish(op_status_msg);
+            
+            std_msgs::Int8 op_fire_msg;       //点火模式
+            op_fire_msg.data = fire_status_plc_callback;
+            fire_status_pub.publish(op_fire_msg);
+
+
+            // fb_steerangle
+            std_msgs::Float32 steer_rad;
+            steer_rad.data = degree2rad(fb_steerangle_plc_callback * 0.01f);
+            encoder_pub.publish(steer_rad); // 铰链转向编码器的值 即角度（单位：弧度）
+
+            std_msgs::Float32 steer;
+            steer.data = fb_steerangle_plc_callback * 0.01f;
+            encoder_debug_pub.publish(steer); // 铰链转向编码器的值 即角度（单位：度）
+
+            geometry_msgs::Twist speed;
+            // speed.linear.x = 0;
+            speed.linear.x = fb_speed_plc_callback * 0.01f;
+            speed.linear.y = 0;
+            fb_speed_pub.publish(speed); // 变速箱增量编码器的值， 即速度
+
+            std_msgs::Int16 left_outrigger_height_msg; // 左侧支腿高度
+            left_outrigger_height_msg.data = left_outrigger;
+            left_outrigger_height_pub.publish(left_outrigger_height_msg);
+
+            std_msgs::Int16 right_outrigger_height_msg; // 右侧支腿高度
+            right_outrigger_height_msg.data = right_outrigger;
+            right_outrigger_height_pub.publish(right_outrigger_height_msg);
+            
+            std_msgs::Int16 weight_msg; //载重
+            weight_msg.data = weight;
+            weight_pub.publish(weight_msg);
+
+            std_msgs::Int8 duibao_msg;          //对包状态
+            duibao_msg.data = duibao_status;
+            duibao_pub.publish(duibao_msg);
+
+            std_msgs::Int8 taibao_msg;          //抬包状态
+            taibao_msg.data = taibao_status;
+            taibao_pub.publish(taibao_msg);
+
+            std_msgs::Int8 fangbao_msg;         //放包状态
+            fangbao_msg.data = fangbao_status;
+            fangbao_pub.publish(fangbao_msg);
+
+            std_msgs::Int8 fanbao_msg;          //翻包状态
+            fanbao_msg.data = fanbao_status;
+            fanbao_pub.publish(fanbao_msg);
+
+            ros::spinOnce();
+            loop_rate.sleep();
+        }
+        modbus_close(mb);
+        modbus_free(mb);
+    }
+}

hinge_car_decision_nav_task_sensor/src/plc_modbus/src/plc_modbus.h

@@ -0,0 +1,60 @@
+#ifndef PLC_MODBUS_H
+#define PLC_MODBUS_H
+
+#include <ros/ros.h>
+#include <std_msgs/Int8.h>
+#include <std_msgs/Int16.h>
+#include <std_msgs/Float32.h>
+#include <geometry_msgs/Twist.h>
+#include <modbus/modbus.h>
+
+using std::string;
+
+
+namespace plc_control{
+
+class PlcControlNode
+    {
+    public:
+        PlcControlNode();
+        ~PlcControlNode();
+ 
+        void run();
+
+
+    private:
+        ros::NodeHandle nh_;                    //!< @brief ros node handle
+        ros::NodeHandle pnh_;                    //!< @brief ros node handle
+
+        float prev_anger;     //记录上一次角度
+        float target_anger;
+
+        //发布者
+        ros::Publisher encoder_pub, encoder_debug_pub, fb_speed_pub, duibao_pub, fangbao_pub, taibao_pub, fanbao_pub, weight_pub,
+                       left_outrigger_height_pub, right_outrigger_height_pub,fire_status_pub,op_mode_status_pub, monitor_vel_pub,
+                       monitor_op_mode_pub,monitor_enable_pub,monitor_for_max_speed,monitor_back_max_speed,monitor_brake,monitor_angular_pub;   
+        ros::Subscriber  op_mode_sub, fire_mode_sub, target_val_sub, brake_sub, emergency_stop_sub, control_status_sub,
+                         steer_hz_sub, forward_max_speed_sub, back_max_speed_sub, duibao_sub, fangbao_sub, fanbao_sub, taibao_sub;
+
+        double degree2rad(const double &d);                     //角度转弧度
+        double rad2degree(const double &r);                     //弧度转角度
+
+        void op_mode_callback(const std_msgs::Int8::ConstPtr &op_msg);                  // 操作模式
+        void fire_mode_callback(const std_msgs::Int8::ConstPtr &fire_msg);              // 点火-熄火模式
+        void target_vel_back(const geometry_msgs::Twist::ConstPtr &targer_vel_msg);     // 目标速度和角度
+        void brake_status_back(const std_msgs::Int8::ConstPtr &brake_msg);
+        void auto_enable_back(const std_msgs::Int8::ConstPtr &auto_enable_msg);         // 自动使能
+        void emergency_stop_callback(const std_msgs::Int8::ConstPtr &emergency_msg);               //急停  （遥操使用）
+        void steer_hz_callback(const std_msgs::Int16::ConstPtr &steer_msg);                    // 遥操转向频率 （遥操使用）
+        void forward_max_speed_callback(const std_msgs::Int8::ConstPtr &forward_max_speed_msg);            // 向前最大速度
+        void back_max_speed_callback(const std_msgs::Int8::ConstPtr &back_max_speed_msg);               // 向后最大速度
+        void duibao_callback(const std_msgs::Int8::ConstPtr &duibao_msg);           //对包
+        void taibao_callback(const std_msgs::Int8::ConstPtr &taibao_msg);           //抬包
+        void fangbao_callback(const std_msgs::Int8::ConstPtr &fangbao_msg);         //放包
+        void fanbao_callback(const std_msgs::Int8::ConstPtr &fanbao_msg);           //翻包
+
+    };
+
+}
+
+#endif // PLC_MODBUS_H

hinge_car_decision_nav_task_sensor/src/plc_modbus/src/plc_modbus_node.cpp

@@ -0,0 +1,269 @@
+#include <ros/ros.h>
+#include <std_msgs/String.h>
+#include <modbus/modbus.h>
+#include <iostream>
+#include <std_msgs/Float32.h>
+#include <std_msgs/Float64.h>
+#include <std_msgs/Int8.h>
+#include <can_msgs/Frame.h>
+#include <geometry_msgs/Twist.h>
+#include <std_msgs/Int64.h>
+#include <std_msgs/Int16.h>
+#include <std_msgs/Bool.h>
+
+using std::cout;
+using std::dec;
+using std::endl;
+using std::hex;
+using std::string;
+
+double cmdvel_rcv_time = 0;
+
+//=======================================================================================================
+double degree2rad(const double &d)
+{
+    return (double)d * M_PI / (double)180.0;
+}
+double rad2degree(const double &r)
+{
+    return (double)r * 180.0 / (double)M_PI;
+}
+
+//=======================================================================================================
+//  D500 操作模式反愦  D501-点火熄火状态  D502-当前角度  D503-当前速度    D515 心跳信号
+#define READ_REGISTER_SIZE 20
+uint16_t fb_registers[READ_REGISTER_SIZE];
+#define op_mode_plc_callback (short)fb_registers[0]
+#define fire_status_plc_callback (short)fb_registers[1]
+#define fb_steerangle_plc_callback (short)fb_registers[2]
+#define fb_speed_plc_callback (short)fb_registers[3]
+#define heart_plc_callback (short)fb_registersp[15]
+#define left_outrigger (short)fb_registers[16]  // 左边支腿高度
+#define right_outrigger (short)fb_registers[17] // 右边支腿高度
+// #define size 1
+// uint16_t fb_registers[size];
+// #define fb_steerangle_plc_callback (short)fb_registers[0]
+
+// D400 操作模式 0-人工   1-遥操   2-无人     D401-点火-1  2-熄火   D402-角度  * 100   D403-目标速度*
+// D404-驻车：1-驻车打开，不驻车  0-无效  2-驻车关闭，驻车  D413 急停
+// D418 自动模式使能 -1    D421-遥控转向频率  D433-向前 最大速度  D434-向后 最大速度
+#define WRITE_REGISTER_SIZE 37
+uint16_t wb_registers[WRITE_REGISTER_SIZE];
+#define op_mode wb_registers[0]            // 操作模式
+#define fire_status wb_registers[1]        // 点火-熄火模式
+#define traget_angular wb_registers[2]     // 目标角度对应402
+#define target_speed wb_registers[3]       // 目标速度对应403
+#define brake_status wb_registers[4]       // 驻车对应404
+#define emergency_stop wb_registers[13]    // 急停 D413
+#define auto_enable wb_registers[18]       // 自动使能
+#define steer_hz wb_registers[21]          // 遥控转向频率
+#define forward_max_speed wb_registers[33] // 向前最大速度
+#define back_max_speed wb_registers[34]    // 向后最大速度
+
+// #define op_mode wb_registers[0]            // 操作模式
+// #define fire_status wb_registers[1]        // 点火-熄火模式
+// #define traget_angular wb_registers[0]     // 目标角度对应402
+// #define target_speed wb_registers[1]       // 目标速度对应403
+// #define brake_status wb_registers[2]       // 驻车对应404
+// #define auto_enable wb_registers[16]       // 自动使能
+// #define steer_hz wb_registers[21]          // 遥控转向频率
+// #define forward_max_speed wb_registers[31] // 向前最大速度
+// #define back_max_speed wb_registers[32]    // 向后最大速度
+
+// input
+// short sys_working_status = 0;
+
+// 操作模式
+void op_mode_callback(const std_msgs::Int8::ConstPtr &msg)
+{
+    op_mode = msg->data;
+}
+
+// // 点火-熄火模式
+void fire_mode_callback(const std_msgs::Int8::ConstPtr &msg)
+{
+    fire_status = msg->data;
+}
+
+// 目标速度和角度
+void target_vel_back(const geometry_msgs::Twist::ConstPtr &msg)
+{
+
+    // traget_angular = (short)(msg->angular.z  * 100);
+    traget_angular = (short)(rad2degree(msg->angular.z) * 100);
+    // target_speed = (short)(msg->linear.y);
+    target_speed = (short)(msg->linear.x * 100);
+
+    cmdvel_rcv_time = ros::Time::now().toSec();
+
+    // ros::Time t1 = ros::Time::now() - ros::Duration(5.5); //t1是5.5s前的时刻，Time加减Duration返回都是Time
+    // ros::Time t2 = ros::Time::now() + ros::Duration(3.3);//t2是当前时刻往后推3.3s的时刻
+    // ros::Duration d1 = t2 - t1;
+    std::cout << "target val " << std::endl;
+}
+
+// void target_vel_back(const geometry_msgs::Twist::ConstPtr &msg)
+// {
+
+//     // traget_angular = (short)(msg->angular.z  * 100);
+//     traget_angular = (short)(rad2degree(msg->angular.z) * 100);
+//     // target_speed = (short)(msg->linear.y);
+//     target_speed = (short)(msg->linear.x * 100);
+// }
+
+void brake_status_back(const std_msgs::Int8::ConstPtr &msg)
+{
+    brake_status = msg->data;
+}
+
+// 自动使能
+void auto_enable_back(const std_msgs::Int8::ConstPtr &msg)
+{
+    auto_enable = msg->data;
+}
+
+void emergency_stop_callback(const std_msgs::Int8::ConstPtr &msg)
+{
+
+    emergency_stop = msg->data;
+}
+
+// // 遥控转向频率
+void steer_hz_callback(const std_msgs::Int16::ConstPtr &msg)
+{
+    steer_hz = msg->data;
+}
+// 向前最大速度
+void forward_max_speed_callback(const std_msgs::Int8::ConstPtr &msg)
+{
+    forward_max_speed = msg->data;
+}
+
+// 向后最大速度
+void back_max_speed_callback(const std_msgs::Int8::ConstPtr &msg)
+{
+    back_max_speed = msg->data;
+}
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "plc_interface");
+    ros::NodeHandle nh;
+    ros::NodeHandle nh2("~");
+
+    string plc_ip;
+    int working_hz = 0;
+    int dev_port;
+    bool modbus_debug = false;
+
+    nh2.getParam("plc_ip", plc_ip);
+    nh2.getParam("working_hz", working_hz);
+    nh2.getParam("modbus_debug", modbus_debug);
+    nh2.getParam("dev_port", dev_port);
+    // D400 操作模式 0-人工   1-遥操   2-无人     D401-点火-1  2-熄火   D402-角度  * 100   D403-目标速度*  D413急停
+    // D418 自动模式使能 -1    D421-遥控转向频率  D433-向前 最大速度  D434-向后 最大速度
+
+    // 操作模式：
+    ros::Subscriber op_mode_sub = nh.subscribe("/op_mode", 1, op_mode_callback); // 操作模式
+    // 点火-熄火指令
+    ros::Subscriber fire_mode_sub = nh.subscribe("/op_fire", 1, fire_mode_callback); // 点火指令
+    // D402-D403  角度-速度指令
+    ros::Subscriber target_val_sub = nh.subscribe("/cmd_vel", 1, target_vel_back); // 导航规划的速度   当前无人中角速度为  rad/s   ->  度/s
+
+    // D404 驻车
+    ros::Subscriber brake_sub = nh.subscribe("/brake_status", 1, brake_status_back); // 驻车状态
+    // D413 急停
+    ros::Subscriber emergency_stop_sub = nh.subscribe("/emergency_stop", 1, emergency_stop_callback); //  紧急停止  D413
+
+    // D418 自动使能 -1
+    ros::Subscriber control_status_sub = nh.subscribe("/automatic_enable", 1, auto_enable_back); // 自动使能
+    // D421 遥控转向频率
+    ros::Subscriber steer_hz_sub = nh.subscribe("/steer_hz", 1, steer_hz_callback); // 遥控转向频率  --涉及遥操
+    // D433 向前最大速度
+    ros::Subscriber forward_max_speed_sub = nh.subscribe("/forward_max_speed", 1, forward_max_speed_callback); // 前向最大速度
+    // D434 向后最大速度
+    ros::Subscriber back_max_speed_sub = nh.subscribe("/back_max_speed", 1, back_max_speed_callback); // 后向最大速度
+
+    ros::Publisher encoder_pub = nh.advertise<std_msgs::Float32>("/joint", 10);                               // 铰链转向编码器的值 即角度（单位：弧度）
+    ros::Publisher encoder_debug_pub = nh.advertise<std_msgs::Float32>("/joint_anger", 10);                   // 铰链转向编码器的值 即角度（单位：度）
+    ros::Publisher fb_speed_pub = nh.advertise<geometry_msgs::Twist>("/current_car_vel", 10);                 // 变速箱增量编码器的值  即速度   //需要修改成geometry_msgs/twist
+    ros::Publisher left_outrigger_height_pub = nh.advertise<std_msgs::Int16>("/left_outrigger_height", 10);   // 左边支腿高度
+    ros::Publisher right_outrigger_height_pub = nh.advertise<std_msgs::Int16>("/right_outrigger_height", 10); // 右边支腿高度
+
+    modbus_t *mb;
+
+    int rc;
+
+    mb = modbus_new_tcp(plc_ip.c_str(), dev_port);
+
+    if (mb == NULL)
+    {
+        std::cout << "connect error" << std::endl;
+        return -1;
+    }
+    else
+    {
+        std::cout << "connect ok" << std::endl;
+    }
+
+    rc = modbus_connect(mb);
+    if (rc == -1)
+    {
+        std::cout << "connect error 2" << std::endl;
+        modbus_free(mb);
+        return -1;
+    }
+
+    rc = modbus_set_debug(mb, modbus_debug);
+
+    ros::Rate loop_rate(working_hz);
+
+    while (ros::ok())
+    {
+
+        double rv_time = ros::Time::now().toSec();
+
+        // D400 操作模式 0-人工   1-遥操   2-无人     D401-点火-1  2-熄火   D402-角度  * 100   D403-目标速度*
+        // D418 自动模式使能 -1    D421-遥控转向频率  D433-向前 最大速度  D434-向后 最大速度
+        // D501-点火熄火状态  D500 操作模式反愦 D502-当前角度  D503-当前速度    D515 心跳信号
+        rc = modbus_read_registers(mb, 500, READ_REGISTER_SIZE, fb_registers);
+
+        if ((rv_time - cmdvel_rcv_time) > 0.2)  //当前时间与收到速度、转角指令时间超过0.2s,则将速度和转角赋值为0，0.2对应速度转角频率5hz；
+        {
+            target_speed = 0;
+            traget_angular = 0;
+        }
+        rc = modbus_write_registers(mb, 400, WRITE_REGISTER_SIZE, wb_registers);
+
+        // fb_steerangle
+        std_msgs::Float32 steer_rad;
+        steer_rad.data = degree2rad(fb_steerangle_plc_callback * 0.01f);
+        encoder_pub.publish(steer_rad); // 铰链转向编码器的值 即角度（单位：弧度）
+
+        std_msgs::Float32 steer;
+        steer.data = fb_steerangle_plc_callback * 0.01f;
+        encoder_debug_pub.publish(steer); // 铰链转向编码器的值 即角度（单位：度）
+
+        geometry_msgs::Twist speed;
+        // speed.linear.x = 0;
+        speed.linear.x = fb_speed_plc_callback * 0.01f;
+        speed.linear.y = 0;
+        fb_speed_pub.publish(speed); // 变速箱增量编码器的值， 即速度
+
+        std_msgs::Int16 left_outrigger_height_msg; // 左侧支腿高度
+        left_outrigger_height_msg.data = left_outrigger;
+        left_outrigger_height_pub.publish(left_outrigger_height_msg);
+
+        std_msgs::Int16 right_outrigger_height_msg; // 右侧支腿高度
+        right_outrigger_height_msg.data = right_outrigger;
+        right_outrigger_height_pub.publish(right_outrigger_height_msg);
+
+        ros::spinOnce();
+        loop_rate.sleep();
+    }
+
+    modbus_close(mb);
+    modbus_free(mb);
+
+    return 0;
+}

hinge_car_decision_nav_task_sensor/src/sensor/line_lidar/CMakeLists.txt

@@ -0,0 +1,208 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(line_lidar)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## 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
+  sensor_msgs
+  std_msgs
+)
+find_package(async_comm REQUIRED)
+
+## System dependencies are found with CMake's conventions
+find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   sensor_msgs#   std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## 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 also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES line_lidar
+#  CATKIN_DEPENDS roscpp sensor_msgs std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/line_lidar.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+add_executable(${PROJECT_NAME} src/line_lidar_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(${PROJECT_NAME}
+   ${catkin_LIBRARIES}
+   async_comm
+ )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_line_lidar.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

hinge_car_decision_nav_task_sensor/src/sensor/line_lidar/README.md

@@ -0,0 +1 @@
+#渣包识别激光雷达  * 3

hinge_car_decision_nav_task_sensor/src/sensor/line_lidar/launch/lidar.launch

@@ -0,0 +1,38 @@
+<launch>
+<!-- 192.168.3.103
+192.168.3.104 -->
+    <arg name="ip_left" default=" 192.168.3.102" /> 
+    <arg name="ip_middle" default=" 192.168.3.103" /> 
+    <arg name="ip_right" default=" 192.168.3.104" /> 
+    <arg name="port" default=" 5566" /> 
+    <arg name="topic_left" value="/scan_left" />
+    <arg name="topic_middle" value="/scan_middle" />
+    <arg name="topic_right" value="/scan_right" />
+    <arg name="frame_id_left" value="laser_left" />
+    <arg name="frame_id_middle" value="laser_middle" />
+    <arg name="frame_id_right" value="laser_right" />
+
+
+<node pkg="line_lidar" type="line_lidar" name="lidar_left" output="screen">
+    <param name="ip" value="$(arg ip_left)" />
+    <param name="port" value="$(arg port)" />
+    <param name="topic" value="$(arg topic_left)" />
+    <param name="frame_id" value="$(arg frame_id_left)" />
+</node>
+
+<node pkg="line_lidar" type="line_lidar" name="lidar_middle" output="screen">
+    <param name="ip" value="$(arg ip_middle)" />
+    <param name="port" value="$(arg port)" />
+    <param name="topic" value="$(arg topic_middle)" />
+    <param name="frame_id" value="$(arg frame_id_middle)" />
+</node>
+
+<node pkg="line_lidar" type="line_lidar" name="lidar_right" output="screen">
+    <param name="ip" value="$(arg ip_right)" />
+    <param name="port" value="$(arg port)" />
+    <param name="topic" value="$(arg topic_right)" />
+    <param name="frame_id" value="$(arg frame_id_right)" />
+</node>
+
+
+</launch>

hinge_car_decision_nav_task_sensor/src/sensor/line_lidar/launch/lidar_left.launch

@@ -0,0 +1,11 @@
+<launch>
+
+<node pkg="line_lidar" type="line_lidar" name="lidar_left" output="screen">
+    <param name="ip" value="192.168.3.102" />
+    <param name="port" value="5566" />
+    <param name="topic" value="/scan_left" />
+    <param name="frame_id" value="laser_left" />
+</node>
+
+
+</launch>

hinge_car_decision_nav_task_sensor/src/sensor/line_lidar/launch/lidar_middle.launch

@@ -0,0 +1,11 @@
+<launch>
+
+<node pkg="line_lidar" type="line_lidar" name="lidar_middle" output="screen">
+    <param name="ip" value="192.168.3.103" />
+    <param name="port" value="5566" />
+    <param name="topic" value="/scan_middle" />
+    <param name="frame_id" value="laser_middle" />
+</node>
+
+
+</launch>

hinge_car_decision_nav_task_sensor/src/sensor/line_lidar/launch/lidar_right.launch

@@ -0,0 +1,11 @@
+<launch>
+
+<node pkg="line_lidar" type="line_lidar" name="lidar_right" output="screen">
+    <param name="ip" value="192.168.3.104" />
+    <param name="port" value="5566" />
+    <param name="topic" value="/scan_right" />
+    <param name="frame_id" value="laser_right" />
+</node>
+
+
+</launch>

hinge_car_decision_nav_task_sensor/src/sensor/line_lidar/launch/sensor_enable.launch

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<launch>
+    <include file="$(find line_lidar)/launch/lidar.launch">
+        <!--<arg name="model" value="$(arg model)" />-->   
+    </include>
+
+    <include file="$(find nalei_radar)/launch/nalei_radar.launch">
+        <!--<arg name="model" value="$(arg model)" />-->   
+    </include>
+
+    <include file="$(find plc_modbus)/launch/plc_modbus.launch">
+        <!--<arg name="model" value="$(arg model)" />-->   
+    </include>
+
+    <include file="$(find rtk_imu_pkg)/launch/rtk_imu.launch">
+        <!--<arg name="model" value="$(arg model)" />-->   
+    </include>
+
+    <include file="$(find wit_pos)/launch/wit.launch">
+        <!--<arg name="model" value="$(arg model)" />-->   
+    </include>
+
+    <include file="$(find wheel_lidar)/launch/wheel_lidar.launch">
+        <!--<arg name="model" value="$(arg model)" />-->   
+    </include>  
+
+</launch>

hinge_car_decision_nav_task_sensor/src/sensor/line_lidar/package.xml

@@ -0,0 +1,68 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>line_lidar</name>
+  <version>0.0.0</version>
+  <description>The line_lidar package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="user@todo.todo">user</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/line_lidar</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>sensor_msgs</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

hinge_car_decision_nav_task_sensor/src/sensor/line_lidar/src/line_lidar_node.cpp

@@ -0,0 +1,283 @@
+#include <ros/ros.h>
+#include <async_comm/tcp_client.h>
+#include <std_msgs/Float64.h>
+#include <sensor_msgs/LaserScan.h>
+
+using std::cout;
+using std::dec;
+using std::endl;
+using std::hex;
+using std::string;
+
+const float max_range_val = 50.0;
+const float min_range_val = 0;
+
+uint8_t cmd[14] = {0xff, 0xff, 0xff, 0xff,
+                   0x4d, 0x44, 0x31,
+                   0x00,
+                   0x00, 0x00, 0x00, 0x00,
+                   0xB5, 0x8B};
+#define RX_BUFFER_SIZE 8192
+uint8_t rx_data_buf[RX_BUFFER_SIZE] = {0x00};
+
+uint8_t rx_buf[4096] = {0x00};
+
+ros::Publisher scan_pub;
+
+bool rcv_head = false;
+bool rcv_data = false;
+bool rcv_heart = false;
+
+int head_index = 0;
+
+int head = 0;
+uint8_t *head_ptr = reinterpret_cast<uint8_t *>(&head);
+
+int data_length = 0;
+uint8_t *data_length_ptr = reinterpret_cast<uint8_t *>(&data_length);
+int data_rest_length = 0;
+int data_all_length = 0;
+
+#define CMD_TYPE rx_data_buf[4]
+#define CMD_OP rx_data_buf[5]
+#define CMD_WORD rx_data_buf[6]
+
+uint8_t data_buf[8192] = {0x00};
+int data_count = 0;
+uint8_t *data_count_ptr = reinterpret_cast<uint8_t *>(&data_count);
+
+string frame_id;
+
+struct lidar_time
+{
+    /* data */
+    uint16_t year;
+    uint8_t month;
+    uint8_t day;
+    uint8_t hour;
+    uint8_t min;
+    uint8_t sec;
+    uint8_t reserve;
+    uint32_t msec;
+
+} start_time, end_time, last_scan_time;
+
+std::vector<float> ranges;
+std::vector<float> intensities;
+
+int dis = 0;
+short inten = 0;
+uint8_t *dis_ptr = reinterpret_cast<uint8_t *>(&dis);
+uint8_t *inten_ptr = reinterpret_cast<uint8_t *>(&inten);
+
+double degree2ran(const double &d)
+{
+    return (double)d * M_PI / (double)180.0;
+}
+void socket_rcv_callback(const uint8_t *buf, size_t len)
+{
+    if (!rcv_head)
+    {
+        if (len >= 0)
+        {
+            memcpy(&rx_buf[0], buf, len);
+            memcpy(head_ptr, &rx_buf[0], 4);
+            // if (rx_buf[0] == 0xff && rx_buf[1] == 0xff && rx_buf[2] == 0xff && rx_buf[3] == 0xff)
+            // {
+            //     cout << "===== head = " << head << endl;
+            //     for (int i = 0; i < 4; i++)
+            //     {
+            //         cout << hex << static_cast<int>(head_ptr[i]) << ",";
+            //     }
+            //     cout << dec << endl;
+            // }
+
+            if (head == -1)
+            {
+                memcpy(&rx_data_buf[0], rx_buf, len);
+                // head_index = len;
+                // rcv_head = true;
+                memcpy(data_length_ptr, &rx_data_buf[8], 4);
+                // cout << "===== data_length = " << data_length << endl;
+
+                if (data_length == 0)
+                {
+
+                    head_index = 0;
+                    rcv_head = false;
+                    // cout << "hear###########################" << endl;
+                }
+                else
+                {
+                    head_index = len;
+                    rcv_head = true;
+                }
+
+                // data_all_length = 12 + data_length + 2;
+                // cout << "===== data_all_length = " << data_all_length << endl;
+                // data_rest_length = data_length - (len - 12) + 2;
+                // cout << "===== data_rest_length = " << data_rest_length << endl;
+            }
+
+            // cout << "=====" << endl;
+            // for (int i = 0; i < len; i++)
+            // {
+            //     cout << hex << static_cast<int>(rx_buf[i]) << ",";
+            // }
+            // cout << dec << endl;
+        }
+    }
+    else
+    {
+        if (len >= 0)
+        {
+            memcpy(&rx_data_buf[head_index], buf, len);
+            head_index = head_index + len;
+            if (head_index >= data_length)
+            {
+
+                // cout << "ok head_index = " << head_index << ", data_length = " << data_length << endl;
+                if (CMD_TYPE == 0x41 && CMD_OP == 0x44 && CMD_WORD == 0x31)
+                {
+                    // cout << "data = " << endl;
+
+                    memcpy(data_count_ptr, &rx_data_buf[68], 4);
+
+                    // cout << "data_count = " << data_count << endl;
+
+                    memcpy(data_buf, &rx_data_buf[72], data_count * 4);
+
+                    memcpy(&start_time, &rx_data_buf[20], sizeof(start_time));
+                    // memcpy(&start_time.year,&rx_data_buf[20+0], sizeof(start_time.year));
+                    // memcpy(&start_time.month,&rx_data_buf[20+sizeof(start_time.year)], sizeof(start_time.month));
+                    // memcpy(&start_time.day,&rx_data_buf[20+sizeof(start_time.month)], sizeof(start_time.month));
+                    // cout<<"year = "<<(short)start_time.year<<endl;
+                    // cout<<"month = "<<(short)start_time.month<<endl;
+                    // cout<<"day = "<<(short)start_time.day<<endl;
+                    // cout<<"hour = "<<(short)start_time.hour<<endl;
+                    // cout<<"min = "<<(short)start_time.min<<endl;
+                    // cout<<"sec = "<<(short)start_time.sec<<endl;
+                    // cout<<"msec = "<<(int)start_time.msec<<endl;
+
+                    memcpy(&end_time, &rx_data_buf[32], sizeof(end_time));
+
+                    ranges.clear();
+                    intensities.clear();
+                    // cout<<"ins = ";
+                    for (int i = 0; i < data_count; i++)
+                    {
+                        // data_buf
+                        int di = i * 4;
+                        int dis_i = di + 2;
+                        // memcpy(dis_ptr, &data_buf[dis_i], 2);
+                        memcpy(dis_ptr, &data_buf[di], 4);
+                        int dis_final = dis >> 12;
+
+
+                        // float dis_data = dis * 0.001f;
+                        float dis_data = dis_final * 0.001f;
+
+                        if(dis_data > max_range_val || dis_data < min_range_val)
+                        {
+                            dis_data = INFINITY;
+                            // cout<<"====="<<endl;
+                        }
+
+
+                        ranges.push_back(dis_data);
+
+                        // cout<<dis_data<<",";
+                        memcpy(&inten_ptr[0], &data_buf[di], 1);
+                        // cout<<(float)inten<<",";
+                        intensities.push_back((float)inten);
+
+                    }
+                    // cout<<endl;
+
+                    double delta_time = (end_time.hour * 3600.0 + end_time.min * 60 + end_time.sec + end_time.msec * 0.001) - (start_time.hour * 3600.0 + start_time.min * 60 + start_time.sec + start_time.msec * 0.001);
+                    // cout<<"delta_time = "<<delta_time<<endl;
+                    double incre_time = delta_time / 1520.0;
+                    // cout<<"incre_time = "<<incre_time<<endl;
+
+                    double scan_time = (end_time.hour * 3600.0 + end_time.min * 60 + end_time.sec + end_time.msec * 0.001) - (last_scan_time.hour * 3600.0 + last_scan_time.min * 60 + last_scan_time.sec + last_scan_time.msec * 0.001);
+                    // cout<<"scan_time = "<<scan_time<<endl;
+                    last_scan_time = end_time;
+
+                    sensor_msgs::LaserScan ls;
+                    ls.header.frame_id = frame_id;
+                    ls.header.stamp = ros::Time::now();
+                    ls.angle_min = degree2ran(-5.0);
+                    ls.angle_max = degree2ran(185.0);
+
+                    ls.angle_increment = degree2ran(0.125);
+                    ls.time_increment = incre_time;
+                    ls.scan_time = scan_time;
+                    ls.range_min = min_range_val;
+                    ls.range_max = max_range_val;
+                    ls.ranges = ranges;
+                    ls.intensities = intensities;
+
+                    scan_pub.publish(ls);
+                }
+
+                head_index = 0;
+                rcv_head = false;
+            }
+        }
+    }
+
+    
+}
+
+int main(int argc, char **argv)
+{
+
+    ros::init(argc, argv, "line_lidar");
+
+    ros::NodeHandle n;
+    ros::NodeHandle nh("~");
+
+    string ip_addr;
+    int port = 99;
+    string topic;
+    
+
+    nh.getParam("ip", ip_addr);
+    nh.getParam("port", port);
+    nh.getParam("topic", topic);
+    nh.getParam("frame_id", frame_id);
+
+    // ROS_INFO("ip was s%", ip_addr);
+    // ROS_INFO("port was %d", port);
+    // ROS_INFO("topic was s%", topic);
+
+    cout << "ip_addr = " << ip_addr << endl;
+    cout << "port = " << port << endl;
+    cout << "topic = " << topic << endl;
+
+    scan_pub = n.advertise<sensor_msgs::LaserScan>(topic, 5);
+
+    async_comm::TCPClient tcp_client(ip_addr, port);
+
+    tcp_client.register_receive_callback(&socket_rcv_callback);
+
+    if (!tcp_client.init())
+    {
+        ROS_ERROR_STREAM("Unable to connect lidar ");
+    }
+    else
+    {
+        ROS_INFO_STREAM("connect lidar ok");
+    }
+
+    ros::Rate loop_rate(1);
+
+    while (ros::ok())
+    {
+
+
+        ros::spinOnce();
+        loop_rate.sleep();
+    }
+    return 0;
+}

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_cx_driver/CMakeLists.txt

@@ -0,0 +1,131 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(lslidar_cx_driver)
+
+set(CMAKE_CXX_STANDARD 14)
+# add_compile_options(-std=c++11)
+add_compile_options(-std=c++14)
+
+set(CMAKE_BUILD_TYPE Release)#RelWithDebInfo
+set(libpcap_LIBRARIES -lpcap)
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+set(${PROJECT_NAME}_CATKIN_DEPS
+    angles
+    pcl_ros
+    roscpp
+    sensor_msgs
+    tf
+    dynamic_reconfigure
+    nodelet
+)
+
+
+find_package(catkin REQUIRED COMPONENTS
+	${${PROJECT_NAME}_CATKIN_DEPS}
+        pcl_conversions
+        rospy
+		roscpp
+		pluginlib
+	    std_msgs
+        genmsg
+        message_generation
+)
+
+find_package(Boost COMPONENTS signals)
+find_package(Boost REQUIRED COMPONENTS thread)
+find_package(PkgConfig REQUIRED)
+
+add_message_files(
+		DIRECTORY msg
+		FILES
+		LslidarPacket.msg
+)
+
+
+add_service_files(
+		FILES
+		lslidar_control.srv
+		motor_control.srv
+		remove_control.srv
+		motor_speed.srv
+		time_service.srv
+		data_port.srv
+		dev_port.srv
+		data_ip.srv
+		destination_ip.srv
+)
+
+
+generate_messages(DEPENDENCIES std_msgs sensor_msgs)
+
+catkin_package(
+    CATKIN_DEPENDS ${${PROJECT_NAME}_CATKIN_DEPS}
+		message_runtime std_msgs
+		roscpp sensor_msgs pluginlib nodelet
+		pcl_ros pcl_conversions
+		DEPENDS
+		Boost
+    )
+
+include_directories(
+		include
+		${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS}
+		${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake)
+add_definitions("-D${PROJECT_NAME}_VERSION=\"${${PROJECT_NAME}_VERSION}\"")
+
+add_library(lslidar_cx_input src/input.cc)
+add_dependencies(lslidar_cx_input
+		${${PROJECT_NAME}_EXPORTED_TARGETS}
+		${catkin_EXPORTED_TARGETS}
+		${PROJECT_NAME}_generate_messages_cpp
+		)
+target_link_libraries(lslidar_cx_input
+		${catkin_LIBRARIES}
+		${libpcap_LIBRARIES})
+
+add_library(lslidar_cx_driver src/lslidar_driver.cpp)
+add_dependencies(lslidar_cx_driver
+		${${PROJECT_NAME}_EXPORTED_TARGETS}
+		${catkin_EXPORTED_TARGETS}
+		${PROJECT_NAME}_generate_messages_cpp
+		)
+target_link_libraries(lslidar_cx_driver
+		lslidar_cx_input
+		${catkin_LIBRARIES})
+
+# build the nodelet version
+add_library(lslidar_cx_driver_nodelet src/lslidar_driver_nodelet.cc src/lslidar_driver.cpp)
+target_link_libraries(lslidar_cx_driver_nodelet
+		lslidar_cx_input
+		${catkin_LIBRARIES}
+		)
+
+add_executable(lslidar_cx_driver_node src/lslidar_driver_node.cpp)
+
+if(catkin_EXPORTED_TARGETS)
+	add_dependencies(lslidar_cx_input ${catkin_EXPORTED_TARGETS})
+endif()
+
+target_link_libraries(lslidar_cx_driver_node
+		lslidar_cx_driver
+		lslidar_cx_input
+		${catkin_LIBRARIES}
+		${libpcap_LIBRARIES}
+		)
+
+
+install(TARGETS lslidar_cx_input lslidar_cx_driver lslidar_cx_driver_nodelet lslidar_cx_driver_node
+		ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+		LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+		RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+		)
+
+
+install(DIRECTORY launch rviz
+		DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+		)
+install(FILES
+		nodelets.xml
+		DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+		)

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_cx_driver/LICENSE

@@ -0,0 +1,13 @@
+Copyright 2022 LeiShen Intelligent
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+    http://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_cx_driver/README.md

@@ -0,0 +1,400 @@
+# LSLIDAR_CX_ROS使用说明
+
+## 1.工程介绍
+​		LSLIDAR_CX_ROS为linux环境下雷达ros驱动，适用于C1, C1Plus, C8,C16,msc16,C32(32度，70度和90度)  4.0版本和n301，5.5版本雷达，程序在ubuntu 20.04 ros noetic，ubuntu18.04 ros melodic以及ubuntu16.04 ros kinetic下测试通过。
+
+## 2.依赖
+
+1.ubuntu20.04 ros noetic/ubuntu18.04 ros melodic/ubuntu16.04 ros kinetic
+
+2.ros依赖
+
+```bash
+# 安装
+sudo apt-get install ros-$ROS_DISTRO-pcl-ros ros-$ROS_DISTRO-pluginlib  ros-$ROS_DISTRO-pcl-conversions  ros-$ROS_DISTRO-diagnostic-updater
+```
+
+3.其他依赖
+
+~~~bash
+sudo apt-get install libpcap-dev
+~~~
+
+
+
+## 3.编译运行
+
+### 3.1 编译
+
+~~~bash
+mkdir -p ~/lidar_ws/src
+#将驱动压缩包解压缩放到~/lidar_ws/src 目录下
+cd ~/lidar_ws
+catkin_make
+source devel/setup.bash
+~~~
+
+### 3.2 运行
+
+运行单个雷达:
+
+~~~bash
+roslaunch lslidar_cx_driver lslidar_cx.launch
+
+~~~
+
+运行多个雷达：
+
+~~~bash
+roslaunch lslidar_cx_driver lslidar_double.launch
+~~~
+
+## 4.参数介绍
+
+lslidar_c32.launch文件内容如下，每个参数含义见注释说明。
+
+~~~bash
+<launch>
+  <arg name="device_ip" default="192.168.1.200" />           #雷达ip
+  <arg name="msop_port" default="2368"/>                     #数据包端口
+  <arg name="difop_port" default="2369"/>                    # 设备包端口
+  <arg name="use_time_service" default="false" />                  #是否给雷达授时
+  <arg name="pcl_type" default="false" />                    # pcl点云类型
+  <arg name="packet_rate" default="1695.0"/>                 #雷达每秒钟发送的数据包的个数，此参数在读取pcap包的时候有用
+
+  <node pkg="lslidar_driver" type="lslidar_driver_node" name="lslidar_driver_node" output="screen">
+    <!--param name="pcap" value="$(find lslidar_cx_driver)/pcap/xxx.pcap" /-->   #pcap包路径，加载pcap包时打开此注释
+    <param name="packet_rate" value="$(arg packet_rate)"/>           
+    <param name="device_ip" value="$(arg device_ip)" />
+    <param name="msop_port" value="$(arg msop_port)" />
+    <param name="difop_port" value="$(arg difop_port)"/>
+    <param name="pcl_type" value="$(arg pcl_type)"/>         #点云类型，默认false点云中的点为xyzirt字段。改为true，点云中的点为xyzi字段。
+    <param name="add_multicast" value="false"/>               #是否添加组播
+    <param name="group_ip" value="224.1.1.2"/>                #组播的ip
+    <param name="use_time_service" value="$(arg use_time_service)"/>
+    <param name="min_range" value="0.15"/>                    # 雷达的最小测量距离
+    <param name="max_range" value="150.0"/>                   # 雷达的最大测量距离
+    <param name="frame_id" value="laser_link"/>               # 雷达点云坐标系
+    <param name="distance_unit" value="0.40"/>                #雷达距离分辨率
+    <param name="angle_disable_min" value="0"/>               #雷达裁剪角度开始值 ，单位0.01°
+    <param name="angle_disable_max" value="0"/>               #雷达裁剪角度结束值，单位0.01°
+    <param name="horizontal_angle_resolution" value="0.18"/>   # 水平角度分辨率 10Hz:0.18  20Hz:0.36 5Hz: 0.09 
+    <param name="scan_num" value="10"/>                       #laserscan线号
+    <param name="read_once" value="false"/>                   #是否重复播放pcap包，  false: 重复播放  true:只播放一次
+    <param name="publish_scan" value="false"/>                #是否发布scan
+    <param name="pointcloud_topic" value="lslidar_point_cloud"/>  # 发布点云话题的名称
+    <param name="coordinate_opt" value="false"/>              #默认false  雷达零度角对应点云方向
+  </node>
+
+  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find lslidar_driver)/rviz_cfg/lslidar.rviz"/>
+  
+ <!--node pkg="tf" type="static_transform_publisher" name="laser_link_to_world" args="0 0 0 0 0 0 world laser_link 100" /-->
+</launch>
+~~~
+
+### 组播模式：
+
+- 上位机设置雷达开启组播模式
+
+- 修改launch文件以下参数
+
+  ~~~shell
+  <param name="add_multicast" value="false"/>               #是否添加组播
+  <param name="group_ip" value="224.1.1.2"/>                #组播的ip
+  ~~~
+
+- 运行以下指令将电脑加入组内（将指令中的enp2s0替换为用户电脑的网卡名,可用ifconfig查看网卡名)
+
+  ~~~shell
+  ifconfig
+  sudo route add -net 224.0.0.0/4 dev enp2s0
+  ~~~
+
+
+
+### 离线pcap模式：
+
+- 把录制好的pcap文件，拷贝到lslidar_cx_driver/pcap文件夹下。
+
+- 修改launch文件以下参数
+
+  ~~~shell
+  #取消注释
+      <param name="pcap" value="$(find lslidar_cx_driver)/pcap/xxx.pcap" />   #pcap包路径，加载pcap包时打开此注释
+  ~~~
+
+###  pcl点云类型：
+
+- 修改launch文件以下参数
+
+  ~~~shell
+  <param name="pcl_type" value="$(arg pcl_type)"/>         #点云类型，默认false点云中的点为xyzirt字段。改为true，点云中的点为xyzi字段。
+  ~~~
+
+  
+
+- 默认false为自定义点云类型，定义参考lslidar_driver/include/lslidar_driver.h头文件
+
+- 改为true,为pcl自带类型 :
+
+  ~~~shell
+  pcl::PointCloud<pcl::PointXYZI>
+  ~~~
+
+
+
+### 修改雷达授时方式：
+
+source devel/setup.bash
+
+GPS授时：
+
+~~~bash
+rosservice call /time_service "time_service_mode: 'gps'
+ntp_ip: ''" 
+~~~
+
+PTP授时：
+
+~~~bash
+rosservice call /time_service "time_service_mode: 'ptp'
+ntp_ip: ''" 
+~~~
+
+NTP授时：
+
+~~~bash
+rosservice call /time_service "time_service_mode: 'ntp'
+ntp_ip: '192.168.1.102'" 
+~~~
+
+
+
+### 雷达上下电(雷达依然转动，只发设备包，不发送数据包)：
+
+source devel/setup.bash
+
+上电：
+
+~~~bash
+rosservice call /lslidar_control "laser_control: 1"
+~~~
+
+下电：
+
+~~~bash
+rosservice call /lslidar_control "laser_control: 0"
+~~~
+
+
+
+### 雷达转动/停止转动(电机停转)：
+
+source devel/setup.bash
+
+转动：
+
+~~~bash
+rosservice call /motor_control "motor_control: 1"
+~~~
+
+停止转动：
+
+~~~bash
+rosservice call /motor_control "motor_control: 0"
+~~~
+
+
+
+### 设置雷达转速：
+
+source devel/setup.bash
+
+可选频率  5Hz/10Hz/20Hz
+
+~~~bash
+rosservice call /set_motor_speed "motor_speed: 20"
+~~~
+
+
+
+### 设置雷达去除雨雾尘等级：
+
+source devel/setup.bash
+
+可选等级  0/1/2/3 ，0-3 数字越大，去除越强
+
+~~~bash
+rosservice call /remove_control "remove_control: 0" 
+~~~
+
+
+
+
+
+### 设置雷达数据包端口
+
+source devel/setup.bash
+
+~~~bash
+rosservice call /set_data_port "data_port: 2368"  #范围[1025,65535]
+~~~
+
+**备注：设置完以后，需要修改launch文件参数，然后重启驱动程序。**
+
+
+
+### 设置雷达设备包端口
+
+source devel/setup.bash
+
+~~~bash
+rosservice call /set_dev_port "dev_port: 2369"  #范围[1025,65535]
+~~~
+
+**备注：设置完以后，需要修改launch文件参数，然后重启驱动程序。**
+
+
+
+### 设置雷达ip
+
+source devel/setup.bash
+
+~~~bash
+rosservice call /set_data_ip "data_ip: '192.168.1.200'"
+~~~
+
+**备注：设置完以后，需要修改launch文件参数，然后重启驱动程序。**
+
+
+
+### 设置雷达目的ip
+
+source devel/setup.bash
+
+~~~bash
+rosservice call /set_destination_ip "destination_ip: '192.168.1.102'"
+~~~
+
+**备注：设置完以后，需要修改launch文件参数，然后重启驱动程序。**
+
+
+
+## FAQ
+
+Bug Report
+
+Original version : LSLIDAR_CX_V4.1.4_220425_ROS
+
+Modify:  original version
+
+Date    : 2022-04-25
+
+--------------------------------------------------------------------
+
+Original version : LSLIDAR_CX_V4.1.5_220620_ROS
+
+Modify:  c8雷达，光学垂直角度修改。
+
+Date    : 2022-06-20
+
+------------------------------------------------------------
+
+Original version : LSLIDAR_CX_V4.2.0_221028_ROS
+
+Modify:  1.新增对4.0版本单线雷达， C32 70度和90度雷达的支持
+
+2.统一laserscan和pointcloud2坐标系
+
+Date    : 2022-10-28
+
+------------------
+
+Original version : LSLIDAR_CX_V4.2.1_221227_ROS
+
+Modify:  1.scan话题新增强度信息
+
+2.fpga升级，C32 90度修改计算公式
+
+3.ROS驱动新增修改授时方式的功能
+
+4.新增雷达上下电,修改雷达ip，端口，转速等功能。
+
+5.修复ntp授时解析问题。
+
+Date    : 2022-12-27
+
+-----
+
+Original version : LSLIDAR_CX_V4.2.2_230322_ROS
+
+Modify:  1.增加使用时长提示
+
+2.新增驱动版本提示
+
+Date    : 2023-03-22
+
+------------
+
+Original version : LSLIDAR_CX_V4.2.3_230403_ROS
+
+Modify:  1.fpga变更，修改C32W的计算公式
+
+Date    : 2023-04-03
+
+------
+
+
+
+Original version : LSLIDAR_CX_V4.2.4_230705_ROS
+
+Modify:  1.修复雷达切换成低功耗模式后，不能切换成正常模式的问题
+
+​               2.兼任C1Plus 型号雷达
+
+Date    : 2023-07-05
+
+-----
+
+
+
+Original version : LSLIDAR_CX_V4.2.5_230913_ROS
+
+Modify:  1.优化代码，降低cpu占用。
+
+​				2.增加nodelet功能。   
+
+​				3. 删除雷达型号参数，解写自动识别雷达型号。
+
+Date    : 2023-09-13
+
+-----
+
+
+
+Original version : LSLIDAR_CX_V4.2.6_231012_ROS
+
+Modify:  1.新增兼容n301,5.5版本。
+
+Date    : 2023-10-12
+
+-----
+
+
+
+Original version : LSLIDAR_CX_V4.2.7_231020_ROS
+
+Modify:  1.增加设置雷达去除雨雾尘等级功能
+
+Date    : 2023-10-20
+
+-----
+
+
+
+
+
+
+
+
+

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_cx_driver/README_en.md

@@ -0,0 +1,406 @@
+# LSLIDAR_CX_ROS
+
+## 1.Introduction
+
+​		LSLIDAR_CX_ROS is the lidar ros driver in linux environment, which is suitable for C1,C1Plus, C8,C16,MSC16 and C32(32 degrees,70 degrees,90 degrees) ，n301 v5.5 lidar. The program has  tested under ubuntu 20.04 ros noetic , ubuntu18.04 ros melodic and ubuntu16.04 ros kinetic.
+
+## 2.Dependencies
+
+1.ros
+
+To run lidar driver in ROS environment, ROS related libraries need to be installed.
+
+**Ubuntu 16.04**: ros-kinetic-desktop-full
+
+**Ubuntu 18.04**: ros-melodic-desktop-full
+
+**Ubuntu 20.04**: ros-noetic-desktop-full
+
+**Installation**: please refer to [http://wiki.ros.org]
+
+2.ros dependencies
+
+```bash
+# install
+sudo apt-get install ros-$ROS_DISTRO-pcl-ros ros-$ROS_DISTRO-pluginlib  ros-$ROS_DISTRO-pcl-conversions  ros-$ROS_DISTRO-diagnostic-updater
+```
+
+3.other dependencies
+
+~~~bash
+sudo apt-get install libpcap-dev 
+sudo apt-get install libboost${BOOST_VERSION}-dev   #Select the appropriate version
+~~~
+
+## 3.Compile && Run
+
+### 3.1 Compile
+
+~~~bash
+mkdir -p ~/lidar_ws/src
+~~~
+
+Copy the whole lidar ROS driver directory into ROS workspace, i.e "~/lidar_ws/src".
+
+~~~bash
+cd ~/lidar_ws
+catkin_make
+source devel/setup.bash
+~~~
+
+3.2 Run
+
+run with single lidar:
+
+~~~bash
+
+roslaunch lslidar_cx_driver lslidar_cx.launch
+
+~~~
+
+run with double lidar:
+
+~~~bash
+roslaunch lslidar_cx_driver lslidar_double.launch
+~~~
+
+
+
+## 4. Introduction to parameters
+
+The content of the lslidar_cx.launch file is as follows, and the meaning of each parameter is shown in the notes.
+
+~~~bash
+<launch>
+  <arg name="device_ip" default="192.168.1.200" />           #lidar ip
+  <arg name="msop_port" default="2368"/>                     # Main data Stream Output Protocol packet port
+  <arg name="difop_port" default="2369"/>                    # Device Information Output Protocol packet port
+  <arg name="use_time_service" default="false" />                  # Whether time synchronization
+  <arg name="pcl_type" default="false" />                    # pointcloud type，false: xyzirt,true:xyzi
+
+  <arg name="packet_rate" default="1695.0"/>                 #The number of data packets sent by the lidar per second. This parameter is useful when reading pcap packets
+
+  <node pkg="lslidar_driver" type="lslidar_driver_node" name="lslidar_driver_node" output="screen">
+    <!--param name="pcap" value="$(find lslidar_driver)/pcap/123.pcap" /-->   #Uncomment to read the data from the pcap file, and add the comment to read the data from the lidar
+    <param name="packet_rate" value="$(arg packet_rate)"/>           
+    <param name="device_ip" value="$(arg device_ip)" />
+    <param name="msop_port" value="$(arg msop_port)" />
+    <param name="difop_port" value="$(arg difop_port)"/>
+    <param name="pcl_type" value="$(arg pcl_type)"/>         # pointcloud type，false: xyzirt,true:xyzi    
+    <param name="add_multicast" value="false"/>               #  Whether to add multicast
+    <param name="group_ip" value="224.1.1.2"/>                 #multicast ip
+    <param name="use_time_service" value="$(arg use_time_service)"/>
+    <param name="min_range" value="0.15"/>                    #Unit: m. The minimum value of the lidar blind area, points smaller than this value are filtered
+    <param name="max_range" value="150.0"/>                   #Unit: m. The maximum value of the lidar blind area, points smaller than this value are filtered
+    <param name="frame_id" value="laser_link"/>                # lidar point cloud coordinate system name
+    <param name="distance_unit" value="0.40"/>                 #lidar range resolution
+    <param name="angle_disable_min" value="0"/>               #lidar clipping angle start value ，unit:0.01°
+    <param name="angle_disable_max" value="0"/>               #lidar clipping angle end value ，unit:0.01°
+     <param name="horizontal_angle_resolution" value="0.18"/>   # Horizontal angle resolution 10Hz:0.18  20Hz:0.36 5Hz: 0.09 
+    <param name="scan_num" value="10"/>                       #laserscan line number
+    <param name="read_once" value="false"/>                   #Whether to play the pcap package repeatedly, false: play it repeatedly true: play it only once
+    <param name="publish_scan" value="false"/>               #Whether to publish the scan
+    <param name="pointcloud_topic" value="lslidar_point_cloud"/>  #point cloud topic name, can be modified
+    <param name="coordinate_opt" value="false"/>             #Default false. The zero degree angle of the lidar corresponds to the direction of the point cloud
+  </node>
+
+  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find lslidar_driver)/rviz_cfg/lslidar.rviz"/>
+  
+ <!--node pkg="tf" type="static_transform_publisher" name="laser_link_to_world" args="0 0 0 0 0 0 world laser_link 100" /-->
+</launch>
+~~~
+
+- ### Multicast mode:
+
+  - The host computer sets the lidar to enable multicast mode
+
+  - Modify the following parameters of the launch file
+
+  ~~~shell
+  <param name="add_multicast" value="true"/>                 # add multicast
+  <param name="group_ip" value="224.1.1.2"/>                 # The multicast ip address set by the host computer
+  ~~~
+
+- Run the following command to add the computer to the group (replace enp2s0 in the command with the network card name of the user's computer, you can use ifconfig to view the network card name)
+
+  ~~~shell
+  ifconfig
+  sudo route add -net 224.0.0.0/4 dev enp2s0
+  ~~~
+
+
+
+- ### Offline pcap mode:
+
+  - Copy the recorded pcap file to the lslidar_cx_driver/pcap folder
+  
+  - Modify the following parameters of the launch file
+  
+  ~~~shell
+  // uncomment
+      <param name="pcap" value="$(find lslidar_cx_driver)/pcap/xxx.pcap" /> 
+  ~~~
+
+- ### pcl point cloud type:
+
+  - Modify the following parameters of the launch file
+
+  ~~~shell
+  <param name="pcl_type" value="$(arg pcl_type)"/>          # pointcloud type，false: xyzirt,true:xyzi
+  ~~~
+
+- The default false is the custom point cloud type, which references the definition in the file of
+
+  lslidar_driver/include/lslidar_driver.h
+
+  Change it to true, which is the own type of pcl:
+
+  ~~~c++
+  pcl::PointCloud<pcl::PointXYZI>
+  ~~~
+
+### Modify lidar time service mode:
+
+source devel/setup.bash
+
+GPS：
+
+~~~bash
+rosservice call /time_service "time_service_mode: 'gps'
+ntp_ip: ''" 
+~~~
+
+PTP：
+
+~~~bash
+rosservice call /time_service "time_service_mode: 'ptp'
+ntp_ip: ''" 
+~~~
+
+NTP：
+
+~~~bash
+rosservice call /time_service "time_service_mode: 'ntp'
+ntp_ip: '192.168.1.102'" 
+~~~
+
+
+
+### lidar power on/off(lidar still rotates,  only send equipment packets):
+
+source devel/setup.bash
+
+power on：
+
+~~~bash
+rosservice call /lslidar_control "laser_control: 1"
+~~~
+
+power off
+
+~~~bash
+rosservice call /lslidar_control "laser_control: 0"
+~~~
+
+
+
+### lidar rotates/stops rotating (motor stops rotating)：
+
+source devel/setup.bash
+
+rotate：
+
+~~~bash
+rosservice call /motor_control "motor_control: 1"
+~~~
+
+stop rotating:
+
+~~~bash
+rosservice call /motor_control "motor_control: 0"
+~~~
+
+
+
+### Set lidar speed:
+
+source devel/setup.bash
+
+Optional frequency  5Hz/10Hz/20Hz
+
+~~~bash
+rosservice call /set_motor_speed "motor_speed: 20"
+~~~
+
+
+
+### Set lidar  to remove rain, fog, and dust levels:
+
+source devel/setup.bash
+
+Optional level 0/1/2/3, the larger the number from 0 to 3, the stronger the removal
+
+~~~bash
+rosservice call /remove_control "remove_control: 0" 
+~~~
+
+
+
+### Set lidar data packet port
+
+source devel/setup.bash
+
+~~~bash
+rosservice call /set_data_port "data_port: 2368"  #range [1025,65535]
+~~~
+
+**Note: After setting, you need to modify the launch file parameters and restart the driver.**
+
+
+
+### Set lidar equipment packet port
+
+source devel/setup.bash
+
+~~~bash
+rosservice call /set_dev_port "dev_port: 2369"  #range[1025,65535]
+~~~
+
+**Note: After setting, you need to modify the launch file parameters and restart the driver.**
+
+
+
+### Set lidar ip
+
+source devel/setup.bash
+
+~~~bash
+rosservice call /set_data_ip "data_ip: '192.168.1.200'"
+~~~
+
+**Note: After setting, you need to modify the launch file parameters and restart the driver.**
+
+
+
+### Set lidar destination ip
+
+source devel/setup.bash
+
+~~~bash
+rosservice call /set_destination_ip "destination_ip: '192.168.1.102'"
+~~~
+
+**Note: After setting, you need to modify the launch file parameters and restart the driver.**
+
+
+
+## FAQ
+
+Bug Report
+
+Original version : LSLIDAR_CX_V4.1.4_220425_ROS
+
+Modify:  original version
+
+Date    : 2022-04-25
+
+--------------------------------------------------------------------
+
+Original version : LSLIDAR_CX_V4.1.5_220620_ROS
+
+Modify: Modify the optical vertical angle.
+
+Date    : 2022-06-20
+
+------------------------------------------------------------
+
+Original version : LSLIDAR_CX_V4.2.0_221028_ROS
+
+Modify:  1. Added support for version 4.0 single line lidar and C32 70 degree and 90 degree lidars
+
+2.Unify laserscan and pointcloud2 coordinate systems
+
+Date    : 2022-10-28
+
+-------------------
+
+Original version : LSLIDAR_CX_V4.2.1_221227_ROS
+
+Modify: 1. Scan topic adds strength information
+
+2. fpga upgrade, C32 90 degree modification of calculation formula
+3. ROS driver adds the function of modifying time service mode
+3. New function to modify lidar configuration
+3. Fixed the ntp timing resolution problem.
+
+Date    : 2022-12-27
+
+----------------------
+
+Original version : LSLIDAR_CX_V4.2.2_230322_ROS
+
+Modify:1.Prompt for usage duration
+
+2.Driver version prompt
+
+Date    : 2023-03-22
+
+--------------
+
+Original version : LSLIDAR_CX_V4.2.3_230403_ROS
+
+Modify:  1. Change the fpga protocol and modify the calculation formula of C32W
+
+Date    : 2023-04-03
+
+------
+
+
+
+Original version : LSLIDAR_CX_V4.2.4_230705_ROS
+
+Modify:  1.Fix the issue of lidar switching to low power mode and not being able to switch to normal mode
+
+​               2. Compatible with C1Plus lidar models  
+
+Date    : 2023-07-05
+
+---
+
+
+
+Original version : LSLIDAR_CX_V4.2.5_230913_ROS
+
+Modify: 
+
+1. Optimize the code to reduce CPU usage.
+2. Add nodelet function.
+2.  Delete the radar model parameters and rewrite the automatic recognition radar model.
+
+Date    : 2023-09-13
+
+---
+
+
+
+Original version : LSLIDAR_CX_V4.2.6_231012_ROS
+
+Modify: 
+
+1. Compatible with n301 lidar models  
+
+Date    : 2023-10-12
+
+---
+
+
+
+Original version : LSLIDAR_CX_V4.2.7_231020_ROS
+
+Modify: 
+
+1.Add the function of setting radar to remove rain, fog, and dust levels
+
+Date    : 2023-10-20
+
+---

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_cx_driver/include/lslidar_driver/input.h

@@ -0,0 +1,117 @@
+/******************************************************************************
+ * This file is part of lslidar_cx driver.
+ *
+ * Copyright 2022 LeiShen Intelligent Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#ifndef __LSLIDAR_INPUT_H_
+#define __LSLIDAR_INPUT_H_
+
+
+
+#include <unistd.h>
+#include <stdio.h>
+#include <pcap.h>
+#include <netinet/in.h>
+#include <ros/ros.h>
+#include <string>
+#include <sstream>
+#include <sys/socket.h>
+#include <arpa/inet.h>
+#include <poll.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <sys/file.h>
+#include <signal.h>
+#include <sensor_msgs/TimeReference.h>
+#include "lslidar_cx_driver/LslidarPacket.h"
+
+
+namespace lslidar_driver {
+    static uint16_t MSOP_DATA_PORT_NUMBER = 2368;   // lslidar default data port on PC
+    static uint16_t DIFOP_DATA_PORT_NUMBER = 2369;  // lslidar default difop data port on PC
+/**
+ *  从在线的网络数据或离线的网络抓包数据（pcap文件）中提取出lidar的原始数据，即packet数据包
+ * @brief The Input class,
+     *
+     * @param private_nh  一个NodeHandled,用于通过节点传递参数
+     * @param port
+     * @returns 0 if successful,
+     *          -1 if end of file
+     *          >0 if incomplete packet (is this possible?)
+ */
+    class Input {
+    public:
+        Input(ros::NodeHandle private_nh, uint16_t port , int packet_size);
+
+        virtual ~Input() {
+        }
+
+        virtual int getPacket(lslidar_cx_driver::LslidarPacketPtr &pkt) = 0;
+
+    protected:
+        ros::NodeHandle private_nh_;
+        uint16_t port_;
+        std::string devip_str_;
+        int cur_rpm_;
+        int return_mode_;
+        bool npkt_update_flag_;
+        bool add_multicast;
+        std::string group_ip;
+        int packet_size_;
+    };
+
+/** @brief Live lslidar input from socket. */
+    class InputSocket : public Input {
+    public:
+        InputSocket(ros::NodeHandle private_nh, uint16_t port = MSOP_DATA_PORT_NUMBER,int packet_size = 1212);
+
+        virtual ~InputSocket();
+
+        virtual int getPacket(lslidar_cx_driver::LslidarPacketPtr &pkt);
+
+    private:
+        int sockfd_;
+        in_addr devip_;
+
+    };
+
+/** @brief lslidar input from PCAP dump file.
+   *
+   * Dump files can be grabbed by libpcap
+   */
+    class InputPCAP : public Input {
+    public:
+        InputPCAP(ros::NodeHandle private_nh, uint16_t port = MSOP_DATA_PORT_NUMBER, int packet_size = 1212, double packet_rate = 0.0,
+                  std::string filename = "", bool read_once = false, bool read_fast = false, double repeat_delay = 0.0);
+
+        virtual ~InputPCAP();
+
+        virtual int getPacket(lslidar_cx_driver::LslidarPacketPtr &pkt);
+
+    private:
+        ros::Rate packet_rate_;
+        std::string filename_;
+        pcap_t *pcap_;
+        bpf_program pcap_packet_filter_;
+        char errbuf_[PCAP_ERRBUF_SIZE];
+        bool empty_;
+        bool read_once_;
+        bool read_fast_;
+        double repeat_delay_;
+    };
+}
+
+#endif  // __LSLIDAR_INPUT_H

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_cx_driver/include/lslidar_driver/lslidar_driver.h

@@ -0,0 +1,341 @@
+/******************************************************************************
+ * This file is part of lslidar_cx driver.
+ *
+ * Copyright 2022 LeiShen Intelligent Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+#ifndef _LS_C16_DRIVER_H_
+#define _LS_C16_DRIVER_H_
+
+#define DEG_TO_RAD 0.017453f
+#define RAD_TO_DEG 57.29577f
+
+class Request;
+
+#include "input.h"
+
+#include <string>
+#include <ros/ros.h>
+#include <ros/package.h>
+#include <std_msgs/Int32.h>
+#include <pcl/point_types.h>
+#include <pcl_ros/impl/transforms.hpp>
+#include <pcl_conversions/pcl_conversions.h>
+#include <boost/shared_ptr.hpp>
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/LaserScan.h>
+#include <pcl_ros/point_cloud.h>
+#include <regex>
+#include <atomic>
+#include <thread>
+#include <memory>
+#include <mutex>
+#include <algorithm>
+#include "lslidar_cx_driver/lslidar_control.h"
+#include "lslidar_cx_driver/time_service.h"
+#include "lslidar_cx_driver/motor_speed.h"
+#include "lslidar_cx_driver/motor_control.h"
+#include "lslidar_cx_driver/remove_control.h"
+#include "lslidar_cx_driver/data_port.h"
+#include "lslidar_cx_driver/dev_port.h"
+#include "lslidar_cx_driver/data_ip.h"
+#include "lslidar_cx_driver/destination_ip.h"
+
+
+namespace lslidar_driver {
+//raw lslidar packet constants and structures
+    static const int SIZE_BLOCK = 100;
+    static const int RAW_SCAN_SIZE = 3;
+    static const int SCANS_PER_BLOCK = 32;
+    static const int BLOCK_DATA_SIZE = SCANS_PER_BLOCK * RAW_SCAN_SIZE;
+    static const float DISTANCE_RESOLUTION = 0.01f; //meters
+    static const uint16_t UPPER_BANK = 0xeeff;
+
+// special defines for lslidarlidar support
+    static const int FIRINGS_PER_BLOCK = 2;
+    static const int SCANS_PER_FIRING = 16;
+    static const int SCANS_PER_FIRING_CX = 32;
+
+    static const int FIRING_TOFFSET = 32;
+    //  static const int PACKET_SIZE = 1212;
+    static const int BLOCKS_PER_PACKET = 12;
+    static const int FIRINGS_PER_PACKET_CX = BLOCKS_PER_PACKET;
+    static const int SCANS_PER_PACKET = SCANS_PER_FIRING * FIRINGS_PER_BLOCK * BLOCKS_PER_PACKET; //384
+    //unit:meter
+    static const float R1_ = 0.0361f;        // C16 4.0 or C32 4.0
+    static const float R1_C32W = 0.03416f;   // 新C32W   byte[1211] = 0x47
+    static const float R1_90 = 0.0209f;   // 90度
+    static const int conversionAngle_ = 2025;
+    static const int conversionAngle_90 = 2776; // 90度
+// Pre-compute the sine and cosine for the altitude angles.
+    //c32 32度
+    static const float c32_vertical_angle[32] = {-16.0f, -8.0f, 0.0f, 8.0f, -15.0f, -7.0f, 1.0f, 9.0f,
+                                                 -14.0f, -6.0f, 2.0f, 10.0f, -13.0f, -5.0f, 3.0f, 11.0f,
+                                                 -12.0f, -4.0f, 4.0f, 12.0f, -11.0f, -3.0f, 5.0f, 13.0f,
+                                                 -10.0f, -2.0f, 6.0f, 14.0f, -9.0f, -1.0f, 7.0f, 15.0f};
+
+    //c32 70度  0x45
+    static const float c32wa_vertical_angle[32] = {
+            -51.0f, -31.0f, -9.0f, 3.0f, -48.5f, -28.0f, -7.5f, 4.5f,
+            -46.0f, -25.0f, -6.0f, 6.0f, -43.5f, -22.0f, -4.5f, 7.5f,
+            -41.0f, -18.5f, -3.0f, 9.0f, -38.5f, -15.0f, -1.5f, 11.0f,
+            -36.0f, -12.0f, 0.0f, 13.0f, -33.5f, -10.5f, 1.5f, 15.0f};
+
+    //c32 70度  0x46
+    static const float c32_70_vertical_angle[32] = {
+            -54.0f, -31.0f, -8.0f, 2.66f, -51.5f, -28.0f, -6.66f, 4.0f,
+            -49.0f, -25.0f, -5.33f, 5.5f, -46.0f, -22.0f, -4.0f, 7.0f,
+            -43.0f, -18.5f, -2.66f, 9.0f, -40.0f, -15.0f, -1.33f, 11.0f,
+            -37.0f, -12.0f, 0.0f, 13.0f, -34.0f, -10.0f, 1.33f, 15.0f};
+
+    //c32 70度  0x47
+    static const float c32_70_vertical_angle2[32] = {
+            -54.7f, -31.0f, -9.0f, 3.0f, -51.5f, -28.0f, -7.5f, 4.5f,
+            -49.0f, -25.0f, -6.0f, 6.0f, -46.0f, -22.0f, -4.5f, 7.5f,
+            -43.0f, -18.5f, -3.0f, 9.0f, -40.0f, -15.0f, -1.5f, 11.0f,
+            -37.0f, -12.0f, 0.0f, 13.0f, -34.0f, -10.5f, 1.5f, 15.0f};
+
+
+    //c32 90度
+    static const float c32_90_vertical_angle[32] = {
+            2.487f, 25.174f, 47.201f, 68.819f, 5.596f, 27.811f, 49.999f, 71.525f,
+            8.591f, 30.429f, 52.798f, 74.274f, 11.494f, 33.191f, 55.596f, 77.074f,
+            14.324f, 36.008f, 58.26f, 79.938f, 17.096f, 38.808f, 60.87f, 82.884f,
+            19.824f, 41.603f, 63.498f, 85.933f, 22.513f, 44.404f, 66.144f, 89.105f};
+
+//    static const double c32_70_vertical_angle[32] = {-55, -30, -8, 2.66, -51.5, -27, -6.65, 3.99, -48, -24, -5.32, 5.5,
+//                                                     -45, -21, -3.99, 7, -42, -18, -2.66, 9, -39, -15, -1.33, 11, -36,
+//                                                     -12.5, 0, 13, -33, -10, 1.33, 15};
+//2°
+    static float c16_vertical_angle[16] = {-16.0f, 0.0f, -14.0f, 2.0f, -12.0f, 4.0f, -10.0f, 6.0f, -8.0f, 8.0f,
+                                           -6.0f, 10.0f, -4.0f, 12.0f, -2.0f, 14.0f};
+    static const int c16_remap_angle[16] = {0, 2, 4, 6, 8, 10, 12, 14, 1, 3, 5, 7, 9, 11, 13, 15};
+    // static const double c8_vertical_angle[8] = {-12, 0, -10, 4, -8, 8, -4, 10};
+    //static const double c8_vertical_angle[8] = {-10, 4, -8, 8, -4, 10, 0, 12};
+    static const float c8_vertical_angle[8] = {-12.0f, 4.0f, -8.0f, 8.0f, -4.0f, 10.0f, 0.0f, 12.0f};
+
+    //static const float c8f_vertical_angle[8] = {-2.0f, 0.0f, 2.0f, 4.0f, 6.0f, 8.0f, 10.0f, 12.0f};
+    static const float c8f_vertical_angle[8] = {-2.0f, 6.0f, 0.0f, 8.0f, 2.0f, 10.0f, 4.0f, 12.0f};
+    // static const float c8s_vertical_angle[8] = {-12.0f, -8.0f, -4.0f, 0.0f, 4.0f, 8.0f, 10.0f, 12.0f};
+    static const float c8s_vertical_angle[8] = {-12.0f, 4.0f, -8.0f, 8.0f, -4.0f, 10.0f, 0.0f, 12.0f};
+
+    static const float c1_vertical_angle[8] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
+
+
+    union TwoBytes {
+        uint16_t distance;
+        uint8_t bytes[2];
+    };
+
+    struct RawBlock {
+        uint16_t header;
+        uint16_t rotation;  //0-35999
+        uint8_t data[BLOCK_DATA_SIZE];
+    };
+
+    struct RawPacket {
+        RawBlock blocks[BLOCKS_PER_PACKET];
+        uint8_t time_stamp[10];
+        uint8_t factory[2];
+    };
+
+    struct FiringCX {
+        uint16_t firing_azimuth[FIRINGS_PER_PACKET_CX];
+        uint16_t azimuth[SCANS_PER_PACKET];
+        float distance[SCANS_PER_PACKET];
+        float intensity[SCANS_PER_PACKET];
+    };
+
+    struct PointXYZIRT {
+        float x;
+        float y;
+        float z;
+        float intensity;
+        std::uint16_t ring;
+        float time;
+
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW //make sure our new allocators are aligned
+    }EIGEN_ALIGN16; //enforce SSE padding for correct memory alignment
+
+    static std::string lidar_type;
+
+    class LslidarDriver {
+    public:
+        LslidarDriver(ros::NodeHandle &node, ros::NodeHandle &private_nh);
+
+        ~LslidarDriver() {}
+
+        bool checkPacketValidity(lslidar_cx_driver::LslidarPacketPtr &packet);
+
+        //check if a point is in the required range
+        bool isPointInRange(const double &distance);
+
+        bool loadParameters();
+
+        void initTimeStamp();
+
+        bool createRosIO();
+
+        void difopPoll();
+
+        void publishPointcloud();
+
+        void publishScan();
+
+        bool powerOn(lslidar_cx_driver::lslidar_control::Request &req,
+                     lslidar_cx_driver::lslidar_control::Response &res);
+
+        bool motorControl(lslidar_cx_driver::motor_control::Request &req,
+                          lslidar_cx_driver::motor_control::Response &res);
+
+        bool removeControl(lslidar_cx_driver::remove_control::Request &req,
+                          lslidar_cx_driver::remove_control::Response &res);
+
+        bool motorSpeed(lslidar_cx_driver::motor_speed::Request &req,
+                        lslidar_cx_driver::motor_speed::Response &res);
+
+        bool timeService(lslidar_cx_driver::time_service::Request &req,
+                         lslidar_cx_driver::time_service::Response &res);
+
+        bool setDataPort(lslidar_cx_driver::data_port::Request &req,
+                         lslidar_cx_driver::data_port::Response &res);
+
+        bool setDevPort(lslidar_cx_driver::dev_port::Request &req,
+                        lslidar_cx_driver::dev_port::Response &res);
+
+        bool setDataIp(lslidar_cx_driver::data_ip::Request &req,
+                       lslidar_cx_driver::data_ip::Response &res);
+
+        bool setDestinationIp(lslidar_cx_driver::destination_ip::Request &req,
+                              lslidar_cx_driver::destination_ip::Response &res);
+
+        static void setPacketHeader(unsigned char *config_data);
+
+        bool sendPacketTolidar(unsigned char *config_data) const;
+
+        //void decodePacket(const RawPacket* &packet);
+        void decodePacket(lslidar_cx_driver::LslidarPacketPtr &packet);
+
+        bool poll();
+
+        void pointcloudToLaserscan(const sensor_msgs::PointCloud2 &cloud_msg, sensor_msgs::LaserScan &output_scan);
+
+        bool initialize();
+
+    public:
+        int msop_udp_port{};
+        int difop_udp_port{};
+        int scan_num{};
+        int point_num{};
+        int angle_disable_min{};
+        int angle_disable_max{};
+        uint16_t last_azimuth;
+        uint64_t packet_time_s{};
+        uint64_t packet_time_ns{};
+        int return_mode;
+
+        in_addr lidar_ip{};
+        std::string lidar_ip_string;
+        std::string group_ip_string;
+        std::string frame_id;
+        std::string dump_file;
+        std::string pointcloud_topic;
+
+        bool use_time_service{};
+        bool pcl_type{};
+        bool publish_scan{};
+        bool coordinate_opt{};
+        bool is_first_sweep;
+        bool add_multicast{};
+        // std::string c32_type;
+        double distance_unit{};
+        double min_range{};
+        double max_range{};
+        double sweep_end_time;
+        double angle_base{};
+        float cos_azimuth_table[36000]{};
+        float sin_azimuth_table[36000]{};
+
+        boost::shared_ptr<Input> msop_input_;
+        boost::shared_ptr<Input> difop_input_;
+        boost::shared_ptr<boost::thread> difop_thread_;
+
+        //lslidar_driver::LslidarScanPtr sweep_data;
+        //lslidar_driver::LslidarScanPtr sweep_data_bak;
+        std::mutex pointcloud_lock;
+        ros::NodeHandle nh;
+        ros::NodeHandle pnh;
+        //ros::Publisher packet_pub;
+        ros::Publisher pointcloud_pub;
+        ros::Publisher scan_pub;
+        ros::ServiceServer time_service_;               //授时
+        ros::ServiceServer lslidar_control_service_;    //上下电
+        ros::ServiceServer motor_control_service_;      //雷达转动/停转
+        ros::ServiceServer remove_control_service_;      //雷达去除雨雾尘级别
+        ros::ServiceServer motor_speed_service_;        //雷达频率
+        ros::ServiceServer data_port_service_;          //数据包端口
+        ros::ServiceServer dev_port_service_;           //设备包端口
+        ros::ServiceServer data_ip_service_;            //数据包ip
+        ros::ServiceServer destination_ip_service_;     //设备包ip
+
+        unsigned char difop_data[1206]{};
+        unsigned char packetTimeStamp[10]{};
+        struct tm cur_time{};
+        ros::Time timeStamp;
+        double packet_rate;
+        double current_packet_time;
+        double last_packet_time;
+        double current_point_time;
+        double last_point_time;
+        FiringCX firings{};
+        float scan_altitude[32] = {0.0f};
+        float cos_scan_altitude[32] = {0.0f};
+        float sin_scan_altitude[32] = {0.0f};
+        float msc16_adjust_angle[16] = {0.0f};
+        float msc16_offset_angle[16] = {0.0f};
+        double horizontal_angle_resolution;
+        int lidar_number_;
+        std::atomic<bool> is_get_difop_{false};
+        std::atomic<int> time_service_mode_{0};
+        int remove_rain_flag;
+        bool start_process_msop_;
+        bool is_new_c32w_;
+        bool is_get_scan_altitude_;
+        bool is_msc16;
+        int config_vertical_angle_flag;
+        pcl::PointCloud<PointXYZIRT>::Ptr point_cloud_xyzirt_;
+        pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_xyzi_;
+        pcl::PointCloud<PointXYZIRT>::Ptr point_cloud_xyzirt_bak_;
+        pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_xyzi_bak_;
+        sensor_msgs::LaserScan::Ptr scan_msg;
+        sensor_msgs::LaserScan::Ptr scan_msg_bak;
+        uint point_size;
+
+    };
+
+    typedef PointXYZIRT VPoint;
+    typedef pcl::PointCloud<VPoint> VPointcloud;
+
+
+}  // namespace lslidar_driver
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(lslidar_driver::PointXYZIRT,
+                                  (float, x, x)
+                                          (float, y, y)
+                                          (float, z, z)
+                                          (float, intensity, intensity)
+                                          (std::uint16_t, ring, ring)
+                                          (float, time, time))
+
+#endif

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_cx_driver/launch/lslidar_c32.launch

@@ -0,0 +1,38 @@
+<launch>
+  <arg name="device_ip" default="192.168.3.100" />
+  <arg name="msop_port" default="2368"/>
+  <arg name="difop_port" default="2369"/>
+  <arg name="use_time_service" default="false" />
+  <arg name="pcl_type" default="false" />
+  <arg name="packet_rate" default="1695.0"/>
+
+  <node pkg="lslidar_cx_driver" type="lslidar_cx_driver_node" name="lslidar_driver_node" output="screen">
+    <!--param name="pcap" value="$(find lslidar_driver)/pcap/xxx.pcap" /-->
+    <param name="packet_rate" value="$(arg packet_rate)"/>
+    <param name="device_ip" value="$(arg device_ip)" />
+    <param name="msop_port" value="$(arg msop_port)" />
+    <param name="difop_port" value="$(arg difop_port)"/>
+    <param name="pcl_type" value="$(arg pcl_type)"/>
+    <param name="add_multicast" value="false"/>
+    <param name="group_ip" value="224.1.1.2"/>
+    <param name="use_time_service" value="$(arg use_time_service)"/>
+    <!--<param name="min_range" value="0.15"/>-->
+    <param name="min_range" value="3"/>
+    <param name="max_range" value="200.0"/>
+    <param name="frame_id" value="laser_link"/>
+    <param name="distance_unit" value="0.40"/>
+    <param name="angle_disable_min" value="0"/>
+    <param name="angle_disable_max" value="0"/>
+    <param name="horizontal_angle_resolution" value="0.18"/>  <!--10Hz:0.18  20Hz:0.36 5Hz:0.09  -->
+    <param name="scan_num" value="15"/>
+    <param name="read_once" value="false"/>
+    <param name="publish_scan" value="false"/>
+    <param name="pointcloud_topic" value="lslidar32_point_cloud"/>
+    <param name="coordinate_opt" value="false"/>
+  </node>
+
+  <!-- <node pkg="rviz" type="rviz" name="rviz" args="-d $(find lslidar_cx_driver)/rviz/lslidar.rviz"/> -->
+ 
+ <!--node pkg="tf" type="static_transform_publisher" name="laser_link_to_world" args="0 0 0 0 0 0 world laser_link 100" /-->
+  
+</launch>

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_cx_driver/launch/lslidar_double.launch

@@ -0,0 +1,69 @@
+<launch>
+  <arg name="device_ip1" default="192.168.3.100" />
+  <arg name="msop_port1" default="2368"/>
+  <arg name="difop_port1" default="2369"/>
+  <!-- C16 -->
+  <arg name="device_ip2" default="192.168.3.101" />
+  <arg name="msop_port2" default="2367"/>
+  <arg name="difop_port2" default="2370"/>
+  <arg name="pcl_type" default="false" />
+  <arg name="use_time_service" default="false" />
+  <arg name="packet_rate1" default="1695.0"/>
+  <arg name="packet_rate2" default="1695.0"/>
+
+  <node pkg="lslidar_cx_driver" type="lslidar_cx_driver_node" name="lslidar_driver_node" output="screen" ns="lidar_1">
+    <!--param name="pcap" value="$(find lslidar_cx_driver)/pcap/xxx.pcap" /-->
+    <param name="packet_rate" value="$(arg packet_rate1)"/>
+    <param name="device_ip" value="$(arg device_ip1)" />
+    <param name="msop_port" value="$(arg msop_port1)" />
+    <param name="difop_port" value="$(arg difop_port1)"/>
+    <param name="pcl_type" value="$(arg pcl_type)"/>
+    <param name="add_multicast" value="false"/>
+    <param name="group_ip" value="224.1.1.2"/>
+    <param name="use_time_service" value="$(arg use_time_service)"/>
+    <param name="min_range" value="0.15"/>
+    <param name="max_range" value="200.0"/>
+    <param name="frame_id" value="laser_link_c16"/>
+    <param name="distance_unit" value="0.4"/>
+    <param name="angle_disable_min" value="0"/>
+    <param name="angle_disable_max" value="0"/>
+    <param name="horizontal_angle_resolution" value="0.18"/>  <!--10Hz:0.18  20Hz:0.36 5Hz: 0.09  -->
+    <param name="scan_num" value="10"/>
+    <param name="read_once" value="false"/>
+    <param name="publish_scan" value="false"/>
+    <param name="pointcloud_topic" value="lslidar_point_cloud"/>
+    <param name="coordinate_opt" value="false"/>
+  </node>
+
+    <node pkg="lslidar_cx_driver" type="lslidar_cx_driver_node" name="lslidar_driver_node" output="screen" ns="lidar_2">
+      <!--param name="pcap" value="$(find lslidar_cx_driver)/pcap/xxx.pcap" /-->
+      <param name="packet_rate" value="$(arg packet_rate2)"/>
+      <param name="device_ip" value="$(arg device_ip2)" />
+      <param name="msop_port" value="$(arg msop_port2)" />
+      <param name="difop_port" value="$(arg difop_port2)"/>
+      <param name="pcl_type" value="$(arg pcl_type)"/>
+      <param name="add_multicast" value="false"/>
+      <param name="group_ip" value="224.1.1.2"/>
+      <param name="use_time_service" value="$(arg use_time_service)"/>
+      <param name="min_range" value="0.15"/>
+      <param name="max_range" value="200.0"/>
+      <param name="frame_id" value="laser_link_c32"/>
+      <param name="distance_unit" value="0.40"/>
+      <param name="angle_disable_min" value="0"/>
+      <param name="angle_disable_max" value="0"/>
+      <param name="horizontal_angle_resolution" value="0.18"/>  <!--10Hz:0.18  20Hz:0.36 5Hz: 0.09  -->
+      <param name="scan_num" value="10"/>
+      <param name="read_once" value="false"/>
+      <param name="publish_scan" value="false"/>
+      <param name="pointcloud_topic" value="lslidar_point_cloud"/>
+      <param name="coordinate_opt" value="false"/>
+    </node>
+
+  <!--node pkg="rviz" type="rviz" name="rviz" args="-d $(find lslidar_driver)/rviz_cfg/lslidar.rviz"/-->
+  <!--node pkg="tf" type="static_transform_publisher" name="laser_link_right_to_world" args="0 0 0 0 0 0 world laser_link_c16 100" /-->
+  <!--node pkg="tf" type="static_transform_publisher" name="laser_link_left_to_world" args="0 0 1 0 0 0 world laser_link_c32 100" /-->
+
+
+	
+ 
+</launch>

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_cx_driver/launch/nodelet_cx.launch

@@ -0,0 +1,38 @@
+
+<launch>
+  <arg name="device_ip" default="192.168.1.201" />
+  <arg name="msop_port" default="2368"/>
+  <arg name="difop_port" default="2369"/>
+  <arg name="use_time_service" default="false" />
+  <arg name="pcl_type" default="false" />
+  <arg name="packet_rate" default="1695.0"/>
+
+    <!-- nodelet manager -->
+
+     <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" output="screen" />
+
+      <node pkg="nodelet" type="nodelet" name="lslidar_cx_driver_nodelet"
+        args="load lslidar_driver/DriverNodelet nodelet_manager" output="screen" respawn="true">
+    <param name="packet_rate" value="$(arg packet_rate)"/>
+    <param name="device_ip" value="$(arg device_ip)" />
+    <param name="msop_port" value="$(arg msop_port)" />
+    <param name="difop_port" value="$(arg difop_port)"/>
+    <param name="pcl_type" value="$(arg pcl_type)"/>
+    <param name="add_multicast" value="false"/>
+    <param name="group_ip" value="224.1.1.2"/>
+    <param name="use_time_service" value="$(arg use_time_service)"/>
+    <param name="min_range" value="0.15"/>
+    <param name="max_range" value="200.0"/>
+    <param name="frame_id" value="laser_link"/>
+    <param name="distance_unit" value="0.40"/>
+    <param name="angle_disable_min" value="0"/>
+    <param name="angle_disable_max" value="0"/>
+    <param name="horizontal_angle_resolution" value="0.18"/>  <!--10Hz:0.18  20Hz:0.36 5Hz:0.09  -->
+    <param name="scan_num" value="15"/>
+    <param name="read_once" value="false"/>
+    <param name="publish_scan" value="false"/>
+    <param name="pointcloud_topic" value="lslidar_point_cloud"/>
+    <param name="coordinate_opt" value="false"/>
+  </node>
+
+</launch>

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_cx_driver/launch/test.launch

@@ -0,0 +1,37 @@
+<launch>
+  <arg name="device_ip" default="192.168.1.200" />
+  <arg name="msop_port" default="2368"/>
+  <arg name="difop_port" default="2369"/>
+  <arg name="use_time_service" default="false" />
+  <arg name="pcl_type" default="false" />
+  <arg name="packet_rate" default="1695.0"/>
+
+  <node pkg="lslidar_cx_driver" type="lslidar_cx_driver_node" name="lslidar_driver_node" output="screen">
+    <!--param name="pcap" value="$(find lslidar_cx_driver)/pcap/xxx.pcap" /-->
+    <param name="packet_rate" value="$(arg packet_rate)"/>
+    <param name="device_ip" value="$(arg device_ip)" />
+    <param name="msop_port" value="$(arg msop_port)" />
+    <param name="difop_port" value="$(arg difop_port)"/>
+    <param name="pcl_type" value="$(arg pcl_type)"/>
+    <param name="add_multicast" value="false"/>
+    <param name="group_ip" value="224.1.1.2"/>
+    <param name="use_time_service" value="$(arg use_time_service)"/>
+    <param name="min_range" value="0.15"/>
+    <param name="max_range" value="200.0"/>
+    <param name="frame_id" value="laser_link"/>
+    <param name="distance_unit" value="0.40"/>
+    <param name="angle_disable_min" value="0"/>
+    <param name="angle_disable_max" value="0"/>
+    <param name="horizontal_angle_resolution" value="0.18"/>  <!--10Hz:0.18  20Hz:0.36 5Hz:0.09  -->
+    <param name="scan_num" value="16"/>
+    <param name="read_once" value="false"/>
+    <param name="publish_scan" value="false"/>
+    <param name="pointcloud_topic" value="lslidar_point_cloud"/>
+    <param name="coordinate_opt" value="true"/>
+  </node>
+
+  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find lslidar_cx_driver)/rviz/lslidar.rviz"/>
+ 
+ <!--node pkg="tf" type="static_transform_publisher" name="laser_link_to_world" args="0 0 0 0 0 0 world laser_link 100" /-->
+  
+</launch>

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_cx_driver/msg/LslidarPacket.msg

@@ -0,0 +1,5 @@
+# Raw Leishen LIDAR packet.
+
+time stamp              # packet timestamp
+uint8[1212] data        # packet contents
+

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_cx_driver/msg/LslidarScan.msg

@@ -0,0 +1,6 @@
+# Altitude of all the points within this scan
+float64 altitude
+
+# The valid points in this scan sorted by azimuth
+# from 0 to 359.99
+LslidarPoint[] points

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_cx_driver/nodelets.xml

@@ -0,0 +1,9 @@
+<library path="lib/liblslidar_cx_driver_nodelet">
+  <class name="lslidar_driver/DriverNodelet"
+         type="lslidar_driver::DriverNodelet"
+         base_class_type="nodelet::Nodelet">
+    <description>
+      Publish one lslidar raw data packet each time.
+    </description>
+  </class>
+</library>

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_cx_driver/package.xml

@@ -0,0 +1,58 @@
+<?xml version="1.0"?>
+<package>
+  <name>lslidar_cx_driver</name>
+  <version>4.2.7</version>
+
+  <maintainer email="lslidar@sina.com">lslidar</maintainer>
+  <author>lslidar</author>
+  <license>BSD</license>
+  <description>
+    ROS device driver for Leishen CX 4.0 lidar.
+  </description>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>angles</build_depend>
+  <build_depend>nodelet</build_depend>
+  <build_depend>pcl_conversions</build_depend>
+  <build_depend>pcl_ros</build_depend>
+  <build_depend>pluginlib</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
+  <build_depend>roslaunch</build_depend>
+  <build_depend>rostest</build_depend>
+  <build_depend>tf2_ros</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>message_runtime</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>pcl_conversions</build_depend>
+  <build_depend>pcl_ros</build_depend>
+  <build_depend>libpcl-all-dev</build_depend>
+
+
+  <run_depend>angles</run_depend>
+  <run_depend>pcl_ros</run_depend>
+  <run_depend>nodelet</run_depend>
+  <run_depend>pluginlib</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>nodelet</run_depend>
+  <run_depend>dynamic_reconfigure</run_depend>
+  <run_depend>message_generation</run_depend>
+  <run_depend>message_runtime</run_depend>
+  <run_depend>pcl_conversions</run_depend>
+  <run_depend>pcl_ros</run_depend>
+  <run_depend>libpcl-all</run_depend>
+  <run_depend>std_msgs</run_depend>
+
+  <export>
+    <nodelet plugin="${prefix}/nodelets.xml"/>
+
+  </export>
+
+</package>

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_cx_driver/rviz/lslidar.rviz

@@ -0,0 +1,187 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 85
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /PointCloud21
+        - /LaserScan1
+      Splitter Ratio: 0.5
+    Tree Height: 686
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: PointCloud2
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: false
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 255
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic: /lslidar_point_cloud
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz/Axes
+      Enabled: true
+      Length: 1
+      Name: Axes
+      Radius: 0.10000000149011612
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic: /scan
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: laser_link
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 19.192291259765625
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.5347968935966492
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 1.3172023296356201
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1018
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001610000034afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006e00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000470000034a000000f100fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000034afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007301000000470000034a000000c200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d00000041fc0100000002fb0000000800540069006d006501000000000000073d0000037100fffffffb0000000800540069006d00650100000000000004500000000000000000000004bf0000034a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1853
+  X: 67
+  Y: 34

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_cx_driver/src/input.cc

@@ -0,0 +1,292 @@
+#include "lslidar_driver/input.h"
+
+extern volatile sig_atomic_t flag;
+namespace lslidar_driver {
+//static const size_t packet_size = sizeof(lslidar_driver::LslidarPacket().data);
+////////////////////////////////////////////////////////////////////////
+// Input base class implementation
+////////////////////////////////////////////////////////////////////////
+
+/** @brief constructor
+ *
+ *  @param private_nh ROS private handle for calling node.
+ *  @param port UDP port number.
+ */
+    Input::Input(ros::NodeHandle private_nh, uint16_t port, int packet_size) : private_nh_(private_nh), port_(port),
+                                                                               packet_size_(packet_size) {
+        npkt_update_flag_ = false;
+        cur_rpm_ = 0;
+        return_mode_ = 1;
+
+        private_nh.param("device_ip", devip_str_, std::string(""));
+        private_nh.param<bool>("add_multicast", add_multicast, false);
+        private_nh.param<std::string>("group_ip", group_ip, "224.1.1.2");
+        if (!devip_str_.empty())
+            ROS_INFO_STREAM("Only accepting packets from IP address: " << devip_str_);
+    }
+
+
+////////////////////////////////////////////////////////////////////////
+// InputSocket class implementation
+////////////////////////////////////////////////////////////////////////
+
+/** @brief constructor
+   *
+   *  @param private_nh ROS private handle for calling node.
+   *  @param port UDP port number
+*/
+    InputSocket::InputSocket(ros::NodeHandle private_nh, uint16_t port, int packet_size) : Input(private_nh, port,
+                                                                                                 packet_size) {
+        sockfd_ = -1;
+
+        if (!devip_str_.empty()) {
+            inet_aton(devip_str_.c_str(), &devip_);
+        }
+
+        ROS_INFO_STREAM("Opening UDP socket: port " << port);
+        sockfd_ = socket(PF_INET, SOCK_DGRAM, 0);
+        if (sockfd_ == -1) {
+            perror("socket");  // TODO: ROS_ERROR errno
+            return;
+        }
+
+        int opt = 1;
+        if (setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (const void *) &opt, sizeof(opt))) {
+            perror("setsockopt error!\n");
+            return;
+        }
+
+        sockaddr_in my_addr;                   // my address information
+        memset(&my_addr, 0, sizeof(my_addr));  // initialize to zeros
+        my_addr.sin_family = AF_INET;          // host byte order
+        my_addr.sin_port = htons(port);        // port in network byte order
+        my_addr.sin_addr.s_addr = INADDR_ANY;  // automatically fill in my IP
+
+        if (bind(sockfd_, (sockaddr *) &my_addr, sizeof(sockaddr)) == -1) {
+            perror("bind");  // TODO: ROS_ERROR errno
+            return;
+        }
+
+        if (add_multicast) {
+            struct ip_mreq group;
+            group.imr_multiaddr.s_addr = inet_addr(group_ip.c_str());
+            group.imr_interface.s_addr = htonl(INADDR_ANY);
+            //group.imr_interface.s_addr = inet_addr("192.168.1.102");
+
+            if (setsockopt(sockfd_, IPPROTO_IP, IP_ADD_MEMBERSHIP, (char *) &group, sizeof(group)) < 0) {
+                perror("Adding multicast group error ");
+                close(sockfd_);
+                exit(1);
+            } else
+                printf("Adding multicast group...OK.\n");
+        }
+        if (fcntl(sockfd_, F_SETFL, O_NONBLOCK | FASYNC) < 0) {
+            perror("non-block");
+            return;
+        }
+    }
+
+
+/** @brief destructor */
+    InputSocket::~InputSocket(void) {
+        (void) close(sockfd_);
+    }
+
+/** @brief Get one lslidar packet. */
+    int InputSocket::getPacket(lslidar_cx_driver::LslidarPacketPtr &pkt) {
+        struct pollfd fds[1];
+        fds[0].fd = sockfd_;
+        fds[0].events = POLLIN;
+        static const int POLL_TIMEOUT = 3000;  // one second (in msec)
+
+        sockaddr_in sender_address{};
+        socklen_t sender_address_len = sizeof(sender_address);
+        while (flag == 1)
+            // while (true)
+        {
+            // Receive packets that should now be available from the
+            // socket using a blocking read.
+            // poll() until input available
+            do {
+                int retval = poll(fds, 1, POLL_TIMEOUT);
+                if (retval < 0)  // poll() error?
+                {
+                    if (errno != EINTR)
+                        ROS_ERROR("poll() error: %s", strerror(errno));
+                    return 1;
+                }
+                if (retval == 0)  // poll() timeout?
+                {
+                    time_t curTime = time(NULL);
+                    struct tm *curTm = localtime(&curTime);
+                    char bufTime[72] = {0};
+                    sprintf(bufTime, "%d-%d-%d %d:%d:%d", curTm->tm_year + 1900, curTm->tm_mon + 1,
+                            curTm->tm_mday, curTm->tm_hour, curTm->tm_min, curTm->tm_sec);
+                    ROS_WARN("%s  lslidar poll() timeout, port:%d", bufTime, port_);
+                    /*
+                    char buffer_data[8] = "re-con";
+                    memset(&sender_address, 0, sender_address_len);          // initialize to zeros
+                    sender_address.sin_family = AF_INET;                     // host byte order
+                    sender_address.sin_port = htons(MSOP_DATA_PORT_NUMBER);  // port in network byte order, set any value
+                    sender_address.sin_addr.s_addr = devip_.s_addr;          // automatically fill in my IP
+                    sendto(sockfd_, &buffer_data, strlen(buffer_data), 0, (sockaddr*)&sender_address, sender_address_len);
+                    */
+                    return 1;
+                }
+                if ((fds[0].revents & POLLERR) || (fds[0].revents & POLLHUP) ||
+                    (fds[0].revents & POLLNVAL))  // device error?
+                {
+                    ROS_ERROR("poll() reports lslidar error");
+                    return 1;
+                }
+            } while ((fds[0].revents & POLLIN) == 0);
+            ssize_t nbytes = recvfrom(sockfd_, &pkt->data[0], packet_size_, 0, (sockaddr *) &sender_address,
+                                      &sender_address_len);
+
+            if (nbytes < 0) {
+                if (errno != EWOULDBLOCK) {
+                    perror("recvfail");
+                    ROS_INFO("recvfail");
+                    return 1;
+                }
+            } else if ((size_t) nbytes == packet_size_) {
+                if (devip_str_ != "" && sender_address.sin_addr.s_addr != devip_.s_addr) {
+                    ROS_WARN_THROTTLE(2, "lidar ip parameter error, please reset lidar ip in launch file");
+                    continue;
+                } else
+                    break;  // done
+            }
+
+            ROS_DEBUG_STREAM("incomplete lslidar packet read: " << nbytes << " bytes");
+        }
+        if (flag == 0) {
+            abort();
+        }
+
+        // Average the times at which we begin and end reading.  Use that to
+        // estimate when the scan occurred. Add the time offset.
+
+        return 0;
+    }
+
+
+
+////////////////////////////////////////////////////////////////////////
+// InputPCAP class implementation
+////////////////////////////////////////////////////////////////////////
+
+/** @brief constructor
+   *
+   *  @param private_nh ROS private handle for calling node.
+   *  @param port UDP port number
+   *  @param packet_rate expected device packet frequency (Hz)
+   *  @param filename PCAP dump file name
+   */
+    InputPCAP::InputPCAP(ros::NodeHandle private_nh, uint16_t port, int packet_size, double packet_rate,
+                         std::string filename,
+                         bool read_once, bool read_fast, double repeat_delay)
+            : Input(private_nh, port, packet_size), packet_rate_(packet_rate), filename_(filename) {
+        pcap_ = NULL;
+        empty_ = true;
+
+        // get parameters using private node handle
+        private_nh.param("read_once", read_once_, false);
+        private_nh.param("read_fast", read_fast_, false);
+        private_nh.param("repeat_delay", repeat_delay_, 0.0);
+
+        if (read_once_)
+            ROS_INFO("Read input file only once.");
+        if (read_fast_)
+            ROS_INFO("Read input file as quickly as possible.");
+        if (repeat_delay_ > 0.0)
+            ROS_INFO("Delay %.3f seconds before repeating input file.", repeat_delay_);
+
+        // Open the PCAP dump file
+        // ROS_INFO("Opening PCAP file \"%s\"", filename_.c_str());
+        ROS_INFO_STREAM("Opening PCAP file " << filename_);
+        if ((pcap_ = pcap_open_offline(filename_.c_str(), errbuf_)) == NULL) {
+            ROS_FATAL("Error opening lslidar socket dump file.");
+            return;
+        }
+
+        std::stringstream filter;
+        if (devip_str_ != "")  // using specific IP?
+        {
+            filter << "src host " << devip_str_ << " && ";
+        }
+        filter << "udp dst port " << port;
+        pcap_compile(pcap_, &pcap_packet_filter_, filter.str().c_str(), 1, PCAP_NETMASK_UNKNOWN);
+    }
+
+/** destructor */
+    InputPCAP::~InputPCAP(void) {
+        pcap_close(pcap_);
+    }
+
+/** @brief Get one lslidar packet. */
+    int InputPCAP::getPacket(lslidar_cx_driver::LslidarPacketPtr &pkt) {
+        struct pcap_pkthdr *header;
+        const u_char *pkt_data;
+
+        // while (flag == 1)
+        while (flag == 1) {
+            int res;
+            if ((res = pcap_next_ex(pcap_, &header, &pkt_data)) >= 0) {
+                // Skip packets not for the correct port and from the
+                // selected IP address.
+                if (!devip_str_.empty() && (0 == pcap_offline_filter(&pcap_packet_filter_, header, pkt_data)))
+                    continue;
+
+                // Keep the reader from blowing through the file.
+                if (read_fast_ == false)
+                    packet_rate_.sleep();
+
+
+                memcpy(&pkt->data[0], pkt_data + 42, packet_size_);
+
+                if (pkt->data[0] == 0xA5 && pkt->data[1] == 0xFF && pkt->data[2] == 0x00 &&
+                    pkt->data[3] == 0x5A) {//difop
+                    int rpm = (pkt->data[8] << 8) | pkt->data[9];
+                    ROS_DEBUG("lidar rpm:%d", rpm);
+                }
+
+                pkt->stamp = ros::Time::now();  // time_offset not considered here, as no
+                // synchronization required
+                empty_ = false;
+                return 0;  // success
+            }
+
+            if (empty_)  // no data in file?
+            {
+                ROS_WARN("Error %d reading lslidar packet: %s", res, pcap_geterr(pcap_));
+                return -1;
+            }
+
+            if (read_once_) {
+                ROS_INFO("end of file reached -- done reading.");
+                return -1;
+            }
+
+            if (repeat_delay_ > 0.0) {
+                ROS_INFO("end of file reached -- delaying %.3f seconds.", repeat_delay_);
+                usleep(rint(repeat_delay_ * 1000000.0));
+            }
+
+            ROS_DEBUG("replaying lslidar dump file");
+
+            // I can't figure out how to rewind the file, because it
+            // starts with some kind of header.  So, close the file
+            // and reopen it with pcap.
+            pcap_close(pcap_);
+            pcap_ = pcap_open_offline(filename_.c_str(), errbuf_);
+            empty_ = true;  // maybe the file disappeared?
+        }                 // loop back and try again
+
+        if (flag == 0) {
+            abort();
+        }
+
+        return 0;
+    }
+
+} //namespace

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_cx_driver/src/lslidar_driver.cpp

@@ -0,0 +1,1413 @@
+/******************************************************************************
+ * This file is part of lslidar_cx driver.
+ *
+ * Copyright 2022 LeiShen Intelligent Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#include "lslidar_driver/lslidar_driver.h"
+#include <std_msgs/String.h>
+#include <thread>
+#include <sensor_msgs/point_cloud2_iterator.h>
+
+namespace lslidar_driver {
+    LslidarDriver::LslidarDriver(ros::NodeHandle &node, ros::NodeHandle &private_nh) : nh(node),
+                                                                                       pnh(private_nh),
+                                                                                       last_azimuth(0),
+                                                                                       sweep_end_time(0.0),
+                                                                                       is_first_sweep(true),
+                                                                                       return_mode(1),
+                                                                                       packet_rate(1695.0),
+                                                                                       current_packet_time(0.0),
+                                                                                       last_packet_time(0.0),
+                                                                                       current_point_time(0.0),
+                                                                                       last_point_time(0.0),
+                                                                                       horizontal_angle_resolution(0.0),
+                                                                                       lidar_number_(1),
+                                                                                       config_vertical_angle_flag(0),
+                                                                                       point_cloud_xyzirt_(
+                                                                                               new pcl::PointCloud<VPoint>),
+                                                                                       point_cloud_xyzi_(
+                                                                                               new pcl::PointCloud<pcl::PointXYZI>),
+                                                                                       point_cloud_xyzirt_bak_(
+                                                                                               new pcl::PointCloud<VPoint>),
+                                                                                       point_cloud_xyzi_bak_(
+                                                                                               new pcl::PointCloud<pcl::PointXYZI>),
+                                                                                       scan_msg(
+                                                                                               new sensor_msgs::LaserScan),
+                                                                                       scan_msg_bak(
+                                                                                               new sensor_msgs::LaserScan),
+                                                                                       start_process_msop_(false),
+                                                                                       is_new_c32w_(true),
+                                                                                       is_get_scan_altitude_(false),
+                                                                                       is_msc16(true) {
+        ROS_INFO("***********************cx4.0 ROS driver version: %s ***********************",
+                 lslidar_cx_driver_VERSION);
+
+    }
+
+    bool LslidarDriver::checkPacketValidity(lslidar_cx_driver::LslidarPacketPtr &packet) {
+        for (size_t blk_idx = 0; blk_idx < BLOCKS_PER_PACKET; ++blk_idx) {
+            if (packet->data[blk_idx * 100] != 0xff && packet->data[blk_idx * 100 + 1] != 0xee) {
+                return false;
+            }
+        }
+        return true;
+    }
+
+    bool LslidarDriver::isPointInRange(const double &distance) {
+        return (distance >= min_range && distance < max_range);
+    }
+
+    bool LslidarDriver::loadParameters() {
+        pnh.param("pcap", dump_file, std::string(""));
+        pnh.param("packet_rate", packet_rate, 1695.0);
+        pnh.param<std::string>("frame_id", frame_id, "laser_link");
+        pnh.param<std::string>("lidar_type", lidar_type, "c16");
+        pnh.param<bool>("add_multicast", add_multicast, false);
+        // pnh.param<std::string>("c32_type", c32_type, "c32_32");
+        pnh.param<bool>("pcl_type", pcl_type, false);
+        pnh.param("group_ip", group_ip_string, std::string("234.2.3.2"));
+        pnh.param("device_ip", lidar_ip_string, std::string("192.168.1.200"));
+        pnh.param("msop_port", msop_udp_port, (int) MSOP_DATA_PORT_NUMBER);
+        pnh.param("difop_port", difop_udp_port, (int) DIFOP_DATA_PORT_NUMBER);
+        pnh.param("point_num", point_num, 2000);
+        pnh.param("scan_num", scan_num, 8);
+        pnh.param("min_range", min_range, 0.3);
+        pnh.param("max_range", max_range, 150.0);
+        pnh.param("distance_unit", distance_unit, 0.40);
+        pnh.param("angle_disable_min", angle_disable_min, 0);
+        pnh.param("angle_disable_max", angle_disable_max, 0);
+        pnh.param("horizontal_angle_resolution", horizontal_angle_resolution, 0.2);
+        pnh.param<bool>("use_time_service", use_time_service, false);
+        pnh.param<bool>("publish_scan", publish_scan, false);
+        pnh.param<bool>("coordinate_opt", coordinate_opt, false);
+        pnh.param<std::string>("pointcloud_topic", pointcloud_topic, "lslidar_point_cloud");
+        inet_aton(lidar_ip_string.c_str(), &lidar_ip);
+        if (add_multicast) ROS_INFO_STREAM("opening UDP socket: group_address " << group_ip_string);
+
+        return true;
+    }
+
+    void LslidarDriver::initTimeStamp() {
+        for (int i = 0; i < 10; i++) {
+            this->packetTimeStamp[i] = 0;
+        }
+        this->packet_time_s = 0;
+        this->packet_time_ns = 0;
+        this->timeStamp = ros::Time(0.0);
+    }
+
+    bool LslidarDriver::createRosIO() {
+        pointcloud_pub = nh.advertise<sensor_msgs::PointCloud2>(pointcloud_topic, 10);
+        scan_pub = nh.advertise<sensor_msgs::LaserScan>("scan", 10);
+        time_service_ = nh.advertiseService("time_service", &LslidarDriver::timeService, this);
+        lslidar_control_service_ = nh.advertiseService("lslidar_control", &LslidarDriver::powerOn, this);
+        motor_control_service_ = nh.advertiseService("motor_control", &LslidarDriver::motorControl, this);
+        remove_control_service_ = nh.advertiseService("remove_control", &LslidarDriver::removeControl, this);
+        motor_speed_service_ = nh.advertiseService("set_motor_speed", &LslidarDriver::motorSpeed, this);
+        data_port_service_ = nh.advertiseService("set_data_port", &LslidarDriver::setDataPort, this);
+        dev_port_service_ = nh.advertiseService("set_dev_port", &LslidarDriver::setDevPort, this);
+        data_ip_service_ = nh.advertiseService("set_data_ip", &LslidarDriver::setDataIp, this);
+        destination_ip_service_ = nh.advertiseService("set_destination_ip", &LslidarDriver::setDestinationIp, this);
+
+        if (!dump_file.empty()) {
+            msop_input_.reset(new lslidar_driver::InputPCAP(pnh, msop_udp_port, 1212, packet_rate, dump_file));
+            difop_input_.reset(new lslidar_driver::InputPCAP(pnh, difop_udp_port, 1206, 1, dump_file));
+        } else {
+            msop_input_.reset(new lslidar_driver::InputSocket(pnh, msop_udp_port, 1212));
+            difop_input_.reset(new lslidar_driver::InputSocket(pnh, difop_udp_port, 1206));
+        }
+        difop_thread_ = boost::shared_ptr<boost::thread>(
+                new boost::thread(boost::bind(&LslidarDriver::difopPoll, this)));
+
+        //std::thread ttt = std::thread(std::bind(&LslidarDriver::difopPoll,this));
+
+        return true;
+    }
+
+    bool LslidarDriver::initialize() {
+        this->initTimeStamp();
+        if (!loadParameters()) {
+            ROS_INFO("cannot load all required ROS parameters.");
+            return false;
+        }
+        if (!createRosIO()) {
+            ROS_INFO("cannot create all ROS IO.");
+            return false;
+        }
+
+/*        if (lidar_type == "c16") {
+            lidar_number_ = 16;
+            ROS_INFO("lidar: c16");
+            for (int i = 0; i < 16; ++i) {
+                sin_scan_altitude[i] = sin(c16_vertical_angle[i] * DEG_TO_RAD);
+                cos_scan_altitude[i] = cos(c16_vertical_angle[i] * DEG_TO_RAD);
+            }
+        } else if (lidar_type == "c8") {
+            lidar_number_ = 8;
+            ROS_INFO("lidar: c8");
+            for (int i = 0; i < 8; ++i) {
+                sin_scan_altitude[i] = sin(c8_vertical_angle[i] * DEG_TO_RAD);
+                cos_scan_altitude[i] = cos(c8_vertical_angle[i] * DEG_TO_RAD);
+            }
+        } else if (lidar_type == "c1") {
+            lidar_number_ = 1;
+            ROS_INFO("lidar: c1");
+            for (int i = 0; i < 8; ++i) {
+                sin_scan_altitude[i] = sin(c1_vertical_angle[i] * DEG_TO_RAD);
+                cos_scan_altitude[i] = cos(c1_vertical_angle[i] * DEG_TO_RAD);
+            }
+        } else if (lidar_type == "c32") {
+            lidar_number_ = 32;
+            ROS_INFO("lidar: c32");
+            if ("c32_32" == c32_type) {
+                ROS_INFO("Vertical angle: 32 degrees");
+                for (int i = 0; i < 32; ++i) {
+                    sin_scan_altitude[i] = sin(c32_vertical_angle[i] * DEG_TO_RAD);
+                    cos_scan_altitude[i] = cos(c32_vertical_angle[i] * DEG_TO_RAD);
+                }
+            } else if ("c32_70" == c32_type) {
+                ROS_INFO("Vertical angle: 70 degrees");
+                for (int k = 0; k < 32; ++k) {
+                    sin_scan_altitude[k] = sin(c32_70_vertical_angle2[k] * DEG_TO_RAD);
+                    cos_scan_altitude[k] = cos(c32_70_vertical_angle2[k] * DEG_TO_RAD);
+                }
+            } else if ("c32_90" == c32_type) {
+                ROS_INFO("Vertical angle: 90 degrees");
+                for (int k = 0; k < 32; ++k) {
+                    sin_scan_altitude[k] = sin(c32_90_vertical_angle[k] * DEG_TO_RAD);
+                    cos_scan_altitude[k] = cos(c32_90_vertical_angle[k] * DEG_TO_RAD);
+                }
+            }
+
+        }*/
+
+        // create the sin and cos table for different azimuth values
+        for (int j = 0; j < 36000; ++j) {
+            float angle = static_cast<float>(j) * 0.01f * DEG_TO_RAD;
+            sin_azimuth_table[j] = sinf(angle);
+            cos_azimuth_table[j] = cosf(angle);
+        }
+        point_cloud_xyzirt_->header.frame_id = frame_id;
+        point_cloud_xyzirt_->height = 1;
+
+        point_cloud_xyzi_->header.frame_id = frame_id;
+        point_cloud_xyzi_->height = 1;
+        if (publish_scan) {
+            scan_msg->angle_min = 0;
+            scan_msg->angle_max = M_PI * 2;
+            scan_msg->range_min = min_range;
+            scan_msg->range_max = max_range;
+            scan_msg->angle_increment = horizontal_angle_resolution * DEG_TO_RAD;
+
+            point_size = ceil((scan_msg->angle_max - scan_msg->angle_min) / scan_msg->angle_increment);
+
+            scan_msg->ranges.assign(point_size, std::numeric_limits<float>::quiet_NaN());
+            scan_msg->intensities.assign(point_size, std::numeric_limits<float>::quiet_NaN());
+        }
+        return true;
+    }
+
+    void LslidarDriver::difopPoll() {
+        // reading and publishing scans as fast as possible.
+        lslidar_cx_driver::LslidarPacketPtr difop_packet_ptr(new lslidar_cx_driver::LslidarPacket);
+        static bool is_print_working_time = true;
+        while (ros::ok()) {
+            // keep reading
+            int rc = difop_input_->getPacket(difop_packet_ptr);
+            if (rc == 0) {
+                if (difop_packet_ptr->data[0] != 0xa5 || difop_packet_ptr->data[1] != 0xff ||
+                    difop_packet_ptr->data[2] != 0x00 || difop_packet_ptr->data[3] != 0x5a) {
+                    return;
+                }
+                time_service_mode_ = difop_packet_ptr->data[45];
+                remove_rain_flag =  difop_packet_ptr->data[110];
+                ROS_INFO_ONCE("Remove rain, fog, and dust, level: %d",remove_rain_flag);
+                for (int i = 0; i < 1206; i++) {
+                    difop_data[i] = difop_packet_ptr->data[i];
+                }
+/*                if(is_msc16 && is_get_scan_altitude_){
+                    for (int j = 0; j < 16; ++j) {
+
+                    }
+
+                    is_msc16 = false;
+                }*/
+                if (is_msc16 && difop_packet_ptr->data[1198] / 16 == 7 && difop_packet_ptr->data[1202] / 16 == 7) {
+                    for (int j = 0; j < 16; ++j) {
+                        int adjust_angle =
+                                difop_packet_ptr->data[680 + 2 * j] * 256 + difop_packet_ptr->data[681 + 2 * j];
+                        adjust_angle = adjust_angle > 32767 ? adjust_angle - 65535 : adjust_angle;
+                        msc16_adjust_angle[j] = static_cast<float>(adjust_angle) * 0.01f;
+
+                        int offset_angle =
+                                difop_packet_ptr->data[712 + 2 * j] * 256 + difop_packet_ptr->data[713 + 2 * j];
+                        offset_angle = offset_angle > 32767 ? offset_angle - 65535 : offset_angle;
+                        msc16_offset_angle[j] = static_cast<float>(offset_angle) * 0.01f;
+                        if (fabs(msc16_adjust_angle[j] - c16_vertical_angle[c16_remap_angle[j]]) > 1.5) {
+                            config_vertical_angle_flag++;
+                        }
+                    }
+                    if (config_vertical_angle_flag == 0) {
+                        for (int k = 0; k < 16; ++k) {
+                            c16_vertical_angle[c16_remap_angle[k]] = msc16_adjust_angle[k];
+                        }
+                    }
+                    is_msc16 = false;
+                }
+
+                is_get_difop_ = true;
+                start_process_msop_ = true;
+                if (is_print_working_time) {
+                    int total_working_time = (difop_packet_ptr->data[105] << 24) + (difop_packet_ptr->data[106] << 16)
+                                             + (difop_packet_ptr->data[107] << 8) + difop_packet_ptr->data[108];
+                    int less_minus40_degree_working_time = (difop_packet_ptr->data[112] << 16)
+                                                           + (difop_packet_ptr->data[113] << 8) +
+                                                           difop_packet_ptr->data[114];
+                    int minus40_to_minus10_working_time = (difop_packet_ptr->data[115] << 16)
+                                                          + (difop_packet_ptr->data[116] << 8) +
+                                                          difop_packet_ptr->data[117];
+                    int minus10_to_30_working_time = (difop_packet_ptr->data[118] << 16)
+                                                     + (difop_packet_ptr->data[119] << 8) + difop_packet_ptr->data[120];
+                    int positive30_to_70_working_time = (difop_packet_ptr->data[121] << 16)
+                                                        + (difop_packet_ptr->data[122] << 8) +
+                                                        difop_packet_ptr->data[123];
+                    int positive70_to_100_working_time = (difop_packet_ptr->data[124] << 16)
+                                                         + (difop_packet_ptr->data[125] << 8) +
+                                                         difop_packet_ptr->data[126];
+                    ROS_INFO("total working time: %d hours:%d minutes", total_working_time / 60,
+                             total_working_time % 60);
+                    ROS_INFO("less than     -40 degrees  working time: %d hours:%d minutes",
+                             less_minus40_degree_working_time / 60, less_minus40_degree_working_time % 60);
+                    ROS_INFO("-40 degrees ~ -10 degrees  working time: %d hours:%d minutes",
+                             minus40_to_minus10_working_time / 60, minus40_to_minus10_working_time % 60);
+                    ROS_INFO("-10 degrees ~ 30  degrees  working time: %d hours:%d minutes",
+                             minus10_to_30_working_time / 60, minus10_to_30_working_time % 60);
+                    ROS_INFO("30  degrees ~ 70  degrees  working time: %d hours:%d minutes",
+                             positive30_to_70_working_time / 60, positive30_to_70_working_time % 60);
+                    ROS_INFO("70  degrees ~ 100 degrees  working time: %d hours:%d minutes",
+                             positive70_to_100_working_time / 60, positive70_to_100_working_time % 60);
+                    is_print_working_time = false;
+                }
+            } else if (rc < 0) {
+                return;
+            }
+            //ros::spinOnce();
+        }
+    }
+
+    void LslidarDriver::pointcloudToLaserscan(const sensor_msgs::PointCloud2 &cloud_msg,
+                                              sensor_msgs::LaserScan &output_scan) {
+        // build laserscan output_scan
+        output_scan.header = cloud_msg.header;
+        output_scan.header.frame_id = cloud_msg.header.frame_id;
+        output_scan.angle_min = -M_PI;
+        output_scan.angle_max = M_PI;
+        output_scan.angle_increment = horizontal_angle_resolution * DEG_TO_RAD;
+        output_scan.time_increment = 0.0;
+//        output_scan.scan_time = scan_time_;
+        output_scan.range_min = min_range;
+        output_scan.range_max = max_range;
+
+        // determine amount of rays to create
+        uint32_t ranges_size = std::ceil((output_scan.angle_max - output_scan.angle_min) / output_scan.angle_increment);
+
+        // determine if laserscan rays with no obstacle data will evaluate to infinity or max_range
+        output_scan.ranges.assign(ranges_size, std::numeric_limits<double>::infinity());
+        output_scan.intensities.assign(ranges_size, std::numeric_limits<float>::quiet_NaN());
+
+        // Iterate through pointcloud
+        for (sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud_msg, "x"), iter_y(cloud_msg, "y"),
+                     iter_z(cloud_msg, "z"), iter_intensity(cloud_msg, "intensity");
+             iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++iter_intensity) {
+            if (std::isnan(*iter_x) || std::isnan(*iter_y) || std::isnan(*iter_z)) {
+                ROS_DEBUG("rejected for nan in point(%f, %f, %f)\n", *iter_x, *iter_y, *iter_z);
+                continue;
+            }
+
+            double range = hypot(*iter_x, *iter_y);
+            if (range < min_range) {
+                ROS_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range, min_range,
+                          *iter_x,
+                          *iter_y, *iter_z);
+                continue;
+            }
+            if (range > max_range) {
+                ROS_DEBUG("rejected for range %f above maximum value %f. Point: (%f, %f, %f)", range, max_range,
+                          *iter_x,
+                          *iter_y, *iter_z);
+                continue;
+            }
+            double angle = atan2(*iter_y, *iter_x);
+            if (angle < output_scan.angle_min || angle > output_scan.angle_max) {
+                ROS_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, output_scan.angle_min,
+                          output_scan.angle_max);
+                continue;
+            }
+
+            // overwrite range at laserscan ray if new range is smaller
+            int index = (angle - output_scan.angle_min) / output_scan.angle_increment;
+            if (range < output_scan.ranges[index]) {
+                output_scan.ranges[index] = range;
+                output_scan.intensities[index] = *iter_intensity;
+            }
+        }
+    }
+
+    void LslidarDriver::publishPointcloud() {
+        std::unique_lock<std::mutex> lock(pointcloud_lock);
+
+        if (pcl_type) {
+            if (point_cloud_xyzi_bak_->points.size() < 65) {
+                return;
+            }
+            sensor_msgs::PointCloud2Ptr pc_msg(new sensor_msgs::PointCloud2);
+            pcl::toROSMsg(*point_cloud_xyzi_bak_, *pc_msg);
+            pc_msg->header.stamp = ros::Time(sweep_end_time);
+            pointcloud_pub.publish(pc_msg);
+        } else {
+            if (point_cloud_xyzirt_bak_->points.size() < 65) {
+                return;
+            }
+            sensor_msgs::PointCloud2Ptr pc_msg(new sensor_msgs::PointCloud2);
+            pcl::toROSMsg(*point_cloud_xyzirt_bak_, *pc_msg);
+            pc_msg->header.stamp = ros::Time(sweep_end_time);
+            pointcloud_pub.publish(pc_msg);
+        }
+        return;
+    }
+
+    void LslidarDriver::publishScan() {
+        std::unique_lock<std::mutex> lock(pointcloud_lock);
+        scan_msg_bak->header.frame_id = frame_id;
+        scan_msg_bak->header.stamp = ros::Time(sweep_end_time);
+        scan_pub.publish(scan_msg_bak);
+    }
+
+    void LslidarDriver::setPacketHeader(unsigned char *config_data) {
+        config_data[0] = 0xAA;
+        config_data[1] = 0x00;
+        config_data[2] = 0xFF;
+        config_data[3] = 0x11;
+        config_data[4] = 0x22;
+        config_data[5] = 0x22;
+        config_data[6] = 0xAA;
+        config_data[7] = 0xAA;
+    }
+
+    bool LslidarDriver::sendPacketTolidar(unsigned char *config_data) const {
+        int socketid;
+        sockaddr_in addrSrv{};
+        socketid = socket(2, 2, 0);
+        addrSrv.sin_addr.s_addr = inet_addr(lidar_ip_string.c_str());
+        addrSrv.sin_family = AF_INET;
+        addrSrv.sin_port = htons(2368);
+        sendto(socketid, (const char *) config_data, 1206, 0, (struct sockaddr *) &addrSrv, sizeof(addrSrv));
+        return true;
+    }
+
+    bool LslidarDriver::powerOn(lslidar_cx_driver::lslidar_control::Request &req,
+                                lslidar_cx_driver::lslidar_control::Response &res) {
+        ROS_WARN("--------------------------");
+        // sleep(1);
+        lslidar_cx_driver::LslidarPacketPtr packet0(new lslidar_cx_driver::LslidarPacket);
+        packet0->data[0] = 0x00;
+        packet0->data[1] = 0x00;
+        int rc_msop = -1;
+
+        if (!is_get_difop_) {
+            res.result = 0;
+            ROS_ERROR("Can not get dev packet! Set failed!");
+            return true;
+        }
+
+        unsigned char config_data[1206];
+        mempcpy(config_data, difop_data, 1206);
+        if (config_data[0] != 0xa5 || config_data[1] != 0xff || config_data[2] != 0x00 || config_data[3] != 0x5a) {
+            res.result = 0;
+            ROS_ERROR("Can not get dev packet! Set failed!");
+            return true;
+        }
+        setPacketHeader(config_data);
+        is_get_difop_ = false;
+
+        if (req.laser_control == 1) {
+            if ((rc_msop = msop_input_->getPacket(packet0)) == 0) {
+                res.result = 1;
+                ROS_WARN("receive cmd: %d,already power on status", req.laser_control);
+                return true;
+            }
+            ROS_WARN("receive cmd: %d,power on", req.laser_control);
+            config_data[50] = 0xBB;
+            sendPacketTolidar(config_data);
+            double time1 = ros::Time::now().toSec();
+
+            do {
+                rc_msop = msop_input_->getPacket(packet0);
+                double time2 = ros::Time::now().toSec();
+                if (time2 - time1 > 20) {
+                    res.result = 0;
+                    ROS_WARN("lidar connect error");
+                    return true;
+                }
+            } while ((rc_msop != 0) && (packet0->data[0] != 0xff) && (packet0->data[1] != 0xee));
+            sleep(5);
+            res.result = 1;
+        } else if (req.laser_control == 0) {
+            config_data[50] = 0xAA;
+            ROS_WARN("receive cmd: %d,power off", req.laser_control);
+            sendPacketTolidar(config_data);
+            res.result = 1;
+        } else {
+            ROS_WARN("cmd error");
+            res.result = 0;
+        }
+        return true;
+    }
+
+    bool LslidarDriver::timeService(lslidar_cx_driver::time_service::Request &req,
+                                    lslidar_cx_driver::time_service::Response &res) {
+        ROS_INFO("Start to modify lidar time service mode");
+        if (!is_get_difop_) {
+            res.result = 0;
+            ROS_ERROR("Can not get dev packet! Set failed!");
+            return true;
+        }
+
+        unsigned char config_data[1206];
+        mempcpy(config_data, difop_data, 1206);
+        if (config_data[0] != 0xa5 || config_data[1] != 0xff || config_data[2] != 0x00 || config_data[3] != 0x5a) {
+            res.result = 0;
+            ROS_ERROR("Can not get dev packet! Set failed!");
+            return true;
+        }
+        setPacketHeader(config_data);
+        is_get_difop_ = false;
+
+        std::string time_service_mode = req.time_service_mode;
+        transform(time_service_mode.begin(), time_service_mode.end(), time_service_mode.begin(), ::tolower);
+
+        if (time_service_mode == "gps") {
+            config_data[45] = 0x00;
+        } else if (time_service_mode == "ptp") {
+            config_data[45] = 0x01;
+        } else if (time_service_mode == "ntp") {
+            config_data[45] = 0x02;
+            std::string ntp_ip = req.ntp_ip;
+            std::regex ipv4(
+                    "\\b(?:(?:25[0-5]|2[0-4][0-9]|[01]?[0-9][0-9]?)\\.){3}(?:25[0-5]|2[0-4][0-9]|[01]?[0-9][0-9]?)\\b");
+            if (!regex_match(ntp_ip, ipv4)) {
+                ROS_ERROR("Parameter error, please check the input parameters");
+                res.result = false;
+                return true;
+            }
+            unsigned short first_value, second_value, third_value, end_value;
+            sscanf(ntp_ip.c_str(), "%hu.%hu.%hu.%hu", &first_value, &second_value, &third_value, &end_value);
+            config_data[28] = first_value;
+            config_data[29] = second_value;
+            config_data[30] = third_value;
+            config_data[31] = end_value;
+        } else {
+            ROS_ERROR("Parameter error, please check the input parameters");
+            res.result = false;
+            return true;
+        }
+
+        printf("byte[45] %X \n", config_data[45]);
+        res.result = true;
+        sendPacketTolidar(config_data);
+        ROS_INFO("Time service method modified successfully!");
+        return true;
+    }
+
+    bool LslidarDriver::motorControl(lslidar_cx_driver::motor_control::Request &req,
+                                     lslidar_cx_driver::motor_control::Response &res) {
+        if (!is_get_difop_) {
+            res.result = 0;
+            ROS_ERROR("Can not get dev packet! Set failed!");
+            return true;
+        }
+
+        unsigned char config_data[1206];
+        mempcpy(config_data, difop_data, 1206);
+        if (config_data[0] != 0xa5 || config_data[1] != 0xff || config_data[2] != 0x00 || config_data[3] != 0x5a) {
+            res.result = 0;
+            ROS_ERROR("Can not get dev packet! Set failed!");
+            return true;
+        }
+        setPacketHeader(config_data);
+        is_get_difop_ = false;
+
+        if (req.motor_control == 1) {
+            config_data[41] = 0x00;
+        } else if (req.motor_control == 0) {
+            config_data[41] = 0x01;
+        } else {
+            ROS_ERROR("Parameter error, please check the input parameters");
+            res.result = false;
+            return true;
+        }
+        printf("byte[41] %X \n", config_data[41]);
+        res.result = true;
+        sendPacketTolidar(config_data);
+        ROS_INFO("Set successfully!");
+        return true;
+    }
+
+    bool LslidarDriver::removeControl(lslidar_cx_driver::remove_control::Request &req,
+                                      lslidar_cx_driver::remove_control::Response &res) {
+        if (!is_get_difop_) {
+            res.result = 0;
+            ROS_ERROR("Can not get dev packet! Set failed!");
+            return true;
+        }
+
+        unsigned char config_data[1206];
+        mempcpy(config_data, difop_data, 1206);
+        if (config_data[0] != 0xa5 || config_data[1] != 0xff || config_data[2] != 0x00 || config_data[3] != 0x5a) {
+            res.result = 0;
+            ROS_ERROR("Can not get dev packet! Set failed!");
+            return true;
+        }
+        setPacketHeader(config_data);
+        is_get_difop_ = false;
+
+        if (req.remove_control == 0) {
+            config_data[110] = 0x00;
+        } else if (req.remove_control == 1) {
+            config_data[110] = 0x01;
+        } else if (req.remove_control == 2) {
+            config_data[110] = 0x02;
+        }else if(req.remove_control == 3) {
+            config_data[110] = 0x03;
+        }else {
+            ROS_ERROR("Parameter error, please check the input parameters");
+            res.result = false;
+            return true;
+        }
+        res.result = true;
+        sendPacketTolidar(config_data);
+        ROS_INFO("Set successfully, Remove rain, fog, and dust, level: %d",req.remove_control);
+        return true;
+    }
+
+    bool LslidarDriver::motorSpeed(lslidar_cx_driver::motor_speed::Request &req,
+                                   lslidar_cx_driver::motor_speed::Response &res) {
+        if (!is_get_difop_) {
+            res.result = 0;
+            ROS_ERROR("Can not get dev packet! Set failed!");
+            return true;
+        }
+
+        unsigned char config_data[1206];
+        mempcpy(config_data, difop_data, 1206);
+        if (config_data[0] != 0xa5 || config_data[1] != 0xff || config_data[2] != 0x00 || config_data[3] != 0x5a) {
+            res.result = 0;
+            ROS_ERROR("Can not get dev packet! Set failed!");
+            return true;
+        }
+        setPacketHeader(config_data);
+        is_get_difop_ = false;
+
+        if (req.motor_speed == 5) {
+            config_data[8] = 0x01;
+            config_data[9] = 0x2c;
+        } else if (req.motor_speed == 10) {
+            config_data[8] = 0x02;
+            config_data[9] = 0x58;
+        } else if (req.motor_speed == 20) {
+            config_data[8] = 0x04;
+            config_data[9] = 0xB0;
+        } else {
+            ROS_ERROR("Parameter error, please check the input parameters");
+            res.result = false;
+            return true;
+        }
+        res.result = true;
+        sendPacketTolidar(config_data);
+        ROS_INFO("Set successfully!");
+        return true;
+    }
+
+    bool LslidarDriver::setDataPort(lslidar_cx_driver::data_port::Request &req,
+                                    lslidar_cx_driver::data_port::Response &res) {
+        if (!is_get_difop_) {
+            res.result = 0;
+            ROS_ERROR("Can not get dev packet! Set failed!");
+            return true;
+        }
+
+        unsigned char config_data[1206];
+        mempcpy(config_data, difop_data, 1206);
+        if (config_data[0] != 0xa5 || config_data[1] != 0xff || config_data[2] != 0x00 || config_data[3] != 0x5a) {
+            res.result = 0;
+            ROS_ERROR("Can not get dev packet! Set failed!");
+            return true;
+        }
+        setPacketHeader(config_data);
+        is_get_difop_ = false;
+        int dev_port = config_data[26] * 256 + config_data[27];
+        if (req.data_port < 1025 || req.data_port > 65535 || req.data_port == dev_port) {
+            ROS_ERROR("Parameter error, please check the input parameters");
+            res.result = false;
+            return true;
+        } else {
+            config_data[24] = req.data_port / 256;
+            config_data[25] = req.data_port % 256;
+        }
+        res.result = true;
+        sendPacketTolidar(config_data);
+        ROS_INFO("Set successfully!");
+        return true;
+    }
+
+    bool LslidarDriver::setDevPort(lslidar_cx_driver::dev_port::Request &req,
+                                   lslidar_cx_driver::dev_port::Response &res) {
+        if (!is_get_difop_) {
+            res.result = 0;
+            ROS_ERROR("Can not get dev packet! Set failed!");
+            return true;
+        }
+
+        unsigned char config_data[1206];
+        mempcpy(config_data, difop_data, 1206);
+        if (config_data[0] != 0xa5 || config_data[1] != 0xff || config_data[2] != 0x00 || config_data[3] != 0x5a) {
+            res.result = 0;
+            ROS_ERROR("Can not get dev packet! Set failed!");
+            return true;
+        }
+        setPacketHeader(config_data);
+        is_get_difop_ = false;
+
+        int data_port = config_data[24] * 256 + config_data[25];
+        if (req.dev_port < 1025 || req.dev_port > 65535 || req.dev_port == data_port) {
+            ROS_ERROR("Parameter error, please check the input parameters");
+            res.result = false;
+            return true;
+        } else {
+            config_data[26] = req.dev_port / 256;
+            config_data[27] = req.dev_port % 256;
+        }
+        res.result = true;
+        sendPacketTolidar(config_data);
+        ROS_INFO("Set successfully!");
+        return true;
+    }
+
+    bool LslidarDriver::setDataIp(lslidar_cx_driver::data_ip::Request &req,
+                                  lslidar_cx_driver::data_ip::Response &res) {
+        std::regex ipv4(
+                "\\b(?:(?:25[0-5]|2[0-4][0-9]|[01]?[0-9][0-9]?)\\.){3}(?:25[0-5]|2[0-4][0-9]|[01]?[0-9][0-9]?)\\b");
+        if (!regex_match(req.data_ip, ipv4)) {
+            ROS_ERROR("Parameter error, please check the input parameters");
+            res.result = false;
+            return true;
+        }
+
+        if (!is_get_difop_) {
+            res.result = 0;
+            ROS_ERROR("Can not get dev packet! Set failed!");
+            return true;
+        }
+
+        unsigned char config_data[1206];
+        mempcpy(config_data, difop_data, 1206);
+        if (config_data[0] != 0xa5 || config_data[1] != 0xff || config_data[2] != 0x00 || config_data[3] != 0x5a) {
+            res.result = 0;
+            ROS_ERROR("Can not get dev packet! Set failed!");
+            return true;
+        }
+        setPacketHeader(config_data);
+        is_get_difop_ = false;
+
+        unsigned short first_value, second_value, third_value, end_value;
+        sscanf(req.data_ip.c_str(), "%hu.%hu.%hu.%hu", &first_value, &second_value, &third_value, &end_value);
+
+        std::string destination_ip = std::to_string(config_data[14]) + "." + std::to_string(config_data[15]) + "." +
+                                     std::to_string(config_data[16]) + "." + std::to_string(config_data[17]);
+        if (first_value == 0 || first_value == 127 ||
+            (first_value >= 240 && first_value <= 255) || destination_ip == req.data_ip) {
+            ROS_ERROR("Parameter error, please check the input parameters");
+            res.result = false;
+            return true;
+        } else {
+            config_data[10] = first_value;
+            config_data[11] = second_value;
+            config_data[12] = third_value;
+            config_data[13] = end_value;
+        }
+        res.result = true;
+        sendPacketTolidar(config_data);
+        ROS_INFO("Set successfully!");
+        return true;
+    }
+
+    bool LslidarDriver::setDestinationIp(lslidar_cx_driver::destination_ip::Request &req,
+                                         lslidar_cx_driver::destination_ip::Response &res) {
+        std::regex ipv4(
+                "\\b(?:(?:25[0-5]|2[0-4][0-9]|[01]?[0-9][0-9]?)\\.){3}(?:25[0-5]|2[0-4][0-9]|[01]?[0-9][0-9]?)\\b");
+        if (!regex_match(req.destination_ip, ipv4)) {
+            ROS_ERROR("Parameter error, please check the input parameters");
+            res.result = false;
+            return true;
+        }
+
+        if (!is_get_difop_) {
+            res.result = 0;
+            ROS_ERROR("Can not get dev packet! Set failed!");
+            return true;
+        }
+
+        unsigned char config_data[1206];
+        mempcpy(config_data, difop_data, 1206);
+        if (config_data[0] != 0xa5 || config_data[1] != 0xff || config_data[2] != 0x00 || config_data[3] != 0x5a) {
+            res.result = 0;
+            ROS_ERROR("Can not get dev packet! Set failed!");
+            return true;
+        }
+        setPacketHeader(config_data);
+        is_get_difop_ = false;
+        unsigned short first_value, second_value, third_value, end_value;
+        sscanf(req.destination_ip.c_str(), "%hu.%hu.%hu.%hu", &first_value, &second_value, &third_value, &end_value);
+
+        std::string data_ip = std::to_string(config_data[10]) + "." + std::to_string(config_data[11]) + "." +
+                              std::to_string(config_data[12]) + "." + std::to_string(config_data[13]);
+        if (first_value == 0 || first_value == 127 ||
+            (first_value >= 240 && first_value <= 255) || data_ip == req.destination_ip) {
+            ROS_ERROR("Parameter error, please check the input parameters");
+            res.result = false;
+            return true;
+        } else {
+            config_data[14] = first_value;
+            config_data[15] = second_value;
+            config_data[16] = third_value;
+            config_data[17] = end_value;
+        }
+        res.result = true;
+        sendPacketTolidar(config_data);
+        ROS_INFO("Set successfully!");
+        return true;
+    }
+
+    void LslidarDriver::decodePacket(lslidar_cx_driver::LslidarPacketPtr &packet) {
+        //couputer azimuth angle for each firing
+        for (size_t b_idx = 0; b_idx < BLOCKS_PER_PACKET; ++b_idx) {
+            firings.firing_azimuth[b_idx] = (packet->data[b_idx * 100 + 2] + (packet->data[b_idx * 100 + 3] << 8)) %
+                                            36000; //* 0.01 * DEG_TO_RAD;
+        }
+        for (size_t block_idx = 0; block_idx < BLOCKS_PER_PACKET; ++block_idx) {
+            // computer distance ,intensity
+            //      for (size_t blk_fir_idx = 0; blk_fir_idx < FIRINGS_PER_BLOCK; ++blk_fir_idx) {
+            //        size_t fir_idx = blk_idx * FIRINGS_PER_BLOCK + blk_fir_idx;
+            int32_t azimuth_diff_b = 0;
+            if (return_mode == 1) {
+                if (block_idx < BLOCKS_PER_PACKET - 1) {
+                    azimuth_diff_b = firings.firing_azimuth[block_idx + 1] - firings.firing_azimuth[block_idx];
+                    azimuth_diff_b = azimuth_diff_b < 0 ? azimuth_diff_b + 36000 : azimuth_diff_b;
+
+                } else {
+                    azimuth_diff_b = firings.firing_azimuth[block_idx] - firings.firing_azimuth[block_idx - 1];
+
+                    azimuth_diff_b = azimuth_diff_b < 0 ? azimuth_diff_b + 36000 : azimuth_diff_b;
+                }
+            } else {
+                //return mode 2
+                if (block_idx < BLOCKS_PER_PACKET - 2) {
+                    azimuth_diff_b = firings.firing_azimuth[block_idx + 2] - firings.firing_azimuth[block_idx];
+                    azimuth_diff_b = azimuth_diff_b < 0 ? azimuth_diff_b + 36000 : azimuth_diff_b;
+                } else {
+                    azimuth_diff_b = firings.firing_azimuth[block_idx] - firings.firing_azimuth[block_idx - 2];
+
+                    azimuth_diff_b = azimuth_diff_b < 0 ? azimuth_diff_b + 36000 : azimuth_diff_b;
+                }
+
+            }
+
+
+            // 32 scan
+            for (size_t scan_fir_idx = 0; scan_fir_idx < SCANS_PER_FIRING_CX; ++scan_fir_idx) {
+                size_t byte_idx = RAW_SCAN_SIZE * scan_fir_idx;
+                //azimuth
+                firings.azimuth[block_idx * 32 + scan_fir_idx] = firings.firing_azimuth[block_idx] +
+                                                                 scan_fir_idx * azimuth_diff_b / FIRING_TOFFSET;
+
+                firings.azimuth[block_idx * 32 + scan_fir_idx] = firings.azimuth[block_idx * 32 + scan_fir_idx] % 36000;
+                // distance
+
+                firings.distance[block_idx * 32 + scan_fir_idx] =
+                        static_cast<float >((packet->data[block_idx * 100 + byte_idx + 4] +
+                                             (packet->data[block_idx * 100 + byte_idx + 5] << 8)) *
+                                            DISTANCE_RESOLUTION *
+                                            distance_unit);
+                //intensity
+                firings.intensity[block_idx * 32 + scan_fir_idx] = static_cast<float>(
+                        packet->data[block_idx * 100 + byte_idx + 6]);
+            }
+        }
+
+        return;
+    }
+
+    bool LslidarDriver::poll() {
+        // Allocate a new shared pointer for zero-copy sharing with other nodelets.
+        lslidar_cx_driver::LslidarPacketPtr packet(new lslidar_cx_driver::LslidarPacket());
+        // Since the rslidar delivers data at a very high rate, keep
+        // reading and publishing scans as fast as possible.
+        while (true) {
+            int rc = msop_input_->getPacket(packet);
+            if (rc == 0) break;
+            if (rc < 0) return false;
+        }
+
+        if (!start_process_msop_) return false;
+        // packet timestamp
+        //lslidar_driver::LslidarPacket pkt = *packet;
+        // 01 08 07(08f)  10  20   46 47  5a
+        if (!is_get_scan_altitude_) {
+            if (packet->data[1211] == 0x01) {
+                for (int i = 0; i < 8; ++i) {
+                    sin_scan_altitude[i] = sin(c1_vertical_angle[i] * DEG_TO_RAD);
+                    cos_scan_altitude[i] = cos(c1_vertical_angle[i] * DEG_TO_RAD);
+                }
+                lidar_type = "c1";
+                lidar_number_ = 1;
+                ROS_INFO("lidar type: c1");
+            } else if (packet->data[1211] == 0x03) {
+                for (int i = 0; i < 8; ++i) {
+                    sin_scan_altitude[i] = sin(c1_vertical_angle[i] * DEG_TO_RAD);
+                    cos_scan_altitude[i] = cos(c1_vertical_angle[i] * DEG_TO_RAD);
+                }
+                lidar_type = "c1p";
+                lidar_number_ = 1;
+                ROS_INFO("lidar type: c1p");
+
+            }else if(packet->data[1211] == 0x06){
+                for (int i = 0; i < 8; ++i) {
+                    sin_scan_altitude[i] = sin(c1_vertical_angle[i] * DEG_TO_RAD);
+                    cos_scan_altitude[i] = cos(c1_vertical_angle[i] * DEG_TO_RAD);
+                }
+                lidar_type = "n301";
+                lidar_number_ = 1;
+		distance_unit = 0.1;
+                ROS_INFO("lidar type: n301");
+            }else if (packet->data[1211] == 0x07) {
+                for (int i = 0; i < 8; ++i) {
+                    sin_scan_altitude[i] = sin(c8f_vertical_angle[i] * DEG_TO_RAD);
+                    cos_scan_altitude[i] = cos(c8f_vertical_angle[i] * DEG_TO_RAD);
+                }
+                lidar_number_ = 8;
+                lidar_type = "c8f";
+                ROS_INFO("lidar type: c8f");
+            } else if (packet->data[1211] == 0x08) {
+                for (int i = 0; i < 8; ++i) {
+                    sin_scan_altitude[i] = sin(c8_vertical_angle[i] * DEG_TO_RAD);
+                    cos_scan_altitude[i] = cos(c8_vertical_angle[i] * DEG_TO_RAD);
+                }
+                lidar_number_ = 8;
+                lidar_type = "c8";
+                ROS_INFO("lidar type: c8");
+            } else if (packet->data[1211] == 0x09) {
+                for (int i = 0; i < 8; ++i) {
+                    sin_scan_altitude[i] = sin(c8s_vertical_angle[i] * DEG_TO_RAD);
+                    cos_scan_altitude[i] = cos(c8s_vertical_angle[i] * DEG_TO_RAD);
+                }
+                lidar_number_ = 8;
+                lidar_type = "c8s";
+                ROS_INFO("lidar type: c8s");
+            } else if (packet->data[1211] == 0x10) {
+                for (int i = 0; i < 16; ++i) {
+                    sin_scan_altitude[i] = sin(c16_vertical_angle[i] * DEG_TO_RAD);
+                    cos_scan_altitude[i] = cos(c16_vertical_angle[i] * DEG_TO_RAD);
+                }
+                lidar_number_ = 16;
+                lidar_type = "c16";
+                ROS_INFO("lidar type: c16");
+            } else if (packet->data[1211] == 0x11) {
+                for (int i = 0; i < 16; ++i) {
+                    sin_scan_altitude[i] = sin(c16_vertical_angle[i] * DEG_TO_RAD);
+                    cos_scan_altitude[i] = cos(c16_vertical_angle[i] * DEG_TO_RAD);
+                }
+                lidar_number_ = 16;
+                lidar_type = "msc16";
+                ROS_INFO("lidar type: msc16");
+            } else if (packet->data[1211] == 0x20) {
+                for (int i = 0; i < 32; ++i) {
+                    sin_scan_altitude[i] = sin(c32_vertical_angle[i] * DEG_TO_RAD);
+                    cos_scan_altitude[i] = cos(c32_vertical_angle[i] * DEG_TO_RAD);
+                }
+                lidar_number_ = 32;
+                lidar_type = "c32";
+                ROS_INFO("lidar type: c32");
+            } else if (packet->data[1211] == 0x45) {
+                for (int k = 0; k < 32; ++k) {
+                    sin_scan_altitude[k] = sin(c32wa_vertical_angle[k] * DEG_TO_RAD);
+                    cos_scan_altitude[k] = cos(c32wa_vertical_angle[k] * DEG_TO_RAD);
+
+                }
+                lidar_number_ = 32;
+                lidar_type = "c32wa";
+                ROS_INFO("lidar type: c32wa");
+
+            } else if (packet->data[1211] == 0x46) {
+                for (int k = 0; k < 32; ++k) {
+                    sin_scan_altitude[k] = sin(c32_70_vertical_angle[k] * DEG_TO_RAD);
+                    cos_scan_altitude[k] = cos(c32_70_vertical_angle[k] * DEG_TO_RAD);
+                }
+                lidar_number_ = 32;
+                lidar_type = "c32w";
+                ROS_INFO("lidar type: c32w");
+            } else if (packet->data[1211] == 0x47) {
+                for (int k = 0; k < 32; ++k) {
+                    sin_scan_altitude[k] = sin(c32_70_vertical_angle2[k] * DEG_TO_RAD);
+                    cos_scan_altitude[k] = cos(c32_70_vertical_angle2[k] * DEG_TO_RAD);
+                }
+                lidar_number_ = 32;
+                lidar_type = "c32w_n";
+                ROS_INFO("lidar type: c32w_n");
+            } else if (packet->data[1211] == 0x5a) {
+                for (int k = 0; k < 32; ++k) {
+                    sin_scan_altitude[k] = sin(c32_90_vertical_angle[k] * DEG_TO_RAD);
+                    cos_scan_altitude[k] = cos(c32_90_vertical_angle[k] * DEG_TO_RAD);
+                }
+                lidar_number_ = 32;
+                lidar_type = "c32r";
+                ROS_INFO("lidar type: c32r");
+            } else {
+                ROS_ERROR("lidar type error,please check lidar model");
+                ros::shutdown();
+                return false;
+            }
+            is_get_scan_altitude_ = true;
+        }
+        if (use_time_service) {
+            if (time_service_mode_ == 1) {    //ptp授时
+                //std::cout << "ptp";
+                uint32_t timestamp_s = packet->data[1201] * pow(2, 32) + (packet->data[1202] << 24) +
+                                       (packet->data[1203] << 16) +
+                                       (packet->data[1204] << 8) + packet->data[1205];
+                uint32_t timestamp_nsce = packet->data[1206] +
+                                          (packet->data[1207] << 8) +
+                                          (packet->data[1208] << 16) +
+                                          (packet->data[1209] << 24); //ns
+                timeStamp = ros::Time(timestamp_s, timestamp_nsce);// s,ns
+                packet->stamp = timeStamp;
+                current_packet_time = timeStamp.toSec();
+            } else if (time_service_mode_ == 0) {          //gps授时
+                memset(&cur_time, 0, sizeof(cur_time));
+                cur_time.tm_year = packet->data[1200] + 2000 - 1900;
+                cur_time.tm_mon = packet->data[1201] - 1;
+                cur_time.tm_mday = packet->data[1202];
+                cur_time.tm_hour = packet->data[1203];
+                cur_time.tm_min = packet->data[1204];
+                cur_time.tm_sec = packet->data[1205];
+                packet_time_s = static_cast<uint64_t>(timegm(&cur_time)); //s
+                packet_time_ns = packet->data[1206] +
+                                 (packet->data[1207] << 8) +
+                                 (packet->data[1208] << 16) +
+                                 (packet->data[1209] << 24); //ns
+                timeStamp = ros::Time(packet_time_s, packet_time_ns);
+                packet->stamp = timeStamp;
+                current_packet_time = timeStamp.toSec();
+            } else if (time_service_mode_ == 2) {          //ntp授时
+                memset(&cur_time, 0, sizeof(cur_time));
+                cur_time.tm_year = packet->data[1200] + 2000 - 1900;
+                cur_time.tm_mon = packet->data[1201] - 1;
+                cur_time.tm_mday = packet->data[1202];
+                cur_time.tm_hour = packet->data[1203];
+                cur_time.tm_min = packet->data[1204];
+                cur_time.tm_sec = packet->data[1205];
+                packet_time_s = static_cast<uint64_t>(timegm(&cur_time)); //s
+                packet_time_ns = (packet->data[1206] +
+                                  (packet->data[1207] << 8) +
+                                  (packet->data[1208] << 16) +
+                                  (packet->data[1209] << 24)) * 1e3; //ns
+                timeStamp = ros::Time(packet_time_s, packet_time_ns);
+                packet->stamp = timeStamp;
+                current_packet_time = timeStamp.toSec();
+            }
+        } else {
+            packet->stamp = ros::Time::now();
+            current_packet_time = packet->stamp.toSec();
+        }
+        if (packet->data[1210] == 0x39) {
+            return_mode = 2;
+        }
+        ROS_INFO_ONCE("return mode: %d", return_mode);
+        //  const RawPacket *raw_packet = (const RawPacket *) (&(packet->data[0]));
+
+        //check if the packet is valid
+        if (!checkPacketValidity(packet)) { return false; }
+
+        //decode the packet
+        decodePacket(packet);
+        // find the start of a new revolution
+        // if there is one, new_sweep_start will be the index of the start firing,
+        // otherwise, new_sweep_start will be FIRINGS_PER_PACKET.
+        size_t new_sweep_start = 0;
+        do {
+            if (abs(firings.azimuth[new_sweep_start] - last_azimuth) > 35000) {
+                break;
+            } else {
+                last_azimuth = firings.azimuth[new_sweep_start];
+                ++new_sweep_start;
+            }
+        } while (new_sweep_start < SCANS_PER_PACKET);
+
+        // The first sweep may not be complete. So, the firings with
+        // the first sweep will be discarded. We will wait for the
+        // second sweep in order to find the 0 azimuth angle.
+        size_t start_fir_idx = 0;
+        size_t end_fir_idx = new_sweep_start;
+        if (is_first_sweep && new_sweep_start == SCANS_PER_PACKET) {
+            return true;
+        } else {
+            if (is_first_sweep) {
+                is_first_sweep = false;
+                start_fir_idx = new_sweep_start;
+                end_fir_idx = SCANS_PER_PACKET;
+                //sweep_end_time = packet->stamp.toSec() - (end_fir_idx - start_fir_idx) * 3.125 * 1e-6;
+            }
+        }
+        for (size_t fir_idx = start_fir_idx; fir_idx < end_fir_idx; ++fir_idx) {
+            if ("c32w" == lidar_type) {
+                if (fir_idx % 32 == 29 || fir_idx % 32 == 6 || fir_idx % 32 == 14 || fir_idx % 32 == 22 ||
+                    fir_idx % 32 == 30 || fir_idx % 32 == 7 || fir_idx % 32 == 15 || fir_idx % 32 == 23) {
+//                    ROS_INFO("firings.azimuth[fir_idx] +=389;");
+                    firings.azimuth[fir_idx] += 389;
+                }
+                if (firings.azimuth[fir_idx] >= 36000) firings.azimuth[fir_idx] -= 36000;
+            }
+            if ("msc16" == lidar_type) {
+                firings.azimuth[fir_idx] += msc16_offset_angle[fir_idx % 16] < 0 ?
+                                            msc16_offset_angle[fir_idx % 16] + 36000 : msc16_offset_angle[fir_idx % 16];
+                if (firings.azimuth[fir_idx] >= 36000) { firings.azimuth[fir_idx] -= 36000; }
+            }
+
+            //check if the point is valid
+            if (!isPointInRange(firings.distance[fir_idx])) { continue; }
+            if (firings.azimuth[fir_idx] > angle_disable_min &&
+                firings.azimuth[fir_idx] < angle_disable_max) { continue; }
+
+            //convert the point to xyz coordinate
+            size_t table_idx = firings.azimuth[fir_idx];
+            float cos_azimuth = cos_azimuth_table[table_idx];
+            float sin_azimuth = sin_azimuth_table[table_idx];
+            float x_coord, y_coord, z_coord;
+            float R1 = 0.0f;
+            if (lidar_type == "c32r") {   // c32r
+                R1 = R1_90;
+            } else if (lidar_type == "c32w_n" || lidar_type == "c32w" || lidar_type == "c32wa") {    //new C32w
+                R1 = R1_C32W;
+            } else {                     //others
+                R1 = R1_;
+            }
+            int conversionAngle = (lidar_type == "c32r") ? conversionAngle_90 : conversionAngle_;
+            if (coordinate_opt) {
+                int tmp_idx = conversionAngle - firings.azimuth[fir_idx] < 0 ?
+                              conversionAngle - firings.azimuth[fir_idx] + 36000 : conversionAngle -
+                                                                                   firings.azimuth[fir_idx];
+                x_coord = firings.distance[fir_idx] * cos_scan_altitude[fir_idx % lidar_number_] * cos_azimuth +
+                          R1 * cos_azimuth_table[tmp_idx];
+                y_coord = -firings.distance[fir_idx] * cos_scan_altitude[fir_idx % lidar_number_] * sin_azimuth +
+                          R1 * sin_azimuth_table[tmp_idx];
+                z_coord = firings.distance[fir_idx] * sin_scan_altitude[fir_idx % lidar_number_];
+
+            } else {
+                //Y-axis correspondence 0 degree
+                int tmp_idx = firings.azimuth[fir_idx] - conversionAngle < 0 ?
+                              firings.azimuth[fir_idx] - conversionAngle + 36000 : firings.azimuth[fir_idx] -
+                                                                                   conversionAngle;
+                x_coord = firings.distance[fir_idx] * cos_scan_altitude[fir_idx % lidar_number_] * sin_azimuth +
+                          R1 * sin_azimuth_table[tmp_idx];
+                y_coord = firings.distance[fir_idx] * cos_scan_altitude[fir_idx % lidar_number_] * cos_azimuth +
+                          R1 * cos_azimuth_table[tmp_idx];
+                z_coord = firings.distance[fir_idx] * sin_scan_altitude[fir_idx % lidar_number_];
+
+            }
+            // computer the time of the point
+            double time;
+            if (last_packet_time > 1e-6) {
+                time =
+                        packet->stamp.toSec() - sweep_end_time -
+                        (current_packet_time - last_packet_time) * (SCANS_PER_PACKET - fir_idx - 1) / SCANS_PER_PACKET;
+            } else {
+                time = current_packet_time;
+            }
+
+            int remapped_scan_idx = 0;
+            switch (lidar_number_) {
+                case 1:
+                    remapped_scan_idx = 0;
+                    break;
+                case 8:
+                    remapped_scan_idx = (fir_idx % 8) % 2 * 4 + (fir_idx % 8) / 2;
+                    break;
+                case 16:
+                    remapped_scan_idx = (fir_idx % 16) % 2 == 0 ? (fir_idx % 16) / 2 : (fir_idx % 16) / 2 + 8;
+                    break;
+                case 32:
+                    remapped_scan_idx = (fir_idx % 32) % 4 * 8 + fir_idx % 32 / 4;
+                    break;
+                default:
+                    remapped_scan_idx = 0;
+                    break;
+            }
+            //add point
+            if (pcl_type) {
+                pcl::PointXYZI point_xyzi;
+                point_xyzi.x = x_coord;
+                point_xyzi.y = y_coord;
+                point_xyzi.z = z_coord;
+                point_xyzi.intensity = firings.intensity[fir_idx];
+                point_cloud_xyzi_->points.push_back(point_xyzi);
+                ++point_cloud_xyzi_->width;
+            } else {
+                VPoint point;
+                point.time = time;
+                point.x = x_coord;
+                point.y = y_coord;
+                point.z = z_coord;
+                point.intensity = firings.intensity[fir_idx];
+                point.ring = remapped_scan_idx;
+                point_cloud_xyzirt_->points.push_back(point);
+                ++point_cloud_xyzirt_->width;
+
+            }
+            if (publish_scan) {
+                if (scan_num == remapped_scan_idx) {
+                    if (coordinate_opt) {
+                        float horizontal_angle = firings.azimuth[fir_idx] * 0.01f * DEG_TO_RAD;
+                        uint point_index = (int) ((horizontal_angle - scan_msg->angle_min) / scan_msg->angle_increment);
+                        point_index = (point_index < point_size) ? point_index : (point_index % point_size);
+                        scan_msg->ranges[point_size - point_index - 1] = firings.distance[fir_idx];
+                        scan_msg->intensities[point_size - point_index - 1] = firings.intensity[fir_idx];
+                    } else {
+                        float h_angle = (45000.0 - firings.azimuth[fir_idx]) < 36000.0 ?
+                                        45000.0 - firings.azimuth[fir_idx] : 9000.0 - firings.azimuth[fir_idx];
+                        // ROS_INFO("a=%f,b=%f",sweep_data->points[j].azimuth,h_angle);
+                        float horizontal_angle = h_angle * 0.01f * DEG_TO_RAD;
+
+                        //float horizontal_angle = firings.azimuth[fir_idx] * 0.01f * DEG_TO_RAD;
+                        uint point_index = (int) ((horizontal_angle - scan_msg->angle_min) / scan_msg->angle_increment);
+                        point_index = (point_index < point_size) ? point_index : (point_index % point_size);
+                        scan_msg->ranges[point_index] = firings.distance[fir_idx];
+                        scan_msg->intensities[point_index] = firings.intensity[fir_idx];
+                    }
+                }
+            }
+
+/*            sweep_data->points.push_back(lslidar_driver::LslidarPoint());
+            lslidar_driver::LslidarPoint &new_point = sweep_data->points[
+                    sweep_data->points.size() - 1];
+            //pack the data into point msg
+            new_point.time = time;
+            new_point.x = x_coord;
+            new_point.y = y_coord;
+            new_point.z = z_coord;
+            new_point.intensity = firings.intensity[fir_idx];
+            new_point.ring = remapped_scan_idx;
+            new_point.azimuth = firings.azimuth[fir_idx];
+            new_point.distance = firings.distance[fir_idx];*/
+        }
+        // a new sweep begins ----------------------------------------------------
+
+        if (end_fir_idx != SCANS_PER_PACKET) {
+            //publish Last frame scan
+            if (last_packet_time > 1e-6) {
+                sweep_end_time =
+                        packet->stamp.toSec() -
+                        (current_packet_time - last_packet_time) * (SCANS_PER_PACKET - end_fir_idx) / SCANS_PER_PACKET;
+            } else {
+                sweep_end_time = current_packet_time;
+            }
+            sweep_end_time = sweep_end_time > 0 ? sweep_end_time : 0;
+            {
+                std::unique_lock<std::mutex> lock(pointcloud_lock);
+                point_cloud_xyzirt_bak_ = point_cloud_xyzirt_;
+                point_cloud_xyzi_bak_ = point_cloud_xyzi_;
+                scan_msg_bak = scan_msg;
+            }
+            std::thread pointcloud_pub_thread([this] { publishPointcloud(); });
+            pointcloud_pub_thread.detach();
+
+            if (publish_scan) {
+                std::thread laserscan_pub_thread(&LslidarDriver::publishScan, this);
+                laserscan_pub_thread.detach();
+            };
+            point_cloud_xyzirt_.reset(new pcl::PointCloud<VPoint>);
+            point_cloud_xyzi_.reset(new pcl::PointCloud<pcl::PointXYZI>);
+            point_cloud_xyzirt_->header.frame_id = frame_id;
+            point_cloud_xyzirt_->height = 1;
+
+            point_cloud_xyzi_->header.frame_id = frame_id;
+            point_cloud_xyzi_->height = 1;
+
+            if (publish_scan) {
+                scan_msg.reset(new sensor_msgs::LaserScan);
+                scan_msg->angle_min = 0;
+                scan_msg->angle_max = M_PI * 2;
+                scan_msg->range_min = min_range;
+                scan_msg->range_max = max_range;
+                scan_msg->angle_increment = horizontal_angle_resolution * DEG_TO_RAD;
+                point_size = ceil((scan_msg->angle_max - scan_msg->angle_min) / scan_msg->angle_increment);
+
+                scan_msg->ranges.assign(point_size, std::numeric_limits<float>::quiet_NaN());
+                scan_msg->intensities.assign(point_size, std::numeric_limits<float>::quiet_NaN());
+            }
+
+            //prepare the next frame scan
+            //sweep_end_time = packet->stamp.toSec() - (end_fir_idx - start_fir_idx) * 3.125 * 1e-6;
+            last_azimuth = firings.azimuth[SCANS_PER_PACKET - 1];
+            start_fir_idx = end_fir_idx;
+            end_fir_idx = SCANS_PER_PACKET;
+            for (size_t fir_idx = start_fir_idx; fir_idx < end_fir_idx; ++fir_idx) {
+                if ("c32w" == lidar_type) {
+                    if (fir_idx % 32 == 29 || fir_idx % 32 == 6 || fir_idx % 32 == 14 || fir_idx % 32 == 22 ||
+                        fir_idx % 32 == 30 || fir_idx % 32 == 7 || fir_idx % 32 == 15 || fir_idx % 32 == 23) {
+//                    ROS_INFO("firings.azimuth[fir_idx] +=389;");
+                        firings.azimuth[fir_idx] += 389;
+                    }
+                    if (firings.azimuth[fir_idx] >= 36000) firings.azimuth[fir_idx] -= 36000;
+                }
+                if ("msc16" == lidar_type) {
+                    firings.azimuth[fir_idx] += msc16_offset_angle[fir_idx % 16] < 0 ?
+                                                msc16_offset_angle[fir_idx % 16] + 36000 : msc16_offset_angle[fir_idx %
+                                                                                                              16];
+                    if (firings.azimuth[fir_idx] >= 36000) { firings.azimuth[fir_idx] -= 36000; }
+                }
+
+                //check if the point is valid
+                if (!isPointInRange(firings.distance[fir_idx])) { continue; }
+                if (firings.azimuth[fir_idx] > angle_disable_min &&
+                    firings.azimuth[fir_idx] < angle_disable_max) { continue; }
+                //convert the point to xyz coordinate
+                size_t table_idx = firings.azimuth[fir_idx];
+                float cos_azimuth = cos_azimuth_table[table_idx];
+                float sin_azimuth = sin_azimuth_table[table_idx];
+                float x_coord, y_coord, z_coord;
+                float R1 = 0.0;
+                if (lidar_type == "c32r") {   // c32r
+                    R1 = R1_90;
+                } else if (lidar_type == "c32w_n" || lidar_type == "c32w" || lidar_type == "c32wa") {    //new C32w
+                    R1 = R1_C32W;
+                } else {                     //others
+                    R1 = R1_;
+                }
+                int conversionAngle = (lidar_type == "c32r") ? conversionAngle_90 : conversionAngle_;
+
+                if (coordinate_opt) {
+                    int tmp_idx = conversionAngle - firings.azimuth[fir_idx] < 0 ?
+                                  conversionAngle - firings.azimuth[fir_idx] + 36000 : conversionAngle -
+                                                                                       firings.azimuth[fir_idx];
+                    x_coord = firings.distance[fir_idx] * cos_scan_altitude[fir_idx % lidar_number_] * cos_azimuth +
+                              R1 * cos_azimuth_table[tmp_idx];
+                    y_coord = -firings.distance[fir_idx] * cos_scan_altitude[fir_idx % lidar_number_] * sin_azimuth +
+                              R1 * sin_azimuth_table[tmp_idx];
+                    z_coord = firings.distance[fir_idx] * sin_scan_altitude[fir_idx % lidar_number_];
+
+                } else {
+                    //Y-axis correspondence 0 degree
+                    int tmp_idx = firings.azimuth[fir_idx] - conversionAngle < 0 ?
+                                  firings.azimuth[fir_idx] - conversionAngle + 36000 : firings.azimuth[fir_idx] -
+                                                                                       conversionAngle;
+                    x_coord = firings.distance[fir_idx] * cos_scan_altitude[fir_idx % lidar_number_] * sin_azimuth +
+                              R1 * sin_azimuth_table[tmp_idx];
+                    y_coord = firings.distance[fir_idx] * cos_scan_altitude[fir_idx % lidar_number_] * cos_azimuth +
+                              R1 * cos_azimuth_table[tmp_idx];
+                    z_coord = firings.distance[fir_idx] * sin_scan_altitude[fir_idx % lidar_number_];
+
+                }
+                // computer the time of the point
+                double time;
+                if (last_packet_time > 1e-6) {
+                    time = (current_packet_time - last_packet_time) * (fir_idx - start_fir_idx) /
+                           SCANS_PER_PACKET;
+                } else {
+                    time = current_packet_time;
+                }
+
+                int remapped_scan_idx = 0;
+                switch (lidar_number_) {
+                    case 1:
+                        remapped_scan_idx = 0;
+                        break;
+                    case 8:
+                        remapped_scan_idx = (fir_idx % 8) % 2 * 4 + (fir_idx % 8) / 2;
+                        break;
+                    case 16:
+                        remapped_scan_idx = (fir_idx % 16) % 2 == 0 ? (fir_idx % 16) / 2 : (fir_idx % 16) / 2 + 8;
+                        break;
+                    case 32:
+                        remapped_scan_idx = (fir_idx % 32) % 4 * 8 + fir_idx % 32 / 4;
+                        break;
+                    default:
+                        remapped_scan_idx = 0;
+                        break;
+                }
+
+                //add point
+                if (pcl_type) {
+                    pcl::PointXYZI point_xyzi;
+                    point_xyzi.x = x_coord;
+                    point_xyzi.y = y_coord;
+                    point_xyzi.z = z_coord;
+                    point_xyzi.intensity = firings.intensity[fir_idx];
+                    point_cloud_xyzi_->points.push_back(point_xyzi);
+                    ++point_cloud_xyzi_->width;
+                } else {
+                    VPoint point;
+                    point.time = time;
+                    point.x = x_coord;
+                    point.y = y_coord;
+                    point.z = z_coord;
+                    point.intensity = firings.intensity[fir_idx];
+                    point.ring = remapped_scan_idx;
+                    point_cloud_xyzirt_->points.push_back(point);
+                    ++point_cloud_xyzirt_->width;
+
+                }
+                if (publish_scan) {
+                    if (scan_num == remapped_scan_idx) {
+                        if (coordinate_opt) {
+                            float horizontal_angle = firings.azimuth[fir_idx] * 0.01f * DEG_TO_RAD;
+                            uint point_index = (int) ((horizontal_angle - scan_msg->angle_min) /
+                                                      scan_msg->angle_increment);
+                            point_index = (point_index < point_size) ? point_index : (point_index % point_size);
+                            scan_msg->ranges[point_size - point_index - 1] = firings.distance[fir_idx];
+                            scan_msg->intensities[point_size - point_index - 1] = firings.intensity[fir_idx];
+                        } else {
+                            float h_angle = (45000.0 - firings.azimuth[fir_idx]) < 36000.0 ?
+                                            45000.0 - firings.azimuth[fir_idx] : 9000.0 - firings.azimuth[fir_idx];
+                            // ROS_INFO("a=%f,b=%f",sweep_data->points[j].azimuth,h_angle);
+                            float horizontal_angle = h_angle * 0.01f * DEG_TO_RAD;
+
+                            //float horizontal_angle = firings.azimuth[fir_idx] * 0.01f * DEG_TO_RAD;
+                            uint point_index = (int) ((horizontal_angle - scan_msg->angle_min) /
+                                                      scan_msg->angle_increment);
+                            point_index = (point_index < point_size) ? point_index : (point_index % point_size);
+                            scan_msg->ranges[point_index] = firings.distance[fir_idx];
+                            scan_msg->intensities[point_index] = firings.intensity[fir_idx];
+                        }
+
+                    }
+                }
+            }
+        }
+        last_packet_time = current_packet_time;
+        //packet_pub.publish(*packet);
+        return true;
+    }
+
+}  // namespace lslidar_driver

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_cx_driver/src/lslidar_driver_node.cpp

@@ -0,0 +1,43 @@
+
+#include <ros/ros.h>
+#include "lslidar_driver/lslidar_driver.h"
+#include "std_msgs/String.h"
+#include <thread>
+
+using namespace lslidar_driver;
+volatile sig_atomic_t flag = 1;
+//std::string lidar_type;
+
+static void my_handler(int sig) {
+    flag = 0;
+}
+
+int main(int argc, char **argv) {
+    ros::init(argc, argv, "lslidar_driver");
+    ros::NodeHandle node;
+    ros::NodeHandle private_nh("~");
+
+    signal(SIGINT, my_handler);
+   // private_nh.param("lidar_type", lidar_type, std::string("c16"));
+   // ROS_INFO("lslidar type: %s", lidar_type.c_str());
+
+    lslidar_driver::LslidarDriver dvr(node, private_nh);
+    // loop until shut down or end of file
+    if (!dvr.initialize()) {
+        ROS_ERROR("cannot initialize lslidar driver.");
+        return 0;
+    }
+
+    std::thread poll_thread([&]() {
+                                while (ros::ok()) {
+                                    dvr.poll();
+                                    //ros::spinOnce();
+                                }
+                            }
+    );
+    poll_thread.detach();
+    ros::MultiThreadedSpinner spinner(4); // Use 4 threads
+    spinner.spin();
+
+    return 0;
+}

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_cx_driver/src/lslidar_driver_nodelet.cc

@@ -0,0 +1,64 @@
+#include <string>
+#include <boost/thread.hpp>
+
+#include <ros/ros.h>
+#include <pluginlib/class_list_macros.h>
+#include <nodelet/nodelet.h>
+
+#include "lslidar_driver/lslidar_driver.h"
+
+volatile sig_atomic_t flag = 1;
+
+namespace lslidar_driver {
+    class DriverNodelet : public nodelet::Nodelet {
+    public:
+        DriverNodelet() : running_(false) {
+        }
+
+        ~DriverNodelet() {
+            if (running_) {
+                NODELET_INFO("shutting down driver thread");
+                running_ = false;
+                deviceThread_->join();
+                NODELET_INFO("driver thread stopped");
+            }
+        }
+
+    private:
+        virtual void onInit(void);
+
+        virtual void devicePoll(void);
+
+        volatile bool running_;  ///< device thread is running
+        boost::shared_ptr<boost::thread> deviceThread_;
+
+        boost::shared_ptr<LslidarDriver> dvr_;  ///< driver implementation class
+    };
+
+    void DriverNodelet::onInit() {
+        // start the driver
+        dvr_.reset(new LslidarDriver(getNodeHandle(), getPrivateNodeHandle()));
+        // spawn device poll thread
+        running_ = true;
+        deviceThread_ = boost::shared_ptr<boost::thread>(
+                new boost::thread(boost::bind(&DriverNodelet::devicePoll, this)));
+        NODELET_INFO("DriverNodelet onInit");
+    }
+
+/** @brief Device poll thread main loop. */
+    void DriverNodelet::devicePoll() {
+        if(!dvr_->initialize()){
+            NODELET_INFO("cannot init ----");
+            return;
+        }
+        while (ros::ok()) {
+            dvr_->poll();
+            //NODELET_INFO("poll----");
+            ros::spinOnce();
+        }
+    }
+}
+
+//
+// parameters are: class type, base class type
+PLUGINLIB_EXPORT_CLASS(lslidar_driver::DriverNodelet, nodelet::Nodelet)

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_cx_driver/srv/data_ip.srv

@@ -0,0 +1,3 @@
+string data_ip
+---
+bool result

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_cx_driver/srv/data_port.srv

@@ -0,0 +1,3 @@
+int32 data_port
+---
+bool result

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_cx_driver/srv/destination_ip.srv

@@ -0,0 +1,3 @@
+string destination_ip
+---
+bool result

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_cx_driver/srv/dev_port.srv

@@ -0,0 +1,3 @@
+int32 dev_port
+---
+bool result

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_cx_driver/srv/lslidar_control.srv

@@ -0,0 +1,3 @@
+int32 laser_control
+---
+bool result

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_cx_driver/srv/motor_control.srv

@@ -0,0 +1,3 @@
+int32 motor_control
+---
+bool result

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_cx_driver/srv/motor_speed.srv

@@ -0,0 +1,3 @@
+int32 motor_speed
+---
+bool result

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_cx_driver/srv/remove_control.srv

@@ -0,0 +1,3 @@
+int32 remove_control
+---
+bool result

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_cx_driver/srv/time_service.srv

@@ -0,0 +1,4 @@
+string time_service_mode
+string ntp_ip
+---
+bool result

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_ros/LICENSE

@@ -0,0 +1,13 @@
+Copyright 2022 LeiShen Intelligent
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+    http://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_ros/README.md

@@ -0,0 +1,263 @@
+## LSLIDAR_CX_ 3.0_ROS 驱动说明
+
+## 1.工程介绍
+
+​		LSLIDAR_CX_3.0_ROS为linux环境下雷达ros驱动，适用于镭神C16/C32,2.6/2.8/3.0版本的雷达 ，程序在ubuntu18.04 ros melodic , ubuntu20.04 ros noetic 下测试通过。
+
+## 2.适用范围：
+
+* 适用于镭神c16、c32, 2.6\2.8\3.0版本的雷达
+
+  
+
+## 3.依赖
+
+1.ubuntu18.04 ros melodic/ubuntu20.04 ros noetic
+
+2.ros依赖
+
+```bash
+# 安装
+sudo apt-get install ros-$ROS_DISTRO-pcl-ros ros-$ROS_DISTRO-pluginlib  ros-$ROS_DISTRO-pcl-conversions 
+```
+
+3.其他依赖
+
+pcap,boost
+
+~~~bash
+sudo apt-get install libpcap-dev
+sudo apt-get install libboost${BOOST_VERSION}-dev   #选择适合的版本
+~~~
+
+
+
+## 4.编译与运行：
+
+~~~shell
+mkdir -p ~/lslidar_ws/src
+cd ~/lslidar_ws/src
+把驱动压缩包拷贝到src目录下，并解压
+cd ~/lslidar_ws
+catkin_make
+source devel/setup.bash
+roslaunch lslidar_driver lslidar_c16.launch #启动c16雷达
+~~~
+
+
+
+## 5.launch 文件参数说明：
+
+- c16为例
+
+~~~xml
+<launch>
+  <arg name="device_ip" default="192.168.1.200" />  //雷达ip
+  <arg name="msop_port" default="2368"/>   //数据包目的端口
+  <arg name="difop_port" default="2369"/>   //设备包目的端口
+  <arg name="use_gps_ts" default="false" />  //雷达是否使用gps或ptp授时，使用改为true
+  <arg name="pcl_type" default="false" />   //点云类型，默认false点云中的点为xyzirt字段。改为true，点云中的点为xyzi字段。
+  <arg name="lidar_type" default="c16"/>   //选择雷达型号,若为c32雷达改为c32
+  <arg name="packet_size" default="1206"/>  //   udp包长1206或1212,若为1212字节雷达改为1212
+  <arg name="c16_type" default="c16_2"/> //c16_2表示16线垂直角度分辨率为2度的雷达，c16_1表示16线垂直角度分辨率为1.33度的雷达
+  <arg name="c32_type" default="c32_2"/> // c32_2表示32线垂直角度分辨率为1度的雷达，c32_1表示32线垂直角度分辨率为0.33度的雷达
+  <arg name = "c32_fpga_type" default="3"/> //3表示32线fpga为\2.8\3.0的版本的雷达，2表示32线fpga为2.6的版本的雷达
+
+  <node pkg="lslidar_driver" type="lslidar_driver_node" name="lslidar_driver_node" output="screen">
+    <!--param name="pcap" value="$(find lslidar_driver)/pcap/123.pcap" /--> //取消注释（删除!-- --），启用离线pcap模式
+    <param name="device_ip" value="$(arg device_ip)" />
+    <param name="msop_port" value="$(arg msop_port)" />
+    <param name="difop_port" value="$(arg difop_port)"/>
+    <param name="pcl_type" value="$(arg pcl_type)"/>
+    <param name="lidar_type" value="$(arg lidar_type)"/>
+    <param name="packet_size" value="$(arg packet_size)"/>
+    <param name="c16_type" value="$(arg c16_type)"/>
+    <param name="c32_type" value="$(arg c32_type)"/>
+    <param name="c32_fpga_type" value="$(arg c32_fpga_type)"/>
+    <param name="add_multicast" value="false"/>  // 是否开启组播模式。
+    <param name="group_ip" value="224.1.1.2"/>  //组播ip地址
+    <param name="use_gps_ts" value="$(arg use_gps_ts)"/> 
+    <param name="min_range" value="0.15"/>   //单位，米。雷达盲区最小值，小于此值的点被过滤
+    <param name="max_range" value="150.0"/>  //单位，米。雷达盲区最大值 ，大于此值的点被过滤
+    <param name="horizontal_angle_resolution" value="0.2"/>  //雷达水平角度分辨率
+    <param name="frame_id" value="laser_link"/>  //坐标系id
+    <param name="distance_unit" value="0.25"/>   //雷达距离分辨率
+    <param name="angle_disable_min" value="0"/>   //雷达裁剪角度开始值 ，单位0.01°
+    <param name="angle_disable_max" value="0"/>  //雷达裁剪角度结束值，单位0.01°
+    <param name="packet_rate" value="840.0"/>    // 离线播放时参数，c16雷达为840.0（双回波1680.0），c32雷达1700.0（双回波3400.0）
+    <param name="scan_num" value="10"/>          // laserscan线号
+    <param name="read_once" value="false"/>      // pcap离线模式下,是否播放一次
+    <param name="publish_scan" value="false"/>  //是否发布laserscan话题，发布改为true
+    <param name="pointcloud_topic" value="lslidar_point_cloud"/> //点云话题名称，可修改
+    <param name="coordinate_opt" value="false"/> //默认false 雷达零度角对应点云y轴,true雷达零度角对应点云x轴
+  </node>
+
+  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find lslidar_driver)/rviz_cfg/lslidar.rviz"/>  // 运行驱动，同时打开rviz
+ 
+ <!--node pkg="tf" type="static_transform_publisher" name="laser_link_to_world" args="0 0 1 0 0 0 world laser_link 100" /-->  //取消注释（删除!-- --）,静态坐标系转换 
+
+
+~~~
+
+### 组播模式：
+
+- 上位机设置雷达开启组播模式
+
+- 修改launch文件以下参数
+
+  ~~~xml
+      <param name="add_multicast" value="true"/> 
+      <param name="group_ip" value="224.1.1.2"/>  //上位机设置的组播ip地址
+  ~~~
+
+- 运行以下指令将电脑加入组内（将指令中的enp2s0替换为用户电脑的网卡名,可用ifconfig查看网卡名)
+
+  ~~~shell
+  ifconfig
+  sudo route add -net 224.0.0.0/4 dev enp2s0
+  ~~~
+
+
+
+
+
+### 离线pcap模式：
+
+- 把录制好的pcap文件，拷贝到cx_4.0/src/lslidar_ros/lslidar_driver/pcap文件夹下。（cx_4.0是ros工作空间,根据实际工作空间修改）
+
+- 修改launch文件以下参数
+
+  ~~~xml
+     //取消注释
+     <param name="pcap" value="$(find lslidar_driver)/pcap/123.pcap"/>  // 123.pcap改为拷贝的pcap文件名
+  
+     <param name="read_once" value="false"/>      //默认false循环播放，只播放一次改为true
+  ~~~
+
+
+
+###  pcl点云类型：
+
+- 修改launch文件以下参数
+
+  ~~~xml
+    <arg name="pcl_type" default="false" />   //点云类型，默认false点云中的点为xyzirt字段。改为true，点云中的点为xyzi字段。
+  ~~~
+
+  
+
+- 默认false为自定义点云类型，定义参考lslidar_driver/include/lslidar_driver.h头文件
+
+- 改为true,为pcl自带类型 :
+
+  ~~~c++
+  pcl::PointCloud<pcl::PointXYZI>
+  ~~~
+
+  
+
+### c16 上下电功能（适用部分版本）：
+
+- 通过ros service通信控制雷达上下电的功能。
+
+- 另开终端，输入以下命令:   
+
+  ~~~shell
+  source devel/setup.bash  
+  rosservice call /lslidarcontrol 0 # 下电
+  rosservice call /lslidarcontrol 1 # 上电
+  
+  ~~~
+
+  
+
+## FAQ
+
+Bug Report
+
+Original version : LSLIDAR_CX_V3.2.2_220507_ROS
+
+Modify:  original version
+
+Date    : 2022-05-07
+
+---------------
+
+
+
+### 更新版本：
+
+-----
+
+- LSLIDAR_CX_V3.2.3_220520_ROS
+
+- 更新说明: 优化c32雷达垂直角度校准
+
+
+
+### 更新版本：
+
+---------
+
+- LSLIDAR_CX_V3.2.5_220729_ROS
+
+- 更新说明:
+
+  - 设备包和数据包在进秒时没对齐的情况下，增加判断，未及时进秒加1秒，提前进秒减1秒
+  - 修复c32雷达点云缺失的bug
+  - 未接收到设备包之前不发布点云      
+
+
+
+
+### 更新版本：
+
+------
+
+- LSLIDAR_CX_V3.2.6_220905_ROS
+
+- 更新说明：
+  - 兼容1212字节版本的雷达， 通过修改launch文件参数 <arg name="packet_size" default="1206"/>  //   udp包长1206或1212,若为1212字节雷达改为1212
+
+
+
+### 更新版本：
+
+--------------
+
+- LSLIDAR_CX_V3.2.7_221008_ROS
+
+- 更新说明：
+  - 修正laserscan类型话题方向，跟pointcloud2点云保持一致。
+
+### 更新版本：
+
+----------------
+
+- LSLIDAR_CX_V3.2.8_221214_ROS
+
+- 更新说明：
+
+  - 优化1206字节雷达包的时间解析。
+  - 数据包间隔时间通过两包相减插值求点的时间。
+
+  
+
+### 更新版本：
+
+----------------
+
+- LSLIDAR_CX_V3.2.9_221228_ROS
+- 更新说明：
+  - 修复C32雷达补偿角度为负数时导致点云分层的问题。
+
+
+
+### 更新版本：
+
+----------------
+
+- LSLIDAR_CX_V3.3.1_230313_ROS
+
+- 更新说明：
+  - 降低cpu占用；boost库改为标准库；点的时间改为相对时间。

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_ros/README_en.md

@@ -0,0 +1,267 @@
+## LSLIDAR_CX_ 3.0_ROS
+
+## 1.Introduction
+
+​		LSLIDAR_CX_ 3.0_ROS is the lidar ros driver in linux environment, which is suitable for C16/C32,2.6/2.8/3.0 lidar. The program has  tested under ubuntu18.04 ros melodic , ubuntu 20.04 ros noetic.
+
+## 2.Dependencies
+
+1.ros
+
+To run lidar driver in ROS environment, ROS related libraries need to be installed.
+
+**Ubuntu 18.04**: ros-melodic-desktop-full
+
+**Ubuntu 20.04**: ros-noetic-desktop-full
+
+**Installation**: please refer to [http://wiki.ros.org](http://wiki.ros.org/)
+
+2.ros dependencies
+
+```bash
+# install
+sudo apt-get install ros-$ROS_DISTRO-pcl-ros ros-$ROS_DISTRO-pluginlib  ros-$ROS_DISTRO-pcl-conversions 
+```
+
+3.other dependencies
+
+~~~bash
+sudo apt-get install libpcap-dev
+sudo apt-get install libboost${BOOST_VERSION}-dev   #Select the appropriate version
+~~~
+
+## 3.Compile & Run
+
+### 3.1 Compile
+
+~~~bash
+mkdir -p ~/lidar_ws/src
+~~~
+
+Copy the whole lidar ROS driver directory into ROS workspace, i.e "~/lidar_ws/src".
+
+~~~bash
+cd ~/lidar_ws
+catkin_make
+source devel/setup.bash
+~~~
+
+### 3.2 Run
+
+run with single  lidar:
+
+~~~bash
+roslaunch lslidar_driver lslidar_hs1.launch
+~~~
+
+run with double  lidar:
+
+~~~bash
+roslaunch lslidar_driver lslidar_hs1_double.launch
+~~~
+
+
+
+## 4. Introduction to parameters
+
+~~~bash
+<launch>
+    <arg name="device_ip" default="192.168.1.200" />    #lidar ip
+    <arg name="msop_port" default="2368" />   #Main data Stream Output Protocol packet port
+    <arg name="difop_port" default="2369" />  #Device Information Output Protocol packet port
+    <arg name="lidar_type" default="hs1"/>       # lidar type
+    <arg name="pcl_type" default="false"/>        #pointcloud type，false: xyzirt,true:xyzi
+    <arg name="use_gps_ts" default="false" />     # Whether gps time synchronization
+  <arg name="packet_size" default="1206"/>  #  The udp packet length is 1206 or 1212. If it is 1212 bytes, the radar will be changed to 1212
+   <arg name="c16_type" default="c16_2"/>    # c16_ 2 refers to radar with 16-line vertical angle resolution of 2 degrees, c16_ 1 represents radar with 16-line vertical angle resolution of 1.33 degrees
+  <arg name="c32_type" default="c32_2"/>   # c32_ 2 refers to the radar with a vertical angle resolution of 1 degree of 32 lines, c32_ 1 represents a radar with a 32-line vertical angle resolution of 0.33 degrees
+  <arg name = "c32_fpga_type" default="3"/>  # 3 represents the radar with 32-line fpga of version 2.8/ 3.0, and 2 represents the radar with 32-line fpga of version 2.6
+  <node pkg="lslidar_driver" type="lslidar_driver_node" name="lslidar_driver_node" output="screen">
+    <!--param name="pcap" value="$(find lslidar_driver)/pcap/tt.pcap"/-->     #Uncomment to read the data from the pcap file, and add the comment to read the data from the lidar
+    <param name="device_ip" value="$(arg device_ip)" />
+    <param name="msop_port" value="$(arg msop_port)" />
+    <param name="difop_port" value="$(arg difop_port)"/>
+    <param name="pcl_type" value="$(arg pcl_type)"/>
+    <param name="lidar_type" value="$(arg lidar_type)"/>
+    <param name="packet_size" value="$(arg packet_size)"/>
+    <param name="c16_type" value="$(arg c16_type)"/>
+    <param name="c32_type" value="$(arg c32_type)"/>
+    <param name="c32_fpga_type" value="$(arg c32_fpga_type)"/>
+    <param name="add_multicast" value="false"/>    # Whether to add multicast
+    <param name="group_ip" value="224.1.1.2"/>       #multicast ip
+    <param name="frame_id" value="laser_link"/>     # lidar point cloud coordinate system name
+    <param name="min_range" value="0.15"/>         #Unit: m. The minimum value of the lidar blind area, points smaller than this value are filtered
+    <param name="max_range" value="1000.0"/>       #Unit: m. The maximum value of the lidar blind area, points smaller than this value are filtered
+    <param name="packet_rate" value="840.0"/>      #Number of packets played per second when playing pcap
+    <param name="distance_unit" value="0.25"/>   #Radar range resolution
+    <param name="angle_disable_min" value="0"/>      #lidar clipping angle start value ，range [0,180]
+    <param name="angle_disable_max" value="0"/>      #lidar clipping angle end value ，range [0,180]  
+    <param name="horizontal_angle_resolution" value="0.2"/>   #Radar horizontal angle resolution
+     <param name="use_gps_ts" value="$(arg use_gps_ts)"/> 
+     <param name="scan_num" value="10"/>          #Laserscan line number
+    <param name="read_once" value="false"/>      #Whether to play once in pcap offline mode
+    <param name="publish_scan" value="false"/>      #Whether to publish the scan
+      <param name="pointcloud_topic" value="lslidar_point_cloud"/> #point cloud topic name, can be modified
+    <param name="coordinate_opt" value="false"/> //By default, false radar zero-degree angle corresponds to the y-axis of the point cloud, and true radar zero-degree angle corresponds to the x-axis of the point cloud
+   
+  </node>
+</launch>
+~~~
+
+### Multicast mode:
+
+- The host computer sets the lidar to enable multicast mode
+
+- Modify the following parameters of the launch file
+
+  ~~~xml
+  add_multicast: true
+  group_ip: 224.1.1.2    //The multicast ip address set by the host computer
+  ~~~
+
+- Run the following command to add the computer to the group (replace enp2s0 in the command with the network card name of the user's computer, you can use ifconfig to view the network card name)
+
+  ~~~shell
+  ifconfig
+  sudo route add -net 224.0.0.0/4 dev enp2s0
+  ~~~
+
+
+
+### Offline pcap mode:
+
+- Modify the following parameters of the launch file
+
+  ~~~xml
+  // uncomment
+      <param name="pcap" value="$(find lslidar_driver)/pcap/tt.pcap"/>   
+  #Uncomment to read the data from the pcap file, and add the comment to read the data from the lidar                 
+  ~~~
+
+
+
+###  pcl point cloud type:
+
+- Modify the following parameters of the launch file
+
+  ~~~xml
+  pcl_type: false      # pointcloud type，false: xyzirt,true:xyzi
+  ~~~
+
+- The default false is the custom point cloud type, which references the definition in the file of
+
+  lslidar_driver/include/lslidar_driver.h
+
+  Change it to true, which is the own type of pcl:
+
+  ~~~c++
+  pcl::PointCloud<pcl::PointXYZI>
+  ~~~
+
+
+
+## FAQ
+
+Bug Report
+
+Original version : LSLIDAR_CX_V3.2.2_220507_ROS
+
+Modify:  original version
+
+Date    : 2022-05-07
+
+
+
+### Updated version:
+
+-----
+
+- LSLIDAR_ CX_ V3.2.3_ 220520_ ROS
+
+- Update description: optimize vertical angle calibration of c32 radar
+
+
+
+### Updated version:
+
+---------
+
+- LSLIDAR_ CX_ V3.2.5_ 220729_ ROS
+
+- Update description:
+
+- If the device packet and data packet are not aligned when entering the second, increase the judgment, add 1 second if the second is not entered in time, and subtract 1 second if the second is entered in advance
+
+- Fix the missing bug of c32 radar point cloud
+
+- Do not publish the point cloud before receiving the device package
+
+
+
+
+
+### Updated version:
+
+------
+
+- LSLIDAR_ CX_ V3.2.6_ 220905_ ROS
+
+- Update description:
+
+- Compatible with 1212-byte version of radar, by modifying the launch file parameter<arg name="packet_size" default="1206"/>//udp package length is 1206 or 1212, if 1212-byte radar is 1212
+
+
+
+
+
+### Updated version:
+
+--------------
+
+- LSLIDAR_ CX_ V3.2.7_ 221008_ ROS
+
+
+
+- Update description:
+
+- Revise the direction of laserscan type topics to keep consistent with pointcloud2 point cloud.
+
+
+
+### Updated version:
+
+----------------
+
+- LSLIDAR_ CX_ V3.2.8_ 221214_ ROS
+
+- Update description:
+
+- Optimize the time resolution of 1206-byte radar packet.
+
+- The time between data packets is calculated by subtracting and interpolating two packets.
+
+
+
+
+
+### Updated version:
+
+----------------
+
+- LSLIDAR_ CX_ V3.2.9_ 221228_ ROS
+
+- Update description:
+
+- Fix the problem of point cloud delamination when the C32 radar compensation angle is negative.
+
+
+
+### Updated version:
+
+----------------
+
+- LSLIDAR_ CX_ V3.3.1_ 230313_ ROS
+
+- Update description:
+
+- Reduce cpu usage, boost library to standard library,point time to relative time .

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_ros/lslidar_driver/CMakeLists.txt

@@ -0,0 +1,104 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(lslidar_driver)
+
+add_compile_options(-std=c++14)
+set(CMAKE_CXX_STANDARD 14)
+
+set(CMAKE_BUILD_TYPE Release)#RelWithDebInfo
+set(libpcap_LIBRARIES -lpcap)
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+set(${PROJECT_NAME}_CATKIN_DEPS
+    angles
+    pcl_ros
+    roscpp
+    sensor_msgs
+    tf
+    dynamic_reconfigure
+    lslidar_msgs
+    nodelet
+)
+
+
+
+find_package(catkin REQUIRED COMPONENTS
+	${${PROJECT_NAME}_CATKIN_DEPS}
+        pcl_conversions
+        rospy
+		roscpp
+		pluginlib
+		lslidar_msgs
+	    std_msgs
+        genmsg
+        message_generation
+)
+
+find_package(Boost COMPONENTS signals)
+find_package(Boost REQUIRED COMPONENTS thread)
+find_package(PkgConfig REQUIRED)
+
+
+include_directories(
+		include
+		${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS}
+	${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake)
+
+
+catkin_package(
+    CATKIN_DEPENDS ${${PROJECT_NAME}_CATKIN_DEPS}
+		message_runtime std_msgs
+		roscpp sensor_msgs pluginlib nodelet
+		pcl_ros pcl_conversions
+		lslidar_msgs
+		DEPENDS
+		Boost
+    )
+
+
+add_library(lslidar_input src/input.cpp)
+target_link_libraries(lslidar_input
+		${catkin_LIBRARIES}
+		${libpcap_LIBRARIES})
+
+add_library(lslidar_driver src/lslidar_driver.cpp)
+
+target_link_libraries(lslidar_driver
+		lslidar_input
+		${catkin_LIBRARIES})
+
+# build the nodelet version
+add_library(lslidar_driver_nodelet src/lslidar_driver_nodelet.cc src/lslidar_driver.cpp)
+target_link_libraries(lslidar_driver_nodelet
+		lslidar_input
+		${catkin_LIBRARIES}
+		)
+
+add_executable(lslidar_driver_node src/lslidar_driver_node.cpp)
+
+if(catkin_EXPORTED_TARGETS)
+	add_dependencies(lslidar_input ${catkin_EXPORTED_TARGETS})
+endif()
+
+target_link_libraries(lslidar_driver_node
+		lslidar_driver
+		lslidar_input
+		${catkin_LIBRARIES}
+		${libpcap_LIBRARIES}
+		)
+
+
+install(TARGETS lslidar_input lslidar_driver lslidar_driver_nodelet lslidar_driver_node
+		ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+		LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+		RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+		)
+
+
+install(DIRECTORY launch
+		DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+		)
+install(FILES
+		nodelet_lslidar.xml
+		DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+		)

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_ros/lslidar_driver/include/lslidar_driver/input.h

@@ -0,0 +1,114 @@
+/******************************************************************************
+ * This file is part of lslidar_cx driver.
+ *
+ * Copyright 2022 LeiShen Intelligent Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#ifndef __LSLIDAR_INPUT_H_
+#define __LSLIDAR_INPUT_H_
+
+#include <unistd.h>
+#include <stdio.h>
+#include <pcap.h>
+#include <netinet/in.h>
+#include <ros/ros.h>
+#include <lslidar_msgs/LslidarPacket.h>
+#include <string>
+#include <sstream>
+#include <sys/socket.h>
+#include <arpa/inet.h>
+#include <poll.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <sys/file.h>
+#include <signal.h>
+#include <sensor_msgs/TimeReference.h>
+
+namespace lslidar_driver {
+    static uint16_t MSOP_DATA_PORT_NUMBER = 2368;   // lslidar default data port on PC
+    static uint16_t DIFOP_DATA_PORT_NUMBER = 2369;  // lslidar default difop data port on PC
+/**
+ *  从在线的网络数据或离线的网络抓包数据（pcap文件）中提取出lidar的原始数据，即packet数据包
+ * @brief The Input class,
+     *
+     * @param private_nh  一个NodeHandled,用于通过节点传递参数
+     * @param port
+     * @returns 0 if successful,
+     *          -1 if end of file
+     *          >0 if incomplete packet (is this possible?)
+ */
+    class Input {
+    public:
+        Input(ros::NodeHandle private_nh, uint16_t port);
+
+        virtual ~Input() {
+        }
+
+        virtual int getPacket(lslidar_msgs::LslidarPacketPtr &pkt) = 0;
+
+
+    protected:
+        ros::NodeHandle private_nh_;
+        uint16_t port_;
+        std::string devip_str_;
+
+        bool add_multicast;
+        std::string group_ip;
+        int packet_size_;
+    };
+
+/** @brief Live lslidar input from socket. */
+    class InputSocket : public Input {
+    public:
+        InputSocket(ros::NodeHandle private_nh, uint16_t port = MSOP_DATA_PORT_NUMBER);
+
+        virtual ~InputSocket();
+
+        virtual int getPacket(lslidar_msgs::LslidarPacketPtr &pkt);
+
+
+    private:
+        int sockfd_;
+        in_addr devip_;
+
+    };
+
+/** @brief lslidar input from PCAP dump file.
+   *
+   * Dump files can be grabbed by libpcap
+   */
+    class InputPCAP : public Input {
+    public:
+        InputPCAP(ros::NodeHandle private_nh, uint16_t port = MSOP_DATA_PORT_NUMBER, double packet_rate = 0.0,
+                  std::string filename = "", bool read_once = false, bool read_fast = false, double repeat_delay = 0.0);
+
+        virtual ~InputPCAP();
+
+        virtual int getPacket(lslidar_msgs::LslidarPacketPtr &pkt);
+
+    private:
+        ros::Rate packet_rate_;
+        std::string filename_;
+        pcap_t *pcap_;
+        bpf_program pcap_packet_filter_;
+        char errbuf_[PCAP_ERRBUF_SIZE];
+        bool empty_;
+        bool read_once_;
+        bool read_fast_;
+        double repeat_delay_;
+    };
+}
+
+#endif  // __LSLIDAR_INPUT_H

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_ros/lslidar_driver/include/lslidar_driver/lslidar_driver.h

@@ -0,0 +1,304 @@
+/******************************************************************************
+ * This file is part of lslidar_cx driver.
+ *
+ * Copyright 2022 LeiShen Intelligent Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+
+#ifndef _LS_C16_DRIVER_H_
+#define _LS_C16_DRIVER_H_
+
+#define DEG_TO_RAD 0.017453293
+#define RAD_TO_DEG 57.29577951
+
+#include "input.h"
+#include <string>
+#include <ros/ros.h>
+#include <ros/package.h>
+#include <std_msgs/Int32.h>
+#include <pcl/point_types.h>
+#include <pcl_ros/impl/transforms.hpp>
+#include <pcl_conversions/pcl_conversions.h>
+#include <boost/shared_ptr.hpp>
+#include <thread>
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/LaserScan.h>
+#include <pcl_ros/point_cloud.h>
+#include <lslidar_msgs/LslidarPacket.h>
+#include <lslidar_msgs/LslidarPoint.h>
+#include <lslidar_msgs/LslidarScan.h>
+#include <lslidar_msgs/LslidarPacket.h>
+#include <lslidar_msgs/lslidar_control.h>
+
+
+namespace lslidar_driver {
+//raw lslidar packet constants and structures
+    static const int SIZE_BLOCK = 100;
+    static const int RAW_SCAN_SIZE = 3;
+    static const int SCANS_PER_BLOCK = 32;
+    static const int BLOCK_DATA_SIZE = SCANS_PER_BLOCK * RAW_SCAN_SIZE;
+    static const double DISTANCE_RESOLUTION = 0.01; //meters
+    static const uint16_t UPPER_BANK = 0xeeff;
+
+// special defines for lslidarlidar support
+    static const int FIRING_TOFFSET = 32;
+    static const int PACKET_SIZE = 1206;
+    static const int BLOCKS_PER_PACKET = 12;
+    static const int SCANS_PER_PACKET = SCANS_PER_BLOCK  * BLOCKS_PER_PACKET; //384
+    //unit:meter
+    static const double R1_ = 0.04319;
+    static const double R2_ = 0.0494;
+// Pre-compute the sine and cosine for the altitude angles.
+    //1.33
+    static const double c16_1_vertical_angle[16] = {-10, 0.665, -8.665, 2, -7.33, 3.33, -6, 4.665, -4.665, 6, -3.33,
+                                                    7.33, -2, 8.665, -0.665, 10};
+    //2°
+    static const double c16_2_vertical_angle[16] = {-15, 1, -13, 3, -11, 5, -9, 7, -7, 9, -5, 11, -3, 13, -1, 15};
+
+
+    // 0.33°
+    static const double c32_1_vertical_angle[32] = {-18, -1, -15, -0.66, -12, -0.33, -10, 0, -8, 0.33, -7, 0.66, -6, 1,
+                                                    -5, 1.33, -4, 1.66, -3.33, 2, -3, 3, -2.66, 4, -2.33, 6, -2, 8,
+                                                    -1.66, 11, -1.33, 14};
+    static const double c32_1_vertical_angle_26[32] = {-18, -15, -12, -10, -8, -7, -6, -5, -4, -3.33, -2.66, -3,
+                                                       -2.33, -2, -1.33, -1.66, -1, -0.66, 0, -0.33, 0.33, 0.66,
+                                                       1.33, 1, 1.66, 2, 3, 4, 6, 8, 11, 14};
+    // 1°
+    static const double c32_2_vertical_angle[32] = {-16, 0, -15, 1, -14, 2, -13, 3, -12, 4, -11, 5, -10, 6, -9, 7, -8,
+                                                    8, -7, 9, -6, 10, -5, 11, -4, 12, -3, 13, -2, 14, -1, 15};
+
+    static const double c32_2_vertical_angle_26[32] = {-16, -15, -14, -13, -12, -11, -10, -9, -8, -7, -6, -5, -4,
+                                                       -3, -2, -1, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
+                                                       15};
+    static const uint8_t adjust_angle_index[32] = {0, 16, 1, 17, 2, 18, 3, 19, 4, 20, 5, 21, 6, 22, 7, 23, 8, 24, 9,
+                                                   25, 10, 26, 11, 27, 12, 28, 13, 29, 14, 30, 15, 31};
+
+
+/*    union TwoBytes {
+        uint16_t distance;
+        uint8_t bytes[2];
+    };*/
+
+    struct RawBlock {
+        uint16_t header;
+        uint16_t rotation;  //0-35999
+        uint8_t data[BLOCK_DATA_SIZE];
+    };
+
+    struct RawPacket {
+        RawBlock blocks[BLOCKS_PER_PACKET];
+        uint32_t time_stamp;
+        uint8_t factory[2];
+    };
+
+    struct Firing {
+        uint16_t firing_azimuth[BLOCKS_PER_PACKET];
+        int azimuth[SCANS_PER_PACKET];
+        double distance[SCANS_PER_PACKET];
+        double intensity[SCANS_PER_PACKET];
+    };
+
+
+
+    struct PointXYZIRT {
+        PCL_ADD_POINT4D
+        float intensity;
+        std::uint16_t ring;
+        float time;
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW //make sure our new allocators are aligned
+    }EIGEN_ALIGN16; //enforce SSE padding for correct memory alignment
+
+    static std::string lidar_type;
+
+
+    class lslidarDriver {
+    public:
+        lslidarDriver(ros::NodeHandle &node, ros::NodeHandle &private_nh);
+
+        virtual ~lslidarDriver() {}
+
+        bool checkPacketValidity(const RawPacket *packet);
+
+        //check if a point is in the required range
+        bool isPointInRange(const double &distance);
+
+        bool loadParameters();
+
+        void initTimeStamp();
+
+        bool createRosIO();
+
+        void publishPointcloud();
+
+        void publishScan();
+
+
+        bool lslidarC16Control(lslidar_msgs::lslidar_control::Request &req,
+                               lslidar_msgs::lslidar_control::Response &res);
+
+        bool SendPacketTolidar(bool power_switch);
+
+        virtual void difopPoll() = 0;
+
+        virtual bool poll() = 0;
+
+        virtual bool initialize() = 0;
+
+        virtual void decodePacket(const RawPacket *packet) = 0;
+
+
+    public:
+        int socket_id;
+        int msop_udp_port;
+        int difop_udp_port;
+        int scan_num;
+        int angle_disable_min;
+        int angle_disable_max;
+        uint16_t last_azimuth;
+        uint16_t last_azimuth_tmp;
+        uint64_t packet_time_s;
+        uint64_t packet_time_ns;
+        int return_mode;
+        int c32_fpga_type;
+        int config_vert_num;
+        int count;
+        int packet_size;
+
+        in_addr lidar_ip;
+        std::string lidar_ip_string;
+        std::string group_ip_string;
+        std::string frame_id;
+        std::string dump_file;
+        std::string pointcloud_topic;
+        std::string c32_type;
+        std::string c16_type;
+
+        bool use_gps_ts;
+        bool pcl_type;
+        bool publish_scan;
+        bool coordinate_opt;
+        bool is_first_sweep;
+        bool add_multicast;
+        bool config_vert;
+        bool is_update_gps_time;
+        double distance_unit;
+        double min_range;
+        double max_range;
+        double sweep_end_time;
+        double horizontal_angle_resolution;
+        double packet_rate;
+        //double packet_start_time;
+        double packet_interval_time;
+        double packet_timestamp;
+        double last_packet_timestamp;
+
+        double cos_azimuth_table[36000];
+        double sin_azimuth_table[36000];
+
+
+        std::shared_ptr<Input> msop_input_;
+        std::shared_ptr<Input> difop_input_;
+        std::shared_ptr<std::thread> difop_thread_;
+
+        lslidar_msgs::LslidarScanPtr sweep_data;
+        ros::NodeHandle nh;
+        ros::NodeHandle pnh;
+        //ros::Publisher packet_pub;
+        ros::Publisher pointcloud_pub;
+        ros::Publisher scan_pub;
+        ros::ServiceServer lslidar_control;
+
+        unsigned char difop_data[1206];
+        unsigned char packetTimeStamp[10];
+        struct tm cur_time;
+        ros::Time timeStamp;
+        ros::Time timeStamp_bak;
+    };
+
+
+    class lslidarC16Driver : public lslidarDriver {
+    public:
+        /**
+       * @brief lslidarDriver
+       * @param node          raw packet output topic
+       * @param private_nh    通过这个节点传参数
+       */
+        lslidarC16Driver(ros::NodeHandle &node, ros::NodeHandle &private_nh);
+
+        virtual ~lslidarC16Driver();
+
+        virtual void difopPoll();
+
+
+        virtual bool poll(void);
+
+
+        virtual bool initialize();
+
+        virtual void decodePacket(const RawPacket *packet);
+
+
+    private:
+        Firing firings;
+        double c16_vertical_angle[16];
+        double c16_config_vertical_angle[16];
+        //double c16_scan_altitude[16];
+        double c16_cos_scan_altitude[16];
+        double c16_sin_scan_altitude[16];
+
+    };
+
+
+    class lslidarC32Driver : public lslidarDriver {
+    public:
+        lslidarC32Driver(ros::NodeHandle &node, ros::NodeHandle &private_nh);
+
+        virtual ~lslidarC32Driver();
+
+        virtual void difopPoll();
+
+        virtual bool poll(void);
+
+        virtual bool initialize();
+
+        virtual void decodePacket(const RawPacket *packet);
+
+
+    private:
+        Firing firings;
+        double c32_vertical_angle[32];
+        double c32_config_vertical_angle[32];
+        double c32_config_tmp_angle[32];
+        // double c32_scan_altitude[32];
+        double c32_cos_scan_altitude[32];
+        double c32_sin_scan_altitude[32];
+        int adjust_angle[4];
+
+    };
+
+    typedef PointXYZIRT VPoint;
+    typedef pcl::PointCloud<VPoint> VPointcloud;
+
+}  // namespace lslidar_driver
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(lslidar_driver::PointXYZIRT,
+                                  (float, x, x)
+                                          (float, y, y)
+                                          (float, z, z)
+                                          (float, intensity, intensity)
+                                          (std::uint16_t, ring, ring)
+                                          (float, time, time))
+
+#endif

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_ros/lslidar_driver/launch/lslidar_c16.launch

@@ -0,0 +1,56 @@
+<launch>
+  <arg name="device_ip" default="192.168.3.101" />
+  <arg name="msop_port" default="2367"/>
+  <arg name="difop_port" default="2370"/>
+  <arg name="use_gps_ts" default="false" />
+  <arg name="pcl_type" default="false" />
+  <!--c16表示机械式16线雷达；c32表示机械式32线雷达 -->
+  <arg name="lidar_type" default="c16"/>
+ <!--包长1206或1212 -->
+  <arg name="packet_size" default="1206"/>
+
+    <!--c16_2表示16线垂直角度分辨率为2度的雷达，c16_1表示16线垂直角度分辨率为1.33度的雷达 -->
+  <arg name="c16_type" default="c16_2"/>
+
+   <!--c32_2表示32线垂直角度分辨率为1度的雷达，c32_1表示32线垂直角度分辨率为0.33度的雷达 -->
+  <arg name="c32_type" default="c32_2"/>
+
+   <!--3表示32线fpga为\2.8\3.0的版本的雷达，2表示32线fpga为2.6的版本的雷达-->
+  <arg name = "c32_fpga_type" default="3"/>
+
+
+  <node pkg="lslidar_driver" type="lslidar_driver_node" name="lslidar_driver_node" output="screen">
+    <!--param name="pcap" value="$(find lslidar_driver)/pcap/xxx.pcap" /-->
+    <param name="device_ip" value="$(arg device_ip)" />
+    <param name="msop_port" value="$(arg msop_port)" />
+    <param name="difop_port" value="$(arg difop_port)"/>
+    <param name="pcl_type" value="$(arg pcl_type)"/>
+    <param name="lidar_type" value="$(arg lidar_type)"/>
+    <param name="packet_size" value="$(arg packet_size)"/>
+    <param name="c16_type" value="$(arg c16_type)"/>
+    <param name="c32_type" value="$(arg c32_type)"/>
+    <param name="c32_fpga_type" value="$(arg c32_fpga_type)"/>
+    <param name="add_multicast" value="false"/>
+    <param name="group_ip" value="224.1.1.2"/>
+    <param name="use_gps_ts" value="$(arg use_gps_ts)"/>
+    <param name="read_once" value="false"/>
+    <!--<param name="min_range" value="0.15"/>-->
+    <param name="min_range" value="5"/>
+    <param name="max_range" value="150.0"/>
+    <param name="horizontal_angle_resolution" value="0.18"/>
+    <param name="frame_id" value="laser_link"/>
+    <param name="distance_unit" value="0.25"/>
+    <param name="angle_disable_min" value="0"/>
+    <param name="angle_disable_max" value="0"/>
+    <param name="packet_rate" value="840.0"/>
+    <param name="scan_num" value="8"/>
+    <param name="publish_scan" value="false"/>
+    <param name="pointcloud_topic" value="lslidar_point_cloud"/>
+    <param name="coordinate_opt" value="false"/>
+  </node>
+
+  <!--node pkg="rviz" type="rviz" name="rviz" args="-d $(find lslidar_driver)/rviz_cfg/lslidar.rviz"/ -->
+ 
+ <!--node pkg="tf" type="static_transform_publisher" name="laser_link_to_world" args="0 0 1 0 0 0 world laser_link 100" /-->
+  
+</launch>

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_ros/lslidar_driver/launch/lslidar_double_c16.launch

@@ -0,0 +1,92 @@
+
+
+<launch>
+  <arg name="device_ip1" default="192.168.1.200" />
+  <arg name="msop_port1" default="2368"/>
+  <arg name="difop_port1" default="2369"/>
+  <arg name="device_ip2" default="192.168.1.201" />
+  <arg name="msop_port2" default="2370"/>
+  <arg name="difop_port2" default="2371"/>
+  <arg name="use_gps_ts" default="false" />
+  <arg name="pcl_type" default="false" />
+  <!--c16表示机械式16线雷达；c32表示机械式32线雷达 -->
+  <arg name="lidar_type" default="c16"/>
+
+ <!--包长1206或1212 -->
+  <arg name="packet_size" default="1206"/>
+    <!--c16_2表示16线垂直角度分辨率为2度的雷达，c16_1表示16线垂直角度分辨率为1.33度的雷达 -->
+  <arg name="c16_type" default="c16_2"/>
+
+
+   <!--c32_2表示32线垂直角度分辨率为1度的雷达，c32_1表示32线垂直角度分辨率为0.33度的雷达 -->
+  <arg name="c32_type" default="c32_2"/>
+
+
+   <!--3表示32线fpga为2.7\2.8\3.0的版本的雷达，2表示32线fpga为2.6的版本的雷达-->
+  <arg name = "c32_fpga_type" default="3"/>
+
+
+  <node pkg="lslidar_driver" type="lslidar_driver_node" name="lslidar_driver_node" output="screen" ns="c16_1">
+    <!--param name="pcap" value="$(find lslidar_driver)/pcap/xxx.pcap" /-->
+    <param name="device_ip" value="$(arg device_ip1)" />
+    <param name="msop_port" value="$(arg msop_port1)" />
+    <param name="difop_port" value="$(arg difop_port1)"/>
+    <param name="lidar_type" value="$(arg lidar_type)"/>
+     <param name="packet_size" value="$(arg packet_size)"/>
+    <param name="pcl_type" value="$(arg pcl_type)"/>
+    <param name="c16_type" value="$(arg c16_type)"/>
+    <param name="c32_type" value="$(arg c32_type)"/>
+    <param name="c32_fpga_type" value="$(arg c32_fpga_type)"/>
+    <param name="add_multicast" value="false"/>
+    <param name="group_ip" value="224.1.1.2"/>
+    <param name="use_gps_ts" value="$(arg use_gps_ts)"/>
+    <param name="min_range" value="0.15"/>
+    <param name="max_range" value="150.0"/>
+    <param name="horizontal_angle_resolution" value="0.18"/>
+    <param name="frame_id" value="laser_link_left"/>
+    <param name="distance_unit" value="0.25"/>
+    <param name="angle_disable_min" value="0"/>
+    <param name="angle_disable_max" value="0"/>
+    <param name="packet_rate" value="840"/>
+    <param name="scan_num" value="10"/>
+    <param name="publish_scan" value="false"/>
+    <param name="pointcloud_topic" value="lslidar_point_cloud"/>
+    <param name="coordinate_opt" value="false"/>
+  </node>
+
+    <node pkg="lslidar_driver" type="lslidar_driver_node" name="lslidar_driver_node" output="screen" ns="c16_2">
+      <!--param name="pcap" value="$(find lslidar_driver)/pcap/xxx.pcap" /-->
+      <param name="device_ip" value="$(arg device_ip2)" />
+      <param name="msop_port" value="$(arg msop_port2)" />
+      <param name="difop_port" value="$(arg difop_port2)"/>
+      <param name="lidar_type" value="$(arg lidar_type)"/>
+       <param name="packet_size" value="$(arg packet_size)"/>
+      <param name="pcl_type" value="$(arg pcl_type)"/>
+      <param name="c16_type" value="$(arg c16_type)"/>
+      <param name="c32_type" value="$(arg c32_type)"/>
+      <param name="c32_fpga_type" value="$(arg c32_fpga_type)"/>
+      <param name="add_multicast" value="false"/>
+      <param name="group_ip" value="224.1.1.2"/>
+      <param name="use_gps_ts" value="$(arg use_gps_ts)"/>
+      <param name="min_range" value="0.15"/>
+      <param name="max_range" value="150.0"/>
+      <param name="horizontal_angle_resolution" value="0.18"/>
+      <param name="frame_id" value="laser_link_right"/>
+      <param name="distance_unit" value="0.25"/>
+      <param name="angle_disable_min" value="0"/>
+      <param name="angle_disable_max" value="0"/>
+      <param name="packet_rate" value="840"/>
+      <param name="scan_num" value="10"/>
+      <param name="publish_scan" value="false"/>
+      <param name="pointcloud_topic" value="lslidar_point_cloud"/>
+      <param name="coordinate_opt" value="false"/>
+    </node>
+
+  <!--node pkg="rviz" type="rviz" name="rviz" args="-d $(find lslidar_driver)/rviz_cfg/lslidar.rviz"/-->
+  <!--node pkg="tf" type="static_transform_publisher" name="laser_link_right_to_world" args="0 0 0 0 0 0 world laser_link_c16 100" /-->
+  <!--node pkg="tf" type="static_transform_publisher" name="laser_link_left_to_world" args="0 0 1 0 0 0 world laser_link_c32 100" /-->
+
+
+	
+ 
+</launch>

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_ros/lslidar_driver/launch/lslidar_double_c32.launch

@@ -0,0 +1,92 @@
+
+
+<launch>
+  <arg name="device_ip1" default="192.168.1.200" />
+  <arg name="msop_port1" default="2368"/>
+  <arg name="difop_port1" default="2369"/>
+  <arg name="device_ip2" default="192.168.1.201" />
+  <arg name="msop_port2" default="2370"/>
+  <arg name="difop_port2" default="2371"/>
+  <arg name="use_gps_ts" default="false" />
+  <arg name="pcl_type" default="false" />
+  <!--c16表示机械式16线雷达；c32表示机械式32线雷达 -->
+  <arg name="lidar_type" default="c32"/>
+
+ <!--包长1206或1212 -->
+  <arg name="packet_size" default="1206"/>
+    <!--c16_2表示16线垂直角度分辨率为2度的雷达，c16_1表示16线垂直角度分辨率为1.33度的雷达 -->
+  <arg name="c16_type" default="c16_2"/>
+
+
+   <!--c32_2表示32线垂直角度分辨率为1度的雷达，c32_1表示32线垂直角度分辨率为0.33度的雷达 -->
+  <arg name="c32_type" default="c32_2"/>
+
+
+   <!--3表示32线fpga为2.7\2.8\3.0的版本的雷达，2表示32线fpga为2.6的版本的雷达-->
+  <arg name = "c32_fpga_type" default="3"/>
+
+
+  <node pkg="lslidar_driver" type="lslidar_driver_node" name="lslidar_driver_node" output="screen" ns="c32_1">
+    <!--param name="pcap" value="$(find lslidar_driver)/pcap/xxx.pcap" /-->
+    <param name="device_ip" value="$(arg device_ip1)" />
+    <param name="msop_port" value="$(arg msop_port1)" />
+    <param name="difop_port" value="$(arg difop_port1)"/>
+    <param name="lidar_type" value="$(arg lidar_type)"/>
+     <param name="packet_size" value="$(arg packet_size)"/>
+    <param name="pcl_type" value="$(arg pcl_type)"/>
+    <param name="c16_type" value="$(arg c16_type)"/>
+    <param name="c32_type" value="$(arg c32_type)"/>
+    <param name="c32_fpga_type" value="$(arg c32_fpga_type)"/>
+    <param name="add_multicast" value="false"/>
+    <param name="group_ip" value="224.1.1.2"/>
+    <param name="use_gps_ts" value="$(arg use_gps_ts)"/>
+    <param name="min_range" value="0.15"/>
+    <param name="max_range" value="150.0"/>
+    <param name="horizontal_angle_resolution" value="0.18"/>
+    <param name="frame_id" value="laser_link_left"/>
+    <param name="distance_unit" value="0.40"/>
+    <param name="angle_disable_min" value="0"/>
+    <param name="angle_disable_max" value="0"/>
+    <param name="packet_rate" value="1700"/>
+    <param name="scan_num" value="10"/>
+    <param name="publish_scan" value="false"/>
+    <param name="pointcloud_topic" value="lslidar_point_cloud"/>
+    <param name="coordinate_opt" value="false"/>
+  </node>
+
+    <node pkg="lslidar_driver" type="lslidar_driver_node" name="lslidar_driver_node" output="screen" ns="c32_2">
+      <!--param name="pcap" value="$(find lslidar_driver)/pcap/xxx.pcap" /-->
+      <param name="device_ip" value="$(arg device_ip2)" />
+      <param name="msop_port" value="$(arg msop_port2)" />
+      <param name="difop_port" value="$(arg difop_port2)"/>
+      <param name="lidar_type" value="$(arg lidar_type)"/>
+       <param name="packet_size" value="$(arg packet_size)"/>
+      <param name="pcl_type" value="$(arg pcl_type)"/>
+      <param name="c16_type" value="$(arg c16_type)"/>
+      <param name="c32_type" value="$(arg c32_type)"/>
+      <param name="c32_fpga_type" value="$(arg c32_fpga_type)"/>
+      <param name="add_multicast" value="false"/>
+      <param name="group_ip" value="224.1.1.2"/>
+      <param name="use_gps_ts" value="$(arg use_gps_ts)"/>
+      <param name="min_range" value="0.15"/>
+      <param name="max_range" value="150.0"/>
+      <param name="horizontal_angle_resolution" value="0.18"/>
+      <param name="frame_id" value="laser_link_right"/>
+      <param name="distance_unit" value="0.40"/>
+      <param name="angle_disable_min" value="0"/>
+      <param name="angle_disable_max" value="0"/>
+      <param name="packet_rate" value="1700"/>
+      <param name="scan_num" value="10"/>
+      <param name="publish_scan" value="false"/>
+      <param name="pointcloud_topic" value="lslidar_point_cloud"/>
+      <param name="coordinate_opt" value="false"/>
+    </node>
+
+  <!--node pkg="rviz" type="rviz" name="rviz" args="-d $(find lslidar_driver)/rviz_cfg/lslidar.rviz"/-->
+  <!--node pkg="tf" type="static_transform_publisher" name="laser_link_right_to_world" args="0 0 0 0 0 0 world laser_link_c16 100" /-->
+  <!--node pkg="tf" type="static_transform_publisher" name="laser_link_left_to_world" args="0 0 1 0 0 0 world laser_link_c32 100" /-->
+
+
+	
+ 
+</launch>

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_ros/lslidar_driver/nodelet_lslidar.xml

@@ -0,0 +1,9 @@
+<library path="lib/liblslidar_driver_nodelet">
+  <class name="lslidar_driver/DriverNodelet"
+         type="lslidar_driver::DriverNodelet"
+         base_class_type="nodelet::Nodelet">
+    <description>
+      Publish one lslidar raw data packet each time.
+    </description>
+  </class>
+</library>

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_ros/lslidar_driver/package.xml

@@ -0,0 +1,58 @@
+<?xml version="1.0"?>
+<package>
+  <name>lslidar_driver</name>
+  <version>1.0.0</version>
+
+  <maintainer email="lslidar@sina.com">lslidar</maintainer>
+  <author>lslidar</author>
+  <license>BSD</license>
+  <description>
+    ROS device driver for Leishen C16 lidar.
+  </description>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>angles</build_depend>
+  <build_depend>nodelet</build_depend>
+  <build_depend>pcl_conversions</build_depend>
+  <build_depend>pcl_ros</build_depend>
+  <build_depend>pluginlib</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
+  <build_depend>roslaunch</build_depend>
+  <build_depend>rostest</build_depend>
+  <build_depend>tf2_ros</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>rslidar_input</build_depend>
+  <build_depend>pcl_conversions</build_depend>
+  <build_depend>pcl_ros</build_depend>
+  <build_depend>libpcl-all-dev</build_depend>
+  <build_depend>lslidar_msgs</build_depend>
+
+
+  <run_depend>angles</run_depend>
+  <run_depend>pcl_ros</run_depend>
+  <run_depend>nodelet</run_depend>
+  <run_depend>pluginlib</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>dynamic_reconfigure</run_depend>
+  <run_depend>message_runtime</run_depend>
+  <run_depend>pcl_conversions</run_depend>
+  <run_depend>pcl_ros</run_depend>
+  <run_depend>libpcl-all</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>lslidar_msgs</run_depend>
+
+  <export>
+    <nodelet plugin="${prefix}/nodelet_lslidar.xml"/>
+
+  </export>
+
+</package>

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_ros/lslidar_driver/rviz_cfg/lslidar.rviz

@@ -0,0 +1,150 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded: ~
+      Splitter Ratio: 0.5
+    Tree Height: 537
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679016
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: PointCloud2
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.0299999993
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 190
+      Min Color: 0; 0; 0
+      Min Intensity: 2
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.00999999978
+      Style: Points
+      Topic: /lslidar_point_cloud
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz/Axes
+      Enabled: true
+      Length: 1
+      Name: Axes
+      Radius: 0.100000001
+      Reference Frame: <Fixed Frame>
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: laser_link
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 10
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.0599999987
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: false
+      Focal Shape Size: 0.0500000007
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.00999999978
+      Pitch: 0.785398185
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.785398185
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 818
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000002a8fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002a8000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004300000003efc0100000002fb0000000800540069006d00650100000000000004300000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000002c0000002a800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1072
+  X: 361
+  Y: 41

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_ros/lslidar_driver/src/input.cpp

@@ -0,0 +1,294 @@
+#include "lslidar_driver/input.h"
+
+extern volatile sig_atomic_t flag;
+namespace lslidar_driver
+{
+////////////////////////////////////////////////////////////////////////
+// Input base class implementation
+////////////////////////////////////////////////////////////////////////
+
+/** @brief constructor
+ *
+ *  @param private_nh ROS private handle for calling node.
+ *  @param port UDP port number.
+ */
+Input::Input(ros::NodeHandle private_nh, uint16_t port) : private_nh_(private_nh), port_(port)
+{
+
+  private_nh.param("device_ip", devip_str_, std::string(""));
+  private_nh.param<bool>("add_multicast", add_multicast, false);
+  private_nh.param<std::string>("group_ip", group_ip, "224.1.1.2");
+  private_nh.param<int>("packet_size", packet_size_, 1206);
+  if (!devip_str_.empty())
+    ROS_INFO_STREAM_ONCE("Only accepting packets from IP address: " << devip_str_);
+}
+
+
+////////////////////////////////////////////////////////////////////////
+// InputSocket class implementation
+////////////////////////////////////////////////////////////////////////
+
+/** @brief constructor
+   *
+   *  @param private_nh ROS private handle for calling node.
+   *  @param port UDP port number
+*/
+InputSocket::InputSocket(ros::NodeHandle private_nh, uint16_t port) : Input(private_nh, port)
+{
+  sockfd_ = -1;
+
+  if (!devip_str_.empty())
+  {
+    inet_aton(devip_str_.c_str(), &devip_);
+  }
+
+  ROS_INFO_STREAM("Opening UDP socket: port " << port);
+  sockfd_ = socket(PF_INET, SOCK_DGRAM, 0);
+  if (sockfd_ == -1)
+  {
+    perror("socket");  // TODO: ROS_ERROR errno
+    return;
+  }
+
+  int opt = 1;
+  if (setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (const void*)&opt, sizeof(opt)))
+  {
+    perror("setsockopt error!\n");
+    return;
+  }
+
+  sockaddr_in my_addr;                   // my address information
+  memset(&my_addr, 0, sizeof(my_addr));  // initialize to zeros
+  my_addr.sin_family = AF_INET;          // host byte order
+  my_addr.sin_port = htons(port);        // port in network byte order
+  my_addr.sin_addr.s_addr = INADDR_ANY;  // automatically fill in my IP
+
+  if (bind(sockfd_, (sockaddr*)&my_addr, sizeof(sockaddr)) == -1)
+  {
+    perror("bind");  // TODO: ROS_ERROR errno
+    return;
+  }
+
+    if (add_multicast) {
+        struct ip_mreq group;
+        group.imr_multiaddr.s_addr = inet_addr(group_ip.c_str());
+        group.imr_interface.s_addr = htonl(INADDR_ANY);
+        //group.imr_interface.s_addr = inet_addr("192.168.1.102");
+
+        if (setsockopt(sockfd_, IPPROTO_IP, IP_ADD_MEMBERSHIP, (char *) &group, sizeof(group)) < 0) {
+            perror("Adding multicast group error ");
+            close(sockfd_);
+            exit(1);
+        } else
+            printf("Adding multicast group...OK.\n");
+    }
+  if (fcntl(sockfd_, F_SETFL, O_NONBLOCK | FASYNC) < 0)
+  {
+    perror("non-block");
+    return;
+  }
+}
+
+/** @brief destructor */
+InputSocket::~InputSocket(void)
+{
+  (void)close(sockfd_);
+}
+
+/** @brief Get one lslidar packet. */
+int InputSocket::getPacket(lslidar_msgs::LslidarPacketPtr &pkt)
+{
+  struct pollfd fds[1];
+  fds[0].fd = sockfd_;
+  fds[0].events = POLLIN;
+  static const int POLL_TIMEOUT = 3000;  // one second (in msec)
+
+  sockaddr_in sender_address;
+  socklen_t sender_address_len = sizeof(sender_address);
+  while (flag == 1)
+ // while (true)
+  {
+    // Receive packets that should now be available from the
+    // socket using a blocking read.
+    // poll() until input available
+    do
+    {
+      int retval = poll(fds, 1, POLL_TIMEOUT);
+      if (retval < 0)  // poll() error?
+      {
+        if (errno != EINTR)
+          ROS_ERROR("poll() error: %s", strerror(errno));
+        return 1;
+      }
+      if (retval == 0)  // poll() timeout?
+      {
+
+
+        ROS_WARN("lslidar poll() timeout,port %d",port_ );
+        /*
+        char buffer_data[8] = "re-con";
+        memset(&sender_address, 0, sender_address_len);          // initialize to zeros
+        sender_address.sin_family = AF_INET;                     // host byte order
+        sender_address.sin_port = htons(MSOP_DATA_PORT_NUMBER);  // port in network byte order, set any value
+        sender_address.sin_addr.s_addr = devip_.s_addr;          // automatically fill in my IP
+        sendto(sockfd_, &buffer_data, strlen(buffer_data), 0, (sockaddr*)&sender_address, sender_address_len);
+		*/
+        return 1;
+      }
+      if ((fds[0].revents & POLLERR) || (fds[0].revents & POLLHUP) || (fds[0].revents & POLLNVAL))  // device error?
+      {
+        ROS_ERROR("poll() reports lslidar error");
+        return 1;
+      }
+    } while ((fds[0].revents & POLLIN) == 0);
+    ssize_t nbytes = recvfrom(sockfd_, &pkt->data[0], packet_size_, 0, (sockaddr*)&sender_address, &sender_address_len);
+    if (nbytes < 0)
+    {
+      if (errno != EWOULDBLOCK)
+      {
+        perror("recvfail");
+        ROS_INFO("recvfail");
+        return 1;
+      }
+    }
+    else if ((size_t)nbytes == packet_size_)
+    {
+      if (devip_str_ != "" && sender_address.sin_addr.s_addr != devip_.s_addr){
+          ROS_WARN_THROTTLE(2.0,"lidar IP parameter set error,please reset in launch file");
+          continue;
+      }
+      else
+        break;  // done
+    }
+
+    ROS_DEBUG_STREAM("incomplete lslidar packet read: " << nbytes << " bytes");
+  }
+  if (flag == 0)
+  {
+    abort();
+  }
+
+  return 0;
+}
+
+////////////////////////////////////////////////////////////////////////
+// InputPCAP class implementation
+////////////////////////////////////////////////////////////////////////
+
+/** @brief constructor
+   *
+   *  @param private_nh ROS private handle for calling node.
+   *  @param port UDP port number
+   *  @param packet_rate expected device packet frequency (Hz)
+   *  @param filename PCAP dump file name
+   */
+InputPCAP::InputPCAP(ros::NodeHandle private_nh, uint16_t port, double packet_rate, std::string filename,
+                     bool read_once, bool read_fast, double repeat_delay)
+  : Input(private_nh, port), packet_rate_(packet_rate), filename_(filename)
+{
+  pcap_ = nullptr;
+  empty_ = true;
+
+  // get parameters using private node handle
+  private_nh.param("read_once", read_once_, false);
+  private_nh.param("read_fast", read_fast_, false);
+  private_nh.param("repeat_delay", repeat_delay_, 0.0);
+
+  if (read_once_)
+    ROS_INFO("Read input file only once.");
+  if (read_fast_)
+    ROS_INFO("Read input file as quickly as possible.");
+  if (repeat_delay_ > 0.0)
+    ROS_INFO("Delay %.3f seconds before repeating input file.", repeat_delay_);
+
+  // Open the PCAP dump file
+  // ROS_INFO("Opening PCAP file \"%s\"", filename_.c_str());
+  ROS_INFO_STREAM("Opening PCAP file " << filename_);
+  if ((pcap_ = pcap_open_offline(filename_.c_str(), errbuf_)) == nullptr)
+  {
+    ROS_FATAL("Error opening lslidar socket dump file.");
+    return;
+  }
+
+  std::stringstream filter;
+  if (devip_str_ != "")  // using specific IP?
+  {
+    filter << "src host " << devip_str_ << " && ";
+  }
+  filter << "udp dst port " << port;
+  pcap_compile(pcap_, &pcap_packet_filter_, filter.str().c_str(), 1, PCAP_NETMASK_UNKNOWN);
+}
+
+/** destructor */
+InputPCAP::~InputPCAP(void)
+{
+  pcap_close(pcap_);
+}
+
+/** @brief Get one lslidar packet. */
+int InputPCAP::getPacket(lslidar_msgs::LslidarPacketPtr &pkt)
+{
+  struct pcap_pkthdr* header;
+  const u_char* pkt_data;
+
+ // while (flag == 1)
+  while (flag == 1)
+  {
+    int res;
+    if ((res = pcap_next_ex(pcap_, &header, &pkt_data)) >= 0)
+    {
+      // Skip packets not for the correct port and from the
+      // selected IP address.
+      if (!devip_str_.empty() && (0 == pcap_offline_filter(&pcap_packet_filter_, header, pkt_data)))
+        continue;
+
+      // Keep the reader from blowing through the file.
+      if (read_fast_ == false)
+        packet_rate_.sleep();
+
+
+      memcpy(&pkt->data[0], pkt_data + 42, packet_size_);
+
+      if (pkt->data[0] == 0xA5 && pkt->data[1] == 0xFF && pkt->data[2] == 0x00 && pkt->data[3] == 0x5A)
+      {//difop
+        int rpm = (pkt->data[8]<<8)|pkt->data[9];
+        ROS_DEBUG("lidar rpm: %d",rpm);
+      }
+      empty_ = false;
+      return 0;  // success
+    }
+
+    if (empty_)  // no data in file?
+    {
+      ROS_WARN("Error %d reading lslidar packet: %s", res, pcap_geterr(pcap_));
+      return -1;
+    }
+
+    if (read_once_)
+    {
+      ROS_INFO("end of file reached -- done reading.");
+      return -1;
+    }
+
+    if (repeat_delay_ > 0.0)
+    {
+      ROS_INFO("end of file reached -- delaying %.3f seconds.", repeat_delay_);
+      usleep(rint(repeat_delay_ * 1000000.0));
+    }
+
+    ROS_DEBUG("replaying lslidar dump file");
+
+    // I can't figure out how to rewind the file, because it
+    // starts with some kind of header.  So, close the file
+    // and reopen it with pcap.
+    pcap_close(pcap_);
+    pcap_ = pcap_open_offline(filename_.c_str(), errbuf_);
+    empty_ = true;  // maybe the file disappeared?
+  }                 // loop back and try again
+
+  if (flag == 0)
+  {
+    abort();
+  }
+}
+}

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_ros/lslidar_driver/src/lslidar_driver.cpp

@@ -0,0 +1,1435 @@
+/******************************************************************************
+ * This file is part of lslidar_cx driver.
+ *
+ * Copyright 2022 LeiShen Intelligent Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+
+#include "lslidar_driver/lslidar_driver.h"
+#include <std_msgs/String.h>
+
+
+namespace lslidar_driver {
+
+
+    lslidarDriver::lslidarDriver(ros::NodeHandle &node, ros::NodeHandle &private_nh) : nh(node),
+                                                                                       pnh(private_nh),
+                                                                                       socket_id(-1),
+                                                                                       last_azimuth(0),
+                                                                                       sweep_end_time(0.0),
+                                                                                       is_first_sweep(true),
+                                                                                       return_mode(1),
+                                                                                       config_vert(true),
+                                                                                       sweep_data(
+                                                                                               new lslidar_msgs::LslidarScan()) {
+        config_vert_num = 0;
+        count = 0;
+        is_update_gps_time = false;
+        last_packet_timestamp = 0.0;
+        packet_timestamp = 0.0;
+        packet_interval_time = 0.0;
+
+        return;
+    }
+
+    bool lslidarDriver::checkPacketValidity(const lslidar_driver::RawPacket *packet) {
+        for (size_t blk_idx = 0; blk_idx < BLOCKS_PER_PACKET; ++blk_idx) {
+            if (packet->blocks[blk_idx].header != UPPER_BANK) {
+                return false;
+            }
+        }
+        return true;
+    }
+
+    bool lslidarDriver::isPointInRange(const double &distance) {
+        return (distance >= min_range && distance < max_range);
+    }
+
+
+    bool lslidarDriver::loadParameters() {
+        pnh.param("pcap", dump_file, std::string(""));
+        pnh.param<std::string>("frame_id", frame_id, "laser_link");
+        pnh.param<std::string>("lidar_type", lidar_type, "c16");
+        pnh.param<std::string>("c16_type", c16_type, "c16_2");
+        pnh.param<std::string>("c32_type", c32_type, "c32_2");
+        pnh.param<int>("c32_fpga_type", c32_fpga_type, 3);
+        pnh.param<bool>("add_multicast", add_multicast, false);
+        pnh.param<bool>("config_vert", config_vert, true);
+        pnh.param("group_ip", group_ip_string, std::string("234.2.3.2"));
+        pnh.param("device_ip", lidar_ip_string, std::string("192.168.1.200"));
+        pnh.param("msop_port", msop_udp_port, (int) MSOP_DATA_PORT_NUMBER);
+        pnh.param("difop_port", difop_udp_port, (int) DIFOP_DATA_PORT_NUMBER);
+        pnh.param("packet_size", packet_size, 1206);
+        pnh.param("scan_num", scan_num, 8);
+        pnh.param("packet_rate", packet_rate, 840.0);
+        pnh.param<double>("horizontal_angle_resolution", horizontal_angle_resolution, 0.20);
+        pnh.param("min_range", min_range, 0.3);
+        pnh.param("max_range", max_range, 150.0);
+        pnh.param("distance_unit", distance_unit, 0.25);
+        pnh.param("angle_disable_min", angle_disable_min, 0);
+        pnh.param("angle_disable_max", angle_disable_max, 0);
+        pnh.param<bool>("use_gps_ts", use_gps_ts, false);
+        pnh.param<bool>("pcl_type", pcl_type, false);
+        pnh.param<bool>("publish_scan", publish_scan, false);
+        pnh.param<bool>("coordinate_opt", coordinate_opt, false);
+        pnh.param<std::string>("pointcloud_topic", pointcloud_topic, "lslidar_point_cloud");
+        inet_aton(lidar_ip_string.c_str(), &lidar_ip);
+        if (add_multicast) ROS_INFO_STREAM("opening UDP socket: group_address " << group_ip_string);
+
+        ROS_INFO("lidar packet size: %d", packet_size);
+        ROS_INFO("use gps time or not: %d", use_gps_ts);
+        return true;
+    }
+
+    void lslidarDriver::initTimeStamp() {
+        for (int i = 0; i < 10; i++) {
+            this->packetTimeStamp[i] = 0;
+        }
+        this->packet_time_s = 0;
+        this->packet_time_ns = 0;
+        this->timeStamp = ros::Time(0.0);
+        this->timeStamp_bak = ros::Time(0.0);
+    }
+
+    bool lslidarDriver::createRosIO() {
+        pointcloud_pub = nh.advertise<sensor_msgs::PointCloud2>(pointcloud_topic, 10);
+        scan_pub = nh.advertise<sensor_msgs::LaserScan>("scan", 10);
+        lslidar_control = nh.advertiseService("lslidarcontrol", &lslidarC16Driver::lslidarC16Control, this);
+
+        if (dump_file != "") {
+            msop_input_.reset(new lslidar_driver::InputPCAP(pnh, msop_udp_port, packet_rate, dump_file));
+            difop_input_.reset(new lslidar_driver::InputPCAP(pnh, difop_udp_port, 1, dump_file));
+        } else {
+            msop_input_.reset(new lslidar_driver::InputSocket(pnh, msop_udp_port));
+            difop_input_.reset(new lslidar_driver::InputSocket(pnh, difop_udp_port));
+        }
+        difop_thread_ = std::shared_ptr<std::thread>(
+                new std::thread(std::bind(&lslidarDriver::difopPoll, this)));
+
+
+        return true;
+    }
+
+
+    void lslidarDriver::publishPointcloud() {
+        if (sweep_data->points.size() < 65 || !is_update_gps_time) {
+            return;
+        }
+/*        if (pointcloud_pub.getNumSubscribers() == 0) {
+            return;
+        }*/
+
+        if (pcl_type) {
+
+            pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZI>);
+            // pcl_conversions::toPCL(sweep_data->header).stamp;
+            point_cloud->header.frame_id = frame_id;
+            point_cloud->height = 1;
+            point_cloud->header.stamp = static_cast<uint64_t>(sweep_end_time * 1e6);
+            size_t j;
+            pcl::PointXYZI point;
+            if (sweep_data->points.size() > 0) {
+                for (j = 0; j < sweep_data->points.size(); ++j) {
+                    if ((sweep_data->points[j].azimuth > angle_disable_min) and
+                        (sweep_data->points[j].azimuth < angle_disable_max)) {
+                        continue;
+                    }
+                    point.x = sweep_data->points[j].x;
+                    point.y = sweep_data->points[j].y;
+                    point.z = sweep_data->points[j].z;
+                    point.intensity = sweep_data->points[j].intensity;
+                    point_cloud->points.push_back(point);
+                    ++point_cloud->width;
+                }
+            }
+            sensor_msgs::PointCloud2 pc_msg;
+            pcl::toROSMsg(*point_cloud, pc_msg);
+            pc_msg.header.stamp = ros::Time(sweep_end_time);
+            pointcloud_pub.publish(pc_msg);
+        } else {
+            VPointcloud::Ptr point_cloud(new VPointcloud());
+            //pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZI>);
+            // pcl_conversions::toPCL(sweep_data->header).stamp;
+            point_cloud->header.frame_id = frame_id;
+            point_cloud->height = 1;
+            point_cloud->header.stamp = static_cast<uint64_t>(sweep_end_time * 1e6);
+            size_t j;
+            VPoint point;
+            if (sweep_data->points.size() > 0) {
+                for (j = 0; j < sweep_data->points.size(); ++j) {
+                    if ((sweep_data->points[j].azimuth > angle_disable_min) and
+                        (sweep_data->points[j].azimuth < angle_disable_max)) {
+                        continue;
+                    }
+                    point.x = sweep_data->points[j].x;
+                    point.y = sweep_data->points[j].y;
+                    point.z = sweep_data->points[j].z;
+                    point.intensity = sweep_data->points[j].intensity;
+                    point.ring = sweep_data->points[j].ring;
+                    point.time = sweep_data->points[j].time;
+                    point_cloud->points.push_back(point);
+                    ++point_cloud->width;
+                }
+            }
+            sensor_msgs::PointCloud2 pc_msg;
+            pcl::toROSMsg(*point_cloud, pc_msg);
+            pc_msg.header.stamp = ros::Time(sweep_end_time);
+            pointcloud_pub.publish(pc_msg);
+
+        }
+        return;
+    }
+
+
+    void lslidarDriver::publishScan() {
+        if (sweep_data->points.size() < 65) {
+            return;
+        }
+        sensor_msgs::LaserScanPtr scan_msg(new sensor_msgs::LaserScan());
+        scan_msg->header.frame_id = frame_id;
+        int layer_num_local = scan_num;
+        ROS_INFO_ONCE("default channel is %d", layer_num_local);
+
+        scan_msg->header.stamp = ros::Time(sweep_end_time);
+        scan_msg->angle_min = 0.0;
+        scan_msg->angle_max = 2.0 * M_PI;
+        scan_msg->angle_increment = horizontal_angle_resolution * DEG_TO_RAD;
+        scan_msg->range_min = min_range;
+        scan_msg->range_max = max_range;
+
+        uint point_size = ceil((scan_msg->angle_max - scan_msg->angle_min) / scan_msg->angle_increment);
+        scan_msg->ranges.assign(point_size, std::numeric_limits<float>::quiet_NaN());
+        scan_msg->intensities.assign(point_size, std::numeric_limits<float>::quiet_NaN());
+        if (coordinate_opt) {
+            for (size_t j = 0; j < sweep_data->points.size(); ++j) {
+                if (layer_num_local == sweep_data->points[j].ring) {
+                    float horizontal_angle = sweep_data->points[j].azimuth * 0.01 * DEG_TO_RAD;
+                    uint point_index = (int) ((horizontal_angle - scan_msg->angle_min) / scan_msg->angle_increment);
+                    point_index = (point_index <= point_size) ? point_index : (point_index % point_size);
+                    scan_msg->ranges[point_size - point_index - 1] = sweep_data->points[j].distance;
+                    scan_msg->intensities[point_size - point_index - 1] = sweep_data->points[j].intensity;
+                }
+            }
+        } else {
+            for (size_t j = 0; j < sweep_data->points.size(); ++j) {
+                if (layer_num_local == sweep_data->points[j].ring) {
+/*                    float h_angle =
+                            (sweep_data->points[j].azimuth + 9000.0) < 36000.0 ? sweep_data->points[j].azimuth + 9000.0
+                                                                               : sweep_data->points[j].azimuth - 27000.0;
+                    if(h_angle < 18000.0){
+                        h_angle = 18000.0 - h_angle;
+                    }else{
+                        h_angle = 36000.0 - (h_angle - 18000.0);
+                    }*/
+                    float h_angle = (45000.0 - sweep_data->points[j].azimuth) < 36000.0 ? 45000.0 -
+                                                                                          sweep_data->points[j].azimuth
+                                                                                        :
+                                    9000.0 - sweep_data->points[j].azimuth;
+                    // ROS_INFO("a=%f,b=%f",sweep_data->points[j].azimuth,h_angle);
+                    float horizontal_angle = h_angle * 0.01 * DEG_TO_RAD;
+                    uint point_index = (int) ((horizontal_angle - scan_msg->angle_min) / scan_msg->angle_increment);
+                    point_index = (point_index <= point_size) ? point_index : (point_index % point_size);
+                    scan_msg->ranges[point_index] = sweep_data->points[j].distance;
+                    scan_msg->intensities[point_index] = sweep_data->points[j].intensity;
+                }
+
+            }
+
+        }
+
+        scan_pub.publish(scan_msg);
+
+    }
+
+    bool lslidarDriver::lslidarC16Control(lslidar_msgs::lslidar_control::Request &req,
+                                          lslidar_msgs::lslidar_control::Response &res) {
+        ROS_WARN("--------------------------");
+        // sleep(1);
+        lslidar_msgs::LslidarPacketPtr packet0(new lslidar_msgs::LslidarPacket);
+        packet0->data[0] = 0x00;
+        packet0->data[1] = 0x00;
+        int rc_msop = -1;
+
+        if (req.LaserControl == 1) {
+            if ((rc_msop = msop_input_->getPacket(packet0)) == 0) {
+                res.status = 1;
+                ROS_WARN("receive cmd: %d,already power on status", req.LaserControl);
+                return true;
+            }
+            ROS_WARN("receive cmd: %d,power on", req.LaserControl);
+            SendPacketTolidar(true);
+            double time1 = ros::Time::now().toSec();
+            do {
+                rc_msop = msop_input_->getPacket(packet0);
+                double time2 = ros::Time::now().toSec();
+                if (time2 - time1 > 20) {
+                    res.status = 0;
+                    ROS_WARN("lidar connect error");
+                    return true;
+                }
+            } while ((rc_msop != 0) && (packet0->data[0] != 0xff) && (packet0->data[1] != 0xee));
+            sleep(5);
+            res.status = 1;
+        } else if (req.LaserControl == 0) {
+            ROS_WARN("receive cmd: %d,power off", req.LaserControl);
+            SendPacketTolidar(false);
+            res.status = 1;
+        } else {
+            ROS_WARN("cmd error");
+            res.status = 0;
+        }
+        return true;
+
+
+    }
+
+    bool lslidarDriver::SendPacketTolidar(bool power_switch) {
+        int socketid;
+        unsigned char config_data[packet_size];
+        //int data_port = difop_data[24] * 256 + difop_data[25];
+        mempcpy(config_data, difop_data, packet_size);
+        config_data[0] = 0xAA;
+        config_data[1] = 0x00;
+        config_data[2] = 0xFF;
+        config_data[3] = 0x11;
+        config_data[4] = 0x22;
+        config_data[5] = 0x22;
+        config_data[6] = 0xAA;
+        config_data[7] = 0xAA;
+        config_data[8] = 0x02;
+        config_data[9] = 0x58;
+        if (power_switch) {
+            config_data[45] = 0x00;
+        } else {
+            config_data[45] = 0x01;
+        }
+
+        sockaddr_in addrSrv;
+        socketid = socket(2, 2, 0);
+        addrSrv.sin_addr.s_addr = inet_addr(lidar_ip_string.c_str());
+        addrSrv.sin_family = AF_INET;
+        addrSrv.sin_port = htons(2368);
+        sendto(socketid, (const char *) config_data, 1206, 0, (struct sockaddr *) &addrSrv, sizeof(addrSrv));
+        return false;
+
+    }
+
+
+    lslidarC16Driver::lslidarC16Driver(ros::NodeHandle &node, ros::NodeHandle &private_nh) : lslidarDriver(node,
+                                                                                                           private_nh) {
+        return;
+    }
+
+
+    lslidarC16Driver::~lslidarC16Driver() {
+        if (difop_thread_ != nullptr) {
+            difop_thread_->join();
+        }
+    }
+
+
+    bool lslidarC16Driver::initialize() {
+
+        this->initTimeStamp();
+        if (!loadParameters()) {
+            ROS_ERROR("cannot load all required ROS parameters.");
+            return false;
+        }
+        if (c16_type == "c16_2") {
+            for (int j = 0; j < 16; ++j) {
+                c16_vertical_angle[j] = c16_2_vertical_angle[j];
+                c16_config_vertical_angle[j] = c16_2_vertical_angle[j];
+                c16_sin_scan_altitude[j] = sin(c16_vertical_angle[j] * DEG_TO_RAD);
+                c16_cos_scan_altitude[j] = cos(c16_vertical_angle[j] * DEG_TO_RAD);
+            }
+        } else {
+            for (int j = 0; j < 16; ++j) {
+                c16_vertical_angle[j] = c16_1_vertical_angle[j];
+                c16_config_vertical_angle[j] = c16_1_vertical_angle[j];
+                c16_sin_scan_altitude[j] = sin(c16_vertical_angle[j] * DEG_TO_RAD);
+                c16_cos_scan_altitude[j] = cos(c16_vertical_angle[j] * DEG_TO_RAD);
+            }
+        }
+
+
+        if (!createRosIO()) {
+            ROS_ERROR("cannot create all ROS IO.");
+            return false;
+        }
+
+        // create the sin and cos table for different azimuth values
+        for (int j = 0; j < 36000; ++j) {
+            sin_azimuth_table[j] = sin(j * 0.01 * DEG_TO_RAD);
+            cos_azimuth_table[j] = cos(j * 0.01 * DEG_TO_RAD);
+        }
+        return true;
+    }
+
+
+/*    bool lslidarC16Driver::checkPacketValidity(const RawPacket *packet) {
+        for (size_t blk_idx = 0; blk_idx < BLOCKS_PER_PACKET; ++blk_idx) {
+            if (packet->blocks[blk_idx].header != UPPER_BANK) {
+                return false;
+            }
+        }
+        return true;
+    }*/
+
+
+    void lslidarC16Driver::difopPoll() {
+        // reading and publishing scans as fast as possible.
+        lslidar_msgs::LslidarPacketPtr difop_packet_ptr(new lslidar_msgs::LslidarPacket);
+        while (ros::ok()) {
+            // keep reading
+            int rc = difop_input_->getPacket(difop_packet_ptr);
+            if (rc == 0) {
+                if (difop_packet_ptr->data[0] != 0xa5 || difop_packet_ptr->data[1] != 0xff ||
+                    difop_packet_ptr->data[2] != 0x00 || difop_packet_ptr->data[3] != 0x5a) {
+                    return;
+                }
+                count++;
+                if (count == 2) { is_update_gps_time = true; }
+                for (int i = 0; i < 1206; i++) {
+                    difop_data[i] = difop_packet_ptr->data[i];
+                }
+
+                int version_data = difop_packet_ptr->data[1202];
+                if (config_vert) {
+                    if (2 == version_data) {
+                        for (int j = 0; j < 16; ++j) {
+                            uint8_t data1 = difop_packet_ptr->data[234 + 2 * j];
+                            uint8_t data2 = difop_packet_ptr->data[234 + 2 * j + 1];
+                            int vert_angle = data1 * 256 + data2;
+                            vert_angle = vert_angle > 32767 ? (vert_angle - 65535) : vert_angle;
+                            c16_config_vertical_angle[j] = (double) vert_angle * 0.01f;
+                            if (fabs(c16_vertical_angle[j] - c16_config_vertical_angle[j]) > 1.5) {
+                                //c16_config_vertical_angle[j] = c16_vertical_angle[j];
+                                config_vert_num++;
+                            }
+
+                        }
+                        if (config_vert_num == 0) {
+                            for (int k = 0; k < 16; ++k) {
+                                c16_sin_scan_altitude[k] = sin(c16_config_vertical_angle[k] * DEG_TO_RAD);
+                                c16_cos_scan_altitude[k] = cos(c16_config_vertical_angle[k] * DEG_TO_RAD);
+                            }
+                        }
+
+                    } else {
+                        for (int j = 0; j < 16; ++j) {
+                            uint8_t data1 = difop_packet_ptr->data[245 + 2 * j];
+                            uint8_t data2 = difop_packet_ptr->data[245 + 2 * j + 1];
+                            int vert_angle = data1 * 256 + data2;
+                            vert_angle = vert_angle > 32767 ? (vert_angle - 65535) : vert_angle;
+                            c16_config_vertical_angle[j] = (double) vert_angle * 0.01f;
+                            if (fabs(c16_vertical_angle[j] - c16_config_vertical_angle[j]) > 1.5) {
+                                config_vert_num++;
+                            }
+                        }
+                        if (config_vert_num == 0) {
+                            for (int k = 0; k < 16; ++k) {
+                                c16_sin_scan_altitude[k] = sin(c16_config_vertical_angle[k] * DEG_TO_RAD);
+                                c16_cos_scan_altitude[k] = cos(c16_config_vertical_angle[k] * DEG_TO_RAD);
+                            }
+                        }
+                    }
+                    config_vert = false;
+                }
+                if (packet_size == 1206) {
+                    if (2 == version_data) {
+                        this->packetTimeStamp[4] = difop_packet_ptr->data[41];
+                        this->packetTimeStamp[5] = difop_packet_ptr->data[40];
+                        this->packetTimeStamp[6] = difop_packet_ptr->data[39];
+                        this->packetTimeStamp[7] = difop_packet_ptr->data[38];
+                        this->packetTimeStamp[8] = difop_packet_ptr->data[37];
+                        this->packetTimeStamp[9] = difop_packet_ptr->data[36];
+                    } else {
+                        this->packetTimeStamp[4] = difop_packet_ptr->data[57];
+                        this->packetTimeStamp[5] = difop_packet_ptr->data[56];
+                        this->packetTimeStamp[6] = difop_packet_ptr->data[55];
+                        this->packetTimeStamp[7] = difop_packet_ptr->data[54];
+                        this->packetTimeStamp[8] = difop_packet_ptr->data[53];
+                        this->packetTimeStamp[9] = difop_packet_ptr->data[52];
+                    }
+
+                }
+            } else if (rc < 0) {
+                return;
+            }
+        }
+    }
+
+
+    void lslidarC16Driver::decodePacket(const RawPacket *packet) {
+        //couputer azimuth angle for each firing
+        //even numbers
+        for (size_t b_idx = 0; b_idx < BLOCKS_PER_PACKET; ++b_idx) {
+            firings.firing_azimuth[b_idx] = packet->blocks[b_idx].rotation % 36000; //* 0.01 * DEG_TO_RAD;
+        }
+
+        // computer distance ,intensity
+        //12 blocks
+        for (size_t blk_idx = 0; blk_idx < BLOCKS_PER_PACKET; ++blk_idx) {
+            const RawBlock &raw_block = packet->blocks[blk_idx];
+
+            int32_t azimuth_diff = 0;
+            if (1 == return_mode) {
+                if (blk_idx < BLOCKS_PER_PACKET - 1) {
+                    azimuth_diff = firings.firing_azimuth[blk_idx + 1] - firings.firing_azimuth[blk_idx];
+                    azimuth_diff = azimuth_diff < 0 ? azimuth_diff + 36000 : azimuth_diff;
+                } else {
+                    azimuth_diff = firings.firing_azimuth[blk_idx] - firings.firing_azimuth[blk_idx - 1];
+                    azimuth_diff = azimuth_diff < 0 ? azimuth_diff + 36000 : azimuth_diff;
+                }
+            } else {
+                if (blk_idx < BLOCKS_PER_PACKET - 2) {
+                    azimuth_diff = firings.firing_azimuth[blk_idx + 2] - firings.firing_azimuth[blk_idx];
+                    azimuth_diff = azimuth_diff < 0 ? azimuth_diff + 36000 : azimuth_diff;
+                } else {
+                    azimuth_diff = firings.firing_azimuth[blk_idx] - firings.firing_azimuth[blk_idx - 2];
+                    azimuth_diff = azimuth_diff < 0 ? azimuth_diff + 36000 : azimuth_diff;
+                }
+
+            }
+            for (size_t scan_idx = 0; scan_idx < SCANS_PER_BLOCK; ++scan_idx) {
+                size_t byte_idx = RAW_SCAN_SIZE * scan_idx;
+                //azimuth
+                firings.azimuth[blk_idx * 32 + scan_idx] =
+                        firings.firing_azimuth[blk_idx] + scan_idx * azimuth_diff / FIRING_TOFFSET;
+                firings.azimuth[blk_idx * 32 + scan_idx] = firings.azimuth[blk_idx * 32 + scan_idx] % 36000;
+                // distance
+/*                TwoBytes raw_distance{0};
+                raw_distance.bytes[0] = raw_block.data[byte_idx];
+                raw_distance.bytes[1] = raw_block.data[byte_idx + 1];*/
+                firings.distance[blk_idx * 32 + scan_idx] =
+                        static_cast<double>(raw_block.data[byte_idx] + raw_block.data[byte_idx + 1] * 256) *
+                        DISTANCE_RESOLUTION * distance_unit;
+
+                //intensity
+                firings.intensity[blk_idx * 32 + scan_idx] = static_cast<double>(
+                        raw_block.data[byte_idx + 2]);
+            }
+
+        }
+        return;
+    }
+
+/** poll the device
+ *  @returns true unless end of file reached
+ */
+    bool lslidarC16Driver::poll(void) {  // Allocate a new shared pointer for zero-copy sharing with other nodelets.
+        lslidar_msgs::LslidarPacketPtr packet(new lslidar_msgs::LslidarPacket());
+
+        // Since the rslidar delivers data at a very high rate, keep
+        // reading and publishing scans as fast as possible.
+        while (true) {
+            int rc = msop_input_->getPacket(packet);
+            if (rc == 0) break;
+            if (rc < 0) {
+                return false;
+            }
+        }
+
+        const RawPacket *raw_packet = (const RawPacket *) (&(packet->data[0]));
+
+        //check if the packet is valid
+        if (!checkPacketValidity(raw_packet)) {
+            return false;
+        }
+
+        // packet timestamp
+        if (use_gps_ts) {
+            lslidar_msgs::LslidarPacket pkt = *packet;
+            memset(&cur_time, 0, sizeof(cur_time));
+            if (packet_size == 1206) {
+                cur_time.tm_year = this->packetTimeStamp[9] + 2000 - 1900;
+                cur_time.tm_mon = this->packetTimeStamp[8] - 1;
+                cur_time.tm_mday = this->packetTimeStamp[7];
+                cur_time.tm_hour = this->packetTimeStamp[6];
+                cur_time.tm_min = this->packetTimeStamp[5];
+                cur_time.tm_sec = this->packetTimeStamp[4];
+                packet_time_s = static_cast<uint64_t>(timegm(&cur_time)); //s
+                packet_time_ns = (pkt.data[1200] +
+                                  pkt.data[1201] * pow(2, 8) +
+                                  pkt.data[1202] * pow(2, 16) +
+                                  pkt.data[1203] * pow(2, 24)) * 1e3; //ns
+                timeStamp = ros::Time(packet_time_s, packet_time_ns);
+                //使用gps授时
+                if ((timeStamp - timeStamp_bak).toSec() < 0.0 && (timeStamp - timeStamp_bak).toSec() > -1.0 &&
+                    packet_time_ns < 100000000 && is_update_gps_time) {
+                    timeStamp = ros::Time(packet_time_s + 1, packet_time_ns);
+                } else if ((timeStamp - timeStamp_bak).toSec() > 1.0 && (timeStamp - timeStamp_bak).toSec() < 1.2
+                           && packet_time_ns > 900000000 && is_update_gps_time) {
+                    timeStamp = ros::Time(packet_time_s - 1, packet_time_ns);
+                }
+                timeStamp_bak = timeStamp;
+                packet->stamp = timeStamp;
+                packet_timestamp = packet->stamp.toSec();
+            } else if (packet_size == 1212) {
+                cur_time.tm_year = pkt.data[1200] + 2000 - 1900;
+                cur_time.tm_mon = pkt.data[1201] - 1;
+                cur_time.tm_mday = pkt.data[1202];
+                cur_time.tm_hour = pkt.data[1203];
+                cur_time.tm_min = pkt.data[1204];
+                cur_time.tm_sec = pkt.data[1205];
+                packet_time_s = static_cast<uint64_t>(timegm(&cur_time)); //s
+                packet_time_ns = (pkt.data[1206] +
+                                  pkt.data[1207] * pow(2, 8) +
+                                  pkt.data[1208] * pow(2, 16) +
+                                  pkt.data[1209] * pow(2, 24)) * 1e3; //ns
+                packet_timestamp = packet_time_s + packet_time_ns * 1e-9;
+                timeStamp = ros::Time(packet_time_s, packet_time_ns);
+                packet->stamp = timeStamp;
+            }
+        } else {
+            packet->stamp = ros::Time::now();
+            packet_timestamp = packet->stamp.toSec();
+        }
+
+        packet_interval_time = (packet_timestamp - last_packet_timestamp) / 384.0;
+        last_packet_timestamp = packet_timestamp;
+
+        if (packet_size == 1206) {
+            if (packet->data[1204] == 0x39) {
+                return_mode = 2;
+            }
+        } else if (packet_size == 1212) {
+            if (packet->data[1210] == 0x39) {
+                return_mode = 2;
+            }
+        }
+
+        ROS_INFO_ONCE("return mode: %d", return_mode);
+
+
+        //decode the packet
+        decodePacket(raw_packet);
+        // find the start of a new revolution
+        // if there is one, new_sweep_start will be the index of the start firing,
+        // otherwise, new_sweep_start will be FIRINGS_PER_PACKET.
+        size_t new_sweep_start = 0;
+        do {
+            if (last_azimuth - firings.azimuth[new_sweep_start] > 35000) {
+                break;
+            } else {
+                last_azimuth = firings.azimuth[new_sweep_start];
+                ++new_sweep_start;
+            }
+        } while (new_sweep_start < SCANS_PER_PACKET);
+
+        // The first sweep may not be complete. So, the firings with
+        // the first sweep will be discarded. We will wait for the
+        // second sweep in order to find the 0 azimuth angle.
+        size_t start_fir_idx = 0;
+        size_t end_fir_idx = new_sweep_start;
+        if (is_first_sweep && new_sweep_start == SCANS_PER_PACKET) {
+            return true;
+        } else {
+            if (is_first_sweep) {
+                is_first_sweep = false;
+                start_fir_idx = new_sweep_start;
+                end_fir_idx = SCANS_PER_PACKET;
+                //sweep_start_time = packet->stamp.toSec() - (end_fir_idx - start_fir_idx) * 3.125 * 1e-6;
+            }
+        }
+        for (size_t fir_idx = start_fir_idx; fir_idx < end_fir_idx; ++fir_idx) {
+            //check if the point is valid
+            if (!isPointInRange(firings.distance[fir_idx]))continue;
+            //convert the point to xyz coordinate
+            size_t table_idx = firings.azimuth[fir_idx];
+            double cos_azimuth = cos_azimuth_table[table_idx];
+            double sin_azimuth = sin_azimuth_table[table_idx];
+
+
+            double x_coord, y_coord, z_coord;
+            if (coordinate_opt) {
+                int tmp_idx = 1468 - firings.azimuth[fir_idx] < 0 ? 1468 - firings.azimuth[fir_idx] + 36000 : 1468 - firings.azimuth[fir_idx];
+                x_coord = firings.distance[fir_idx] * c16_cos_scan_altitude[fir_idx % 16] * cos_azimuth +
+                          R1_ * cos_azimuth_table[tmp_idx];
+                y_coord = -firings.distance[fir_idx] * c16_cos_scan_altitude[fir_idx % 16] * sin_azimuth +
+                          R1_ * sin_azimuth_table[tmp_idx];
+                z_coord = firings.distance[fir_idx] * c16_sin_scan_altitude[fir_idx % 16];
+
+            } else {
+                //Y-axis correspondence 0 degree
+                int tmp_idx = firings.azimuth[fir_idx] - 1468 < 0 ? firings.azimuth[fir_idx] -1468 + 36000 : firings.azimuth[fir_idx] -1468;
+                x_coord = firings.distance[fir_idx] * c16_cos_scan_altitude[fir_idx % 16] * sin_azimuth +
+                          R1_ * sin_azimuth_table[tmp_idx];
+                y_coord = firings.distance[fir_idx] * c16_cos_scan_altitude[fir_idx % 16] * cos_azimuth +
+                          R1_ * cos_azimuth_table[tmp_idx];
+                z_coord = firings.distance[fir_idx] * c16_sin_scan_altitude[fir_idx % 16];
+            }
+            // computer the time of the point
+            double time = packet_timestamp  -
+                          (SCANS_PER_PACKET - fir_idx - 1) * packet_interval_time - sweep_end_time;
+
+            int remapped_scan_idx = (fir_idx % 16) % 2 == 0 ? (fir_idx % 16) / 2 : (fir_idx % 16) / 2 + 8;
+
+            sweep_data->points.push_back(lslidar_msgs::LslidarPoint());
+            lslidar_msgs::LslidarPoint &new_point = sweep_data->points[
+                    sweep_data->points.size() - 1];
+            //pack the data into point msg
+            new_point.time = time;
+            new_point.x = x_coord;
+            new_point.y = y_coord;
+            new_point.z = z_coord;
+            new_point.intensity = firings.intensity[fir_idx];
+            new_point.ring = remapped_scan_idx;
+            new_point.azimuth = firings.azimuth[fir_idx];
+            new_point.distance = firings.distance[fir_idx];
+        }
+        // a new sweep begins ----------------------------------------------------
+
+        if (end_fir_idx != SCANS_PER_PACKET) {
+            //publish Last frame scan
+            sweep_end_time =  packet_timestamp - (SCANS_PER_PACKET - end_fir_idx -1) * packet_interval_time;
+
+            sweep_end_time = sweep_end_time > 0 ? sweep_end_time : 0;
+            publishPointcloud();
+            if (publish_scan) publishScan();
+
+            sweep_data = lslidar_msgs::LslidarScanPtr(new lslidar_msgs::LslidarScan());
+            //prepare the next frame scan
+            //sweep_start_time = packet->stamp.toSec() - (end_fir_idx - start_fir_idx) * 3.125 * 1e-6;
+            last_azimuth = firings.azimuth[SCANS_PER_PACKET - 1];
+            start_fir_idx = end_fir_idx;
+            end_fir_idx = SCANS_PER_PACKET;
+            for (size_t fir_idx = start_fir_idx; fir_idx < end_fir_idx; ++fir_idx) {
+
+                //check if the point is valid
+                if (!isPointInRange(firings.distance[fir_idx]))continue;
+                //convert the point to xyz coordinate
+                size_t table_idx = firings.azimuth[fir_idx];
+                double cos_azimuth = cos_azimuth_table[table_idx];
+                double sin_azimuth = sin_azimuth_table[table_idx];
+                double x_coord, y_coord, z_coord;
+                if (coordinate_opt) {
+                    int tmp_idx = 1468 - firings.azimuth[fir_idx] < 0 ? 1468 - firings.azimuth[fir_idx] + 36000 : 1468 - firings.azimuth[fir_idx];
+                    x_coord = firings.distance[fir_idx] * c16_cos_scan_altitude[fir_idx % 16] * cos_azimuth +
+                              R1_ * cos_azimuth_table[tmp_idx];
+                    y_coord = -firings.distance[fir_idx] * c16_cos_scan_altitude[fir_idx % 16] * sin_azimuth +
+                              R1_ * sin_azimuth_table[tmp_idx];
+                    z_coord = firings.distance[fir_idx] * c16_sin_scan_altitude[fir_idx % 16];
+
+                } else {
+                    //Y-axis correspondence 0 degree
+                    int tmp_idx = firings.azimuth[fir_idx] - 1468 < 0 ? firings.azimuth[fir_idx] -1468 + 36000 : firings.azimuth[fir_idx] -1468;
+                    x_coord = firings.distance[fir_idx] * c16_cos_scan_altitude[fir_idx % 16] * sin_azimuth +
+                              R1_ * sin_azimuth_table[tmp_idx];
+                    y_coord = firings.distance[fir_idx] * c16_cos_scan_altitude[fir_idx % 16] * cos_azimuth +
+                              R1_ * cos_azimuth_table[tmp_idx];
+                    z_coord = firings.distance[fir_idx] * c16_sin_scan_altitude[fir_idx % 16];
+                }
+                // computer the time of the point
+                double time = packet_timestamp  -
+                              (SCANS_PER_PACKET - fir_idx -1) * packet_interval_time - sweep_end_time;
+                int remapped_scan_idx = (fir_idx % 16) % 2 == 0 ? (fir_idx % 16) / 2 : (fir_idx % 16) / 2 + 8;
+                sweep_data->points.push_back(lslidar_msgs::LslidarPoint());
+                lslidar_msgs::LslidarPoint &new_point = sweep_data->points[
+                        sweep_data->points.size() - 1];
+                //pack the data into point msg
+                new_point.time = time;
+                new_point.x = x_coord;
+                new_point.y = y_coord;
+                new_point.z = z_coord;
+                new_point.intensity = firings.intensity[fir_idx];
+                new_point.ring = remapped_scan_idx;
+                new_point.azimuth = firings.azimuth[fir_idx];
+                new_point.distance = firings.distance[fir_idx];
+            }
+
+        }
+        //packet_pub.publish(*packet);
+        return true;
+    }
+
+
+    lslidarC32Driver::lslidarC32Driver(ros::NodeHandle &node, ros::NodeHandle &private_nh) : lslidarDriver(node,
+                                                                                                           private_nh) {
+        return;
+    }
+
+    lslidarC32Driver::~lslidarC32Driver() {
+        if (difop_thread_ != nullptr) {
+            difop_thread_->join();
+        }
+    }
+
+    bool lslidarC32Driver::initialize() {
+        this->initTimeStamp();
+        if (!loadParameters()) {
+            ROS_ERROR("cannot load all required ROS parameters.");
+            return false;
+        }
+        if (c32_type == "c32_2") {
+            if (c32_fpga_type == 2) {
+                for (int j = 0; j < 32; ++j) {
+                    c32_vertical_angle[j] = c32_2_vertical_angle_26[j];
+                    c32_config_vertical_angle[j] = c32_2_vertical_angle_26[j];
+                    c32_sin_scan_altitude[j] = sin(c32_vertical_angle[j] * DEG_TO_RAD);
+                    c32_cos_scan_altitude[j] = cos(c32_vertical_angle[j] * DEG_TO_RAD);
+                }
+            } else {
+                for (int j = 0; j < 32; ++j) {
+                    c32_vertical_angle[j] = c32_2_vertical_angle[j];
+                    c32_config_vertical_angle[j] = c32_2_vertical_angle[j];
+                    c32_sin_scan_altitude[j] = sin(c32_vertical_angle[j] * DEG_TO_RAD);
+                    c32_cos_scan_altitude[j] = cos(c32_vertical_angle[j] * DEG_TO_RAD);
+                }
+            }
+        } else {
+            if (c32_fpga_type == 2) {
+                for (int j = 0; j < 32; ++j) {
+                    c32_vertical_angle[j] = c32_1_vertical_angle_26[j];
+                    c32_config_vertical_angle[j] = c32_1_vertical_angle_26[j];
+                    c32_sin_scan_altitude[j] = sin(c32_vertical_angle[j] * DEG_TO_RAD);
+                    c32_cos_scan_altitude[j] = cos(c32_vertical_angle[j] * DEG_TO_RAD);
+                }
+            } else {
+                for (int j = 0; j < 32; ++j) {
+                    c32_vertical_angle[j] = c32_1_vertical_angle[j];
+                    c32_config_vertical_angle[j] = c32_1_vertical_angle[j];
+                    c32_sin_scan_altitude[j] = sin(c32_vertical_angle[j] * DEG_TO_RAD);
+                    c32_cos_scan_altitude[j] = cos(c32_vertical_angle[j] * DEG_TO_RAD);
+                }
+            }
+        }
+
+        if (!createRosIO()) {
+            ROS_ERROR("cannot create all ROS IO.");
+            return false;
+        }
+
+        for (int i = 0; i < 4; ++i) {
+            adjust_angle[i] = 0.0;
+        }
+
+        for (int j = 0; j < 36000; ++j) {
+            double angle = static_cast<double>(j) / 100.0 * DEG_TO_RAD;
+            sin_azimuth_table[j] = sin(angle);
+            cos_azimuth_table[j] = cos(angle);
+        }
+
+        return true;
+    }
+
+    void lslidarC32Driver::difopPoll() {
+        // reading and publishing scans as fast as possible.
+        lslidar_msgs::LslidarPacketPtr difop_packet_ptr(new lslidar_msgs::LslidarPacket);
+        while (ros::ok()) {
+            // keep reading
+            int rc = difop_input_->getPacket(difop_packet_ptr);
+            if (rc == 0) {
+                if (difop_packet_ptr->data[0] != 0xa5 || difop_packet_ptr->data[1] != 0xff ||
+                    difop_packet_ptr->data[2] != 0x00 || difop_packet_ptr->data[3] != 0x5a) {
+                    return;
+                }
+                count++;
+                if (count == 2) { is_update_gps_time = true; }
+                for (int i = 0; i < 1206; i++) {
+                    difop_data[i] = difop_packet_ptr->data[i];
+                }
+                ROS_INFO_ONCE("c32 vertical angle resolution type: %s; c32 fpga type: %0.1f", c32_type.c_str(),
+                              difop_data[1202] + int(difop_data[1203] / 16) * 0.1);
+                //int version_data = difop_packet_ptr->data[1202];
+                if (config_vert) {
+                    if (3 == c32_fpga_type) {
+                        for (int i = 0; i < 32; ++i) {
+                            uint8_t data1 = difop_packet_ptr->data[245 + 2 * i];
+                            uint8_t data2 = difop_packet_ptr->data[245 + 2 * i + 1];
+                            int vert_angle = data1 * 256 + data2;
+                            vert_angle = vert_angle > 32767 ? (vert_angle - 65535) : vert_angle;
+                            c32_config_tmp_angle[i] = (double) vert_angle * 0.01f;
+
+                        }
+                        for (int j = 0; j < 32; ++j) {
+                            c32_config_vertical_angle[j] = c32_config_tmp_angle[adjust_angle_index[j]];
+
+                            if (fabs(c32_vertical_angle[j] - c32_config_vertical_angle[j]) > 3) {
+                                config_vert_num++;
+                            }
+                        }
+                        if (config_vert_num == 0) {
+                            for (int k = 0; k < 32; ++k) {
+                                c32_sin_scan_altitude[k] = sin(c32_config_vertical_angle[k] * DEG_TO_RAD);
+                                c32_cos_scan_altitude[k] = cos(c32_config_vertical_angle[k] * DEG_TO_RAD);
+                            }
+                        }
+                        // horizontal correction angle
+                        int angle_a0 = difop_packet_ptr->data[186] * 256 + difop_packet_ptr->data[187];
+                        adjust_angle[0] = angle_a0 > 32767 ? 32767 - angle_a0 : angle_a0;
+
+                        int angle_a1 = difop_packet_ptr->data[190] * 256 + difop_packet_ptr->data[191];
+                        adjust_angle[1] = angle_a1 > 32767 ? 32767 - angle_a1 : angle_a1;
+
+                        int angle_a2 = difop_packet_ptr->data[188] * 256 + difop_packet_ptr->data[189];
+                        adjust_angle[2] = angle_a2 > 32767 ? 32767 - angle_a2 : angle_a2;
+
+                        int angle_a3 = difop_packet_ptr->data[192] * 256 + difop_packet_ptr->data[193];
+                        adjust_angle[3] = angle_a3 > 32767 ? 32767 - angle_a3 : angle_a3;
+                    } else {
+                        for (int j = 0; j < 32; ++j) {
+                            uint8_t data1 = difop_packet_ptr->data[882 + 2 * j];
+                            uint8_t data2 = difop_packet_ptr->data[882 + 2 * j + 1];
+                            int vert_angle = data1 * 256 + data2;
+                            vert_angle = vert_angle > 32767 ? (vert_angle - 65535) : vert_angle;
+                            c32_config_vertical_angle[j] = (double) vert_angle * 0.01f;
+
+                            if (fabs(c32_vertical_angle[j] - c32_config_vertical_angle[j]) > 3.0) {
+                                config_vert_num++;
+                            }
+                        }
+                        if (config_vert_num == 0) {
+                            for (int k = 0; k < 32; ++k) {
+                                c32_sin_scan_altitude[k] = sin(c32_config_vertical_angle[k] * DEG_TO_RAD);
+                                c32_cos_scan_altitude[k] = cos(c32_config_vertical_angle[k] * DEG_TO_RAD);
+
+                            }
+                        }
+                        // horizontal correction angle
+                        int angle_a0 = difop_packet_ptr->data[34] * 256 + difop_packet_ptr->data[35];
+                        adjust_angle[0] = angle_a0 > 32767 ? 32767 - angle_a0 : angle_a0;
+
+                        int angle_a1 = difop_packet_ptr->data[42] * 256 + difop_packet_ptr->data[43];
+                        adjust_angle[1] = angle_a1 > 32767 ? 32767 - angle_a1 : angle_a1;
+
+                        int angle_a2 = difop_packet_ptr->data[66] * 256 + difop_packet_ptr->data[67];
+                        adjust_angle[2] = angle_a2 > 32767 ? 32767 - angle_a2 : angle_a2;
+
+                        int angle_a3 = difop_packet_ptr->data[68] * 256 + difop_packet_ptr->data[69];
+                        adjust_angle[3] = angle_a3 > 32767 ? 32767 - angle_a3 : angle_a3;
+                    }
+                    config_vert = false;
+                }
+                if (packet_size == 1206) {
+                    if (2 == c32_fpga_type) {
+                        this->packetTimeStamp[4] = difop_packet_ptr->data[41];
+                        this->packetTimeStamp[5] = difop_packet_ptr->data[40];
+                        this->packetTimeStamp[6] = difop_packet_ptr->data[39];
+                        this->packetTimeStamp[7] = difop_packet_ptr->data[38];
+                        this->packetTimeStamp[8] = difop_packet_ptr->data[37];
+                        this->packetTimeStamp[9] = difop_packet_ptr->data[36];
+                    } else {
+                        this->packetTimeStamp[4] = difop_packet_ptr->data[57];
+                        this->packetTimeStamp[5] = difop_packet_ptr->data[56];
+                        this->packetTimeStamp[6] = difop_packet_ptr->data[55];
+                        this->packetTimeStamp[7] = difop_packet_ptr->data[54];
+                        this->packetTimeStamp[8] = difop_packet_ptr->data[53];
+                        this->packetTimeStamp[9] = difop_packet_ptr->data[52];
+                    }
+                }
+
+            } else if (rc < 0) {
+                return;
+            }
+        }
+    }
+
+
+    void lslidarC32Driver::decodePacket(const RawPacket *packet) {
+        //couputer azimuth angle for each firing, 12
+
+        for (size_t fir_idx = 0; fir_idx < BLOCKS_PER_PACKET; ++fir_idx) {
+            firings.firing_azimuth[fir_idx] = packet->blocks[fir_idx].rotation % 36000; //* 0.01 * DEG_TO_RAD;
+            // ROS_INFO("azi==%d",packet->blocks[fir_idx].rotation);
+        }
+        // ROS_WARN("-----------------");
+        // computer distance ,intensity
+        //12 blocks
+        for (size_t blk_idx = 0; blk_idx < BLOCKS_PER_PACKET; ++blk_idx) {
+            const RawBlock &raw_block = packet->blocks[blk_idx];
+
+            int32_t azimuth_diff = 0;
+            if (1 == return_mode) {
+                if (blk_idx < BLOCKS_PER_PACKET - 1) {
+                    azimuth_diff = firings.firing_azimuth[blk_idx + 1] - firings.firing_azimuth[blk_idx];
+                } else {
+                    azimuth_diff = firings.firing_azimuth[blk_idx] - firings.firing_azimuth[blk_idx - 1];
+                }
+                azimuth_diff = azimuth_diff < 0 ? azimuth_diff + 36000 : azimuth_diff;
+            } else {
+                if (blk_idx < BLOCKS_PER_PACKET - 2) {
+                    azimuth_diff = firings.firing_azimuth[blk_idx + 2] - firings.firing_azimuth[blk_idx];
+
+                } else {
+                    azimuth_diff = firings.firing_azimuth[blk_idx] - firings.firing_azimuth[blk_idx - 2];
+                }
+                azimuth_diff = azimuth_diff < 0 ? azimuth_diff + 36000 : azimuth_diff;
+            }
+            // 32 scan
+            for (size_t scan_idx = 0; scan_idx < SCANS_PER_BLOCK; ++scan_idx) {
+                size_t byte_idx = RAW_SCAN_SIZE * scan_idx;
+                //azimuth
+                firings.azimuth[blk_idx * 32 + scan_idx] = firings.firing_azimuth[blk_idx] +
+                                                           scan_idx * azimuth_diff / FIRING_TOFFSET;
+
+                firings.azimuth[blk_idx * 32 + scan_idx] = firings.azimuth[blk_idx * 32 + scan_idx] % 36000;
+
+                /*
+                // calibration azimuth ，1°
+              if ("c32_2" == c32_type) {
+                    // -----结果是否是正数 ？
+                    int adjust_diff = adjust_angle[1] - adjust_angle[0];
+                    if (adjust_diff > 300 && adjust_diff < 460) {
+                        // fpga :v 2.7
+                        if (3 == c32_fpga_type) {
+                            if ( 1 >= scan_fir_idx % 4 ) {
+                                firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[1];
+                            } else {
+                                firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[0];
+                            }
+                           // ROS_INFO("id: %d--azi: %d",blk_idx * 32 + scan_fir_idx,firings.azimuth[blk_idx * 32 + scan_fir_idx]);
+                        } else {
+                            if (0 == scan_fir_idx % 2) {
+                                firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[1];
+                            } else {
+                                firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[0];
+                            }
+                        }
+                    } else {
+                        // fpga: v2.6
+                        if (0 == scan_fir_idx % 2) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[0];
+                        } else {
+                            int tmp_azimuth = firings.azimuth[blk_idx * 32 + scan_fir_idx] - adjust_angle[0];
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] =
+                                    tmp_azimuth < 0 ? tmp_azimuth + 36000 : tmp_azimuth;
+                        }
+                    }
+                }
+                //  calibration azimuth ,0.33°
+                if ("c32_1" == c32_type) {
+                    int adjust_diff_one = adjust_angle[1] - adjust_angle[0];
+                    int adjust_diff_two = adjust_angle[3] - adjust_angle[2];
+                    if (3 == c32_fpga_type) {
+                        // fpga v3.0
+                        if (0 == scan_fir_idx || 1 == scan_fir_idx || 4 == scan_fir_idx || 8 == scan_fir_idx ||
+                            9 == scan_fir_idx || 12 == scan_fir_idx
+                            || 16 == scan_fir_idx || 17 == scan_fir_idx || 21 == scan_fir_idx || 24 == scan_fir_idx ||
+                            25 == scan_fir_idx || 29 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[3];
+                        }
+                        if (2 == scan_fir_idx || 3 == scan_fir_idx || 6 == scan_fir_idx || 10 == scan_fir_idx ||
+                            11 == scan_fir_idx || 14 == scan_fir_idx
+                            || 18 == scan_fir_idx || 19 == scan_fir_idx || 23 == scan_fir_idx || 26 == scan_fir_idx ||
+                            27 == scan_fir_idx || 31 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[2];
+                        }
+                        if (5 == scan_fir_idx || 13 == scan_fir_idx || 20 == scan_fir_idx || 28 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[1];
+                        }
+                        if (7 == scan_fir_idx || 15 == scan_fir_idx || 22 == scan_fir_idx || 30 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[0];
+                        }
+                    } else if (adjust_diff_one > 500 && adjust_diff_one < 660 && adjust_diff_two > 150 &&
+                               adjust_diff_two < 350) {
+                        //fpga v2.7
+                        if (10 == scan_fir_idx || 14 == scan_fir_idx || 18 == scan_fir_idx || 22 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[3];
+                        }
+                        if (11 == scan_fir_idx || 15 == scan_fir_idx || 19 == scan_fir_idx || 23 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[2];
+                        }
+                        if (0 == scan_fir_idx || 2 == scan_fir_idx || 4 == scan_fir_idx || 6 == scan_fir_idx ||
+                            8 == scan_fir_idx || 12 == scan_fir_idx
+                            || 16 == scan_fir_idx || 20 == scan_fir_idx || 24 == scan_fir_idx || 26 == scan_fir_idx ||
+                            28 == scan_fir_idx || 30 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[1];
+                        }
+                        if (1 == scan_fir_idx || 3 == scan_fir_idx || 5 == scan_fir_idx || 7 == scan_fir_idx ||
+                            9 == scan_fir_idx || 13 == scan_fir_idx
+                            || 17 == scan_fir_idx || 21 == scan_fir_idx || 25 == scan_fir_idx || 27 == scan_fir_idx ||
+                            29 == scan_fir_idx || 31 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[0];
+                        }
+                    } else {
+                        // fpga v2.6
+                        if (10 == scan_fir_idx || 14 == scan_fir_idx || 18 == scan_fir_idx || 22 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[0];
+                        }
+                        if (11 == scan_fir_idx || 15 == scan_fir_idx || 19 == scan_fir_idx || 23 == scan_fir_idx) {
+                            int tmp_azimuth = firings.azimuth[blk_idx * 32 + scan_fir_idx] - adjust_angle[0];
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] =
+                                    tmp_azimuth < 0 ? tmp_azimuth + 36000 : tmp_azimuth;
+                        }
+                        if (0 == scan_fir_idx || 2 == scan_fir_idx || 4 == scan_fir_idx || 6 == scan_fir_idx ||
+                            8 == scan_fir_idx || 12 == scan_fir_idx
+                            || 16 == scan_fir_idx || 20 == scan_fir_idx || 24 == scan_fir_idx || 26 == scan_fir_idx ||
+                            28 == scan_fir_idx || 30 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[1];
+                        }
+                        if (1 == scan_fir_idx || 3 == scan_fir_idx || 5 == scan_fir_idx || 7 == scan_fir_idx ||
+                            9 == scan_fir_idx || 13 == scan_fir_idx
+                            || 17 == scan_fir_idx || 21 == scan_fir_idx || 25 == scan_fir_idx || 27 == scan_fir_idx ||
+                            29 == scan_fir_idx || 31 == scan_fir_idx) {
+                            int tmp_azimuth = firings.azimuth[blk_idx * 32 + scan_fir_idx] - adjust_angle[1];
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] =
+                                    tmp_azimuth < 0 ? tmp_azimuth + 36000 : tmp_azimuth;
+                        }
+
+                    }
+
+                }
+
+
+                firings.azimuth[blk_idx * 32 + scan_fir_idx] = firings.azimuth[blk_idx * 32 + scan_fir_idx] % 36000;
+                 */
+
+                // distance
+                firings.distance[blk_idx * 32 + scan_idx] =
+                        static_cast<double>(raw_block.data[byte_idx] + raw_block.data[byte_idx + 1] * 256) *
+                        DISTANCE_RESOLUTION * distance_unit;
+
+                //intensity
+                firings.intensity[blk_idx * 32 + scan_idx] = static_cast<double>(
+                        raw_block.data[byte_idx + 2]);
+            }
+
+        }
+        return;
+    }
+
+/** poll the device
+ *  @returns true unless end of file reached
+ */
+    bool lslidarC32Driver::poll(void) {
+        lslidar_msgs::LslidarPacketPtr packet(new lslidar_msgs::LslidarPacket());
+
+
+        // Since the rslidar delivers data at a very high rate, keep
+        // reading and publishing scans as fast as possible.
+        while (true) {
+            int rc = msop_input_->getPacket(packet);
+            if (rc == 0) break;
+            if (rc < 0) return false;
+        }
+        const RawPacket *raw_packet = (const RawPacket *) (&(packet->data[0]));
+
+        //check if the packet is valid
+        if (!checkPacketValidity(raw_packet)) return false;
+
+        // packet timestamp
+        if (use_gps_ts) {
+            lslidar_msgs::LslidarPacket pkt = *packet;
+            memset(&cur_time, 0, sizeof(cur_time));
+            if (packet_size == 1206) {
+                cur_time.tm_year = this->packetTimeStamp[9] + 2000 - 1900;
+                cur_time.tm_mon = this->packetTimeStamp[8] - 1;
+                cur_time.tm_mday = this->packetTimeStamp[7];
+                cur_time.tm_hour = this->packetTimeStamp[6];
+                cur_time.tm_min = this->packetTimeStamp[5];
+                cur_time.tm_sec = this->packetTimeStamp[4];
+                packet_time_s = static_cast<uint64_t>(timegm(&cur_time)); //s
+                packet_time_ns = (pkt.data[1200] +
+                                  pkt.data[1201] * pow(2, 8) +
+                                  pkt.data[1202] * pow(2, 16) +
+                                  pkt.data[1203] * pow(2, 24)) * 1e3; //ns
+                timeStamp = ros::Time(packet_time_s, packet_time_ns);
+                //使用gps授时
+                if ((timeStamp - timeStamp_bak).toSec() < 0.0 && (timeStamp - timeStamp_bak).toSec() > -1.0 &&
+                    packet_time_ns < 100000000 && is_update_gps_time) {
+                    timeStamp = ros::Time(packet_time_s + 1, packet_time_ns);
+                } else if ((timeStamp - timeStamp_bak).toSec() > 1.0 && (timeStamp - timeStamp_bak).toSec() < 1.2
+                           && packet_time_ns > 900000000 && is_update_gps_time) {
+                    timeStamp = ros::Time(packet_time_s - 1, packet_time_ns);
+                }
+                timeStamp_bak = timeStamp;
+                packet->stamp = timeStamp;
+                packet_timestamp = packet->stamp.toSec();
+            } else if (packet_size == 1212) {
+                cur_time.tm_year = pkt.data[1200] + 2000 - 1900;
+                cur_time.tm_mon = pkt.data[1201] - 1;
+                cur_time.tm_mday = pkt.data[1202];
+                cur_time.tm_hour = pkt.data[1203];
+                cur_time.tm_min = pkt.data[1204];
+                cur_time.tm_sec = pkt.data[1205];
+                packet_time_s = static_cast<uint64_t>(timegm(&cur_time)); //s
+                packet_time_ns = (pkt.data[1206] +
+                                  pkt.data[1207] * pow(2, 8) +
+                                  pkt.data[1208] * pow(2, 16) +
+                                  pkt.data[1209] * pow(2, 24)) * 1e3; //ns
+                packet_timestamp = packet_time_s + packet_time_ns * 1e-9;
+                timeStamp = ros::Time(packet_time_s, packet_time_ns);
+                packet->stamp = timeStamp;
+            }
+        } else {
+            packet->stamp = ros::Time::now();
+            packet_timestamp = packet->stamp.toSec();
+        }
+
+        packet_interval_time = (packet_timestamp - last_packet_timestamp) / 384.0;
+        last_packet_timestamp = packet_timestamp;
+
+        if (packet_size == 1206) {
+            if (packet->data[1204] == 0x39) {
+                return_mode = 2;
+            }
+        } else if (packet_size == 1212) {
+            if (packet->data[1210] == 0x39) {
+                return_mode = 2;
+            }
+        }
+        ROS_INFO_ONCE("return mode: %d", return_mode);
+
+
+        //decode the packet
+        decodePacket(raw_packet);
+        // find the start of a new revolution
+        // if there is one, new_sweep_start will be the index of the start firing,
+        // otherwise, new_sweep_start will be FIRINGS_PER_PACKET.
+        size_t new_sweep_start = 0;
+        do {
+            if (last_azimuth - firings.azimuth[new_sweep_start] > 35000) {
+                break;
+            } else {
+                last_azimuth = firings.azimuth[new_sweep_start];
+                ++new_sweep_start;
+            }
+        } while (new_sweep_start < SCANS_PER_PACKET);
+
+        // The first sweep may not be complete. So, the firings with
+        // the first sweep will be discarded. We will wait for the
+        // second sweep in order to find the 0 azimuth angle.
+        size_t start_fir_idx = 0;
+        size_t end_fir_idx = new_sweep_start;
+        if (is_first_sweep && new_sweep_start == SCANS_PER_PACKET) {
+            return true;
+        } else {
+            if (is_first_sweep) {
+                is_first_sweep = false;
+                start_fir_idx = new_sweep_start;
+                end_fir_idx = SCANS_PER_PACKET;
+                //sweep_start_time = packet->stamp.toSec() - (end_fir_idx - start_fir_idx) * 3.125 * 1e-6;
+            }
+        }
+        last_azimuth_tmp = firings.azimuth[SCANS_PER_PACKET - 1];
+
+        for (int blk_idx = 0; blk_idx < BLOCKS_PER_PACKET; ++blk_idx) {
+            for (int scan_fir_idx = 0; scan_fir_idx < SCANS_PER_BLOCK; ++scan_fir_idx) {
+
+                // calibration azimuth ，1°
+                if ("c32_2" == c32_type) {
+
+//                    int adjust_diff = adjust_angle[1] - adjust_angle[0];
+
+//                    if (adjust_diff > 300 && adjust_diff < 460) {
+                        // fpga :v 2.8 3.0
+                        if (3 == c32_fpga_type) {
+                            if (1 >= scan_fir_idx % 4) {
+                                firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[1];
+                            } else {
+                                firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[0];
+                            }
+                            // ROS_INFO("id: %d--azi: %d",blk_idx * 32 + scan_fir_idx,firings.azimuth[blk_idx * 32 + scan_fir_idx]);
+                        }
+//                    }
+                else {
+                        // fpga: v2.6
+                        if (0 == scan_fir_idx % 2) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[0];
+                        } else {
+                            int tmp_azimuth = firings.azimuth[blk_idx * 32 + scan_fir_idx] - adjust_angle[0];
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] =
+                                    tmp_azimuth < 0 ? tmp_azimuth + 36000 : tmp_azimuth;
+                        }
+                    }
+                }
+                //  calibration azimuth ,0.33°
+                if ("c32_1" == c32_type) {
+                    int adjust_diff_one = adjust_angle[1] - adjust_angle[0];
+                    int adjust_diff_two = adjust_angle[3] - adjust_angle[2];
+                    if (3 == c32_fpga_type) {
+                        // fpga v 2.8 3.0
+                        if (0 == scan_fir_idx || 1 == scan_fir_idx || 4 == scan_fir_idx || 8 == scan_fir_idx ||
+                            9 == scan_fir_idx || 12 == scan_fir_idx
+                            || 16 == scan_fir_idx || 17 == scan_fir_idx || 21 == scan_fir_idx || 24 == scan_fir_idx ||
+                            25 == scan_fir_idx || 29 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[3];
+                        }
+                        if (2 == scan_fir_idx || 3 == scan_fir_idx || 6 == scan_fir_idx || 10 == scan_fir_idx ||
+                            11 == scan_fir_idx || 14 == scan_fir_idx
+                            || 18 == scan_fir_idx || 19 == scan_fir_idx || 23 == scan_fir_idx || 26 == scan_fir_idx ||
+                            27 == scan_fir_idx || 31 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[2];
+                        }
+                        if (5 == scan_fir_idx || 13 == scan_fir_idx || 20 == scan_fir_idx || 28 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[1];
+                        }
+                        if (7 == scan_fir_idx || 15 == scan_fir_idx || 22 == scan_fir_idx || 30 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[0];
+                        }
+                    } else {
+                        // fpga v2.6
+                        if (10 == scan_fir_idx || 14 == scan_fir_idx || 18 == scan_fir_idx || 22 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[0];
+                        }
+                        if (11 == scan_fir_idx || 15 == scan_fir_idx || 19 == scan_fir_idx || 23 == scan_fir_idx) {
+                            int tmp_azimuth = firings.azimuth[blk_idx * 32 + scan_fir_idx] - adjust_angle[0];
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] =
+                                    tmp_azimuth < 0 ? tmp_azimuth + 36000 : tmp_azimuth;
+                        }
+                        if (0 == scan_fir_idx || 2 == scan_fir_idx || 4 == scan_fir_idx || 6 == scan_fir_idx ||
+                            8 == scan_fir_idx || 12 == scan_fir_idx
+                            || 16 == scan_fir_idx || 20 == scan_fir_idx || 24 == scan_fir_idx || 26 == scan_fir_idx ||
+                            28 == scan_fir_idx || 30 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[1];
+                        }
+                        if (1 == scan_fir_idx || 3 == scan_fir_idx || 5 == scan_fir_idx || 7 == scan_fir_idx ||
+                            9 == scan_fir_idx || 13 == scan_fir_idx
+                            || 17 == scan_fir_idx || 21 == scan_fir_idx || 25 == scan_fir_idx || 27 == scan_fir_idx ||
+                            29 == scan_fir_idx || 31 == scan_fir_idx) {
+                            int tmp_azimuth = firings.azimuth[blk_idx * 32 + scan_fir_idx] - adjust_angle[1];
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] =
+                                    tmp_azimuth < 0 ? tmp_azimuth + 36000 : tmp_azimuth;
+                        }
+
+                    }
+
+                }
+                if(firings.azimuth[blk_idx * 32 + scan_fir_idx] < 0)	    firings.azimuth[blk_idx * 32 + scan_fir_idx]+=36000;
+                if(firings.azimuth[blk_idx * 32 + scan_fir_idx] > 36000)	firings.azimuth[blk_idx * 32 + scan_fir_idx]-=36000;
+            }
+
+        }
+
+
+        for (size_t fir_idx = start_fir_idx; fir_idx < end_fir_idx; ++fir_idx) {
+            //check if the point is valid
+            if (!isPointInRange(firings.distance[fir_idx]))continue;
+            if (firings.azimuth[fir_idx] >= 36000) {
+                firings.azimuth[fir_idx] = firings.azimuth[fir_idx] - 36000;
+            }
+            //convert the point to xyz coordinate
+            size_t table_idx = firings.azimuth[fir_idx];
+            double cos_azimuth = cos_azimuth_table[table_idx];
+            double sin_azimuth = sin_azimuth_table[table_idx];
+            double x_coord, y_coord, z_coord;
+            if (coordinate_opt) {
+            int tmp_idx = 1298 - firings.azimuth[fir_idx] < 0 ? 1298 - firings.azimuth[fir_idx] + 36000 : 1298 - firings.azimuth[fir_idx];
+                x_coord = firings.distance[fir_idx] * c32_cos_scan_altitude[fir_idx % 32] * cos_azimuth +
+                          R2_ * cos((12.98 - firings.azimuth[fir_idx] * 0.01) * DEG_TO_RAD);
+                y_coord = -firings.distance[fir_idx] * c32_cos_scan_altitude[fir_idx % 32] * sin_azimuth +
+                          R2_ * sin((12.98 - firings.azimuth[fir_idx] * 0.01) * DEG_TO_RAD);
+                z_coord = firings.distance[fir_idx] * c32_sin_scan_altitude[fir_idx % 32];
+
+            } else {
+                //Y-axis correspondence 0 degree
+               int tmp_idx = firings.azimuth[fir_idx] - 1298 < 0 ? firings.azimuth[fir_idx] -1298 + 36000 : firings.azimuth[fir_idx] -1298;
+                x_coord = firings.distance[fir_idx] * c32_cos_scan_altitude[fir_idx % 32] * sin_azimuth +
+                          R2_ * sin((firings.azimuth[fir_idx] * 0.01 - 12.98) * DEG_TO_RAD);
+                y_coord = firings.distance[fir_idx] * c32_cos_scan_altitude[fir_idx % 32] * cos_azimuth +
+                          R2_ * cos((firings.azimuth[fir_idx] * 0.01 - 12.98) * DEG_TO_RAD);
+                z_coord = firings.distance[fir_idx] * c32_sin_scan_altitude[fir_idx % 32];
+            }
+            // computer the time of the point
+            double time = packet_timestamp  -
+                          (SCANS_PER_PACKET - fir_idx - 1) * packet_interval_time - sweep_end_time;
+
+
+            int remapped_scan_idx;
+            if (c32_fpga_type == 2) {
+                remapped_scan_idx = fir_idx % 32;
+            } else {
+                remapped_scan_idx = (fir_idx % 32) % 2 == 0 ? (fir_idx % 32) / 2 : (fir_idx % 32) / 2 + 16;
+            }
+            sweep_data->points.push_back(lslidar_msgs::LslidarPoint());
+            lslidar_msgs::LslidarPoint &new_point = sweep_data->points[
+                    sweep_data->points.size() - 1];
+            //pack the data into point msg
+            new_point.time = time;
+            new_point.x = x_coord;
+            new_point.y = y_coord;
+            new_point.z = z_coord;
+            new_point.intensity = firings.intensity[fir_idx];
+            new_point.ring = remapped_scan_idx;
+            new_point.azimuth = firings.azimuth[fir_idx];
+            new_point.distance = firings.distance[fir_idx];
+        }
+        // a new sweep begins ----------------------------------------------------
+
+        if (end_fir_idx != SCANS_PER_PACKET) {
+            //publish Last frame scan
+
+            sweep_end_time =  packet_timestamp - (SCANS_PER_PACKET - end_fir_idx -1) * packet_interval_time;
+            sweep_end_time = sweep_end_time > 0 ? sweep_end_time : 0;
+            publishPointcloud();
+            if (publish_scan) publishScan();
+
+            sweep_data = lslidar_msgs::LslidarScanPtr(new lslidar_msgs::LslidarScan());
+            //prepare the next frame scan
+            //sweep_start_time = packet->stamp.toSec() - (end_fir_idx - start_fir_idx) * 3.125 * 1e-6;
+            last_azimuth = last_azimuth_tmp;
+            start_fir_idx = end_fir_idx;
+            end_fir_idx = SCANS_PER_PACKET;
+            for (size_t fir_idx = start_fir_idx; fir_idx < end_fir_idx; ++fir_idx) {
+
+                //check if the point is valid
+                if (!isPointInRange(firings.distance[fir_idx]))continue;
+                if (firings.azimuth[fir_idx] >= 36000) {
+                    firings.azimuth[fir_idx] = firings.azimuth[fir_idx] - 36000;
+                }
+                //convert the point to xyz coordinate
+                size_t table_idx = firings.azimuth[fir_idx];
+                double cos_azimuth = cos_azimuth_table[table_idx];
+                double sin_azimuth = sin_azimuth_table[table_idx];
+                double x_coord, y_coord, z_coord;
+                if (coordinate_opt) {
+                  int tmp_idx = 1298 - firings.azimuth[fir_idx] < 0 ? 1298 - firings.azimuth[fir_idx] + 36000 : 1298 - firings.azimuth[fir_idx];
+                    x_coord = firings.distance[fir_idx] * c32_cos_scan_altitude[fir_idx % 32] * cos_azimuth +
+                              R2_ * cos((12.98 - firings.azimuth[fir_idx] * 0.01) * DEG_TO_RAD);
+                    y_coord = -firings.distance[fir_idx] * c32_cos_scan_altitude[fir_idx % 32] * sin_azimuth +
+                              R2_ * sin((12.98 - firings.azimuth[fir_idx] * 0.01) * DEG_TO_RAD);
+                    z_coord = firings.distance[fir_idx] * c32_sin_scan_altitude[fir_idx % 32];
+
+                } else {
+                    //Y-axis correspondence 0 degree
+                    int tmp_idx = firings.azimuth[fir_idx] - 1298 < 0 ? firings.azimuth[fir_idx] -1298 + 36000 : firings.azimuth[fir_idx] -1298;
+                    x_coord = firings.distance[fir_idx] * c32_cos_scan_altitude[fir_idx % 32] * sin_azimuth +
+                              R2_ * sin((firings.azimuth[fir_idx] * 0.01 - 12.98) * DEG_TO_RAD);
+                    y_coord = firings.distance[fir_idx] * c32_cos_scan_altitude[fir_idx % 32] * cos_azimuth +
+                              R2_ * cos((firings.azimuth[fir_idx] * 0.01 - 12.98) * DEG_TO_RAD);
+                    z_coord = firings.distance[fir_idx] * c32_sin_scan_altitude[fir_idx % 32];
+
+                }
+                // computer the time of the point
+                double time = packet_timestamp  -
+                              (SCANS_PER_PACKET - fir_idx - 1) * packet_interval_time - sweep_end_time;
+                int remapped_scan_idx;
+                if (c32_fpga_type == 2) {
+                    remapped_scan_idx = fir_idx % 32;
+                } else {
+                    remapped_scan_idx = (fir_idx % 32) % 2 == 0 ? (fir_idx % 32) / 2 : (fir_idx % 32) / 2 + 16;
+                }
+                sweep_data->points.push_back(lslidar_msgs::LslidarPoint());
+                lslidar_msgs::LslidarPoint &new_point = sweep_data->points[
+                        sweep_data->points.size() - 1];
+                //pack the data into point msg
+                new_point.time = time;
+                new_point.x = x_coord;
+                new_point.y = y_coord;
+                new_point.z = z_coord;
+                new_point.intensity = firings.intensity[fir_idx];
+                new_point.ring = remapped_scan_idx;
+                new_point.azimuth = firings.azimuth[fir_idx];
+                new_point.distance = firings.distance[fir_idx];
+            }
+
+        }
+        //packet_pub.publish(*packet);
+        return true;
+    }
+
+
+}  // namespace lslidar_driver

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_ros/lslidar_driver/src/lslidar_driver_node.cpp

@@ -0,0 +1,48 @@
+#include <ros/ros.h>
+#include "lslidar_driver/lslidar_driver.h"
+#include "std_msgs/String.h"
+
+using namespace lslidar_driver;
+volatile sig_atomic_t flag = 1;
+
+static void my_handler(int sig) {
+    flag = 0;
+}
+
+int main(int argc, char **argv) {
+    ros::init(argc, argv, "lslidar_driver");
+    ros::NodeHandle node;
+    ros::NodeHandle private_nh("~");
+
+    signal(SIGINT, my_handler);
+    private_nh.param("lidar_type", lidar_type, std::string("c16"));
+    ROS_INFO("lslidar type: %s", lidar_type.c_str());
+
+    // start the driver
+    if ("c16" == lidar_type) {
+        lslidar_driver::lslidarC16Driver dvr(node, private_nh);
+        // loop until shut down or end of file
+        if (!dvr.initialize()) {
+            ROS_ERROR("cannot initialize lslidar driver.");
+            return 0;
+        }
+        while (ros::ok() ) {
+            dvr.poll();
+            ros::spinOnce();
+        }
+
+    } else {
+        lslidar_driver::lslidarC32Driver dvr(node, private_nh);
+        // loop until shut down or end of file
+        if (!dvr.initialize()) {
+            ROS_ERROR("cannot initialize lslidar driver.");
+            return 0;
+        }
+        while (ros::ok()) {
+            dvr.poll();
+            ros::spinOnce();
+        }
+
+    }
+    return 0;
+}

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_ros/lslidar_driver/src/lslidar_driver_nodelet.cc

@@ -0,0 +1,63 @@
+#include <string>
+#include <boost/thread.hpp>
+
+#include <ros/ros.h>
+#include <pluginlib/class_list_macros.h>
+#include <nodelet/nodelet.h>
+
+#include "lslidar_driver/lslidar_driver.h"
+
+volatile sig_atomic_t flag = 1;
+
+namespace lslidar_driver {
+    class DriverNodelet : public nodelet::Nodelet {
+    public:
+        DriverNodelet() : running_(false) {
+        }
+
+        ~DriverNodelet() {
+            if (running_) {
+                NODELET_INFO("shutting down driver thread");
+                running_ = false;
+                deviceThread_->join();
+                NODELET_INFO("driver thread stopped");
+            }
+        }
+
+    private:
+        virtual void onInit(void);
+
+        virtual void devicePoll(void);
+
+        volatile bool running_;  ///< device thread is running
+        boost::shared_ptr<boost::thread> deviceThread_;
+
+        boost::shared_ptr<lslidarDriver> dvr_;  ///< driver implementation class
+    };
+
+    void DriverNodelet::onInit() {
+        // start the driver
+        if (lidar_type == "c16") {
+            dvr_.reset(new lslidarC16Driver(getNodeHandle(), getPrivateNodeHandle()));
+        } else {
+            dvr_.reset(new lslidarC32Driver(getNodeHandle(), getPrivateNodeHandle()));
+        }
+
+        // spawn device poll thread
+        running_ = true;
+        deviceThread_ = boost::shared_ptr<boost::thread>(
+                new boost::thread(boost::bind(&DriverNodelet::devicePoll, this)));
+        // NODELET_INFO("DriverNodelet onInit");
+    }
+
+/** @brief Device poll thread main loop. */
+    void DriverNodelet::devicePoll() {
+        while (ros::ok()) {
+            dvr_->poll();
+            ros::spinOnce();
+        }
+    }
+} //namespace
+
+// parameters are: class type, base class type
+PLUGINLIB_EXPORT_CLASS(lslidar_driver::DriverNodelet, nodelet::Nodelet)

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_ros/lslidar_msgs/CMakeLists.txt

@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(lslidar_msgs)
+
+set(CMAKE_BUILD_TYPE Release)
+find_package(catkin REQUIRED COMPONENTS
+  message_generation
+  std_msgs
+  sensor_msgs
+)
+
+add_message_files(
+  DIRECTORY msg
+  FILES
+  LslidarPacket.msg
+  LslidarPoint.msg
+  LslidarScan.msg
+)
+
+
+add_service_files(
+  FILES
+  lslidar_control.srv
+)
+
+
+generate_messages(DEPENDENCIES std_msgs sensor_msgs)
+
+catkin_package(
+  CATKIN_DEPENDS message_runtime std_msgs sensor_msgs
+)

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_ros/lslidar_msgs/msg/LslidarPacket.msg

@@ -0,0 +1,5 @@
+# Raw Leishen LIDAR packet.
+
+time stamp              # packet timestamp
+uint8[1212] data        # packet contents
+

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_ros/lslidar_msgs/msg/LslidarPoint.msg

@@ -0,0 +1,14 @@
+# Time when the point is captured
+float64 time
+
+# Converted distance in the sensor frame
+float64 x
+float64 y
+float64 z
+
+# Raw measurement from Leishen C16
+float64 azimuth
+float64 distance
+float64 intensity
+uint16 ring
+

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_ros/lslidar_msgs/msg/LslidarScan.msg

@@ -0,0 +1,6 @@
+# Altitude of all the points within this scan
+float64 altitude
+
+# The valid points in this scan sorted by azimuth
+# from 0 to 359.99
+LslidarPoint[] points

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_ros/lslidar_msgs/package.xml

@@ -0,0 +1,22 @@
+<package>
+
+  <name>lslidar_msgs</name>
+  <version>1.2.0</version>
+  <description>
+    ROS message definitions for Leishen C16 LIDARs.
+  </description>
+  <url>http://www.ros.org/wiki/lslidar_c16</url>
+  <maintainer email="yutongyan@lslidar.com">Yutong</maintainer>
+  <author>Yutong</author>
+  <license>GNU General Public License V3.0</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>message_generation</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <run_depend>message_runtime</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>std_msgs</run_depend>
+
+</package>

hinge_car_decision_nav_task_sensor/src/sensor/lslidar_ros/lslidar_msgs/srv/lslidar_control.srv

@@ -0,0 +1,3 @@
+int32 LaserControl
+---
+bool status

hinge_car_decision_nav_task_sensor/src/sensor/nalei_radar/CMakeLists.txt

@@ -0,0 +1,209 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(nalei_radar)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## 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
+  can_msgs
+  pcl_ros
+  roscpp
+  sensor_msgs
+  std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   can_msgs#   sensor_msgs#   std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## 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 also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES nalei_radar
+#  CATKIN_DEPENDS can_msgs pcl_ros roscpp sensor_msgs std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/nalei_radar.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+add_executable(${PROJECT_NAME} src/nalei_radar_node.cpp)
+# add_executable(${PROJECT_NAME} src/test.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_nalei_radar.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

hinge_car_decision_nav_task_sensor/src/sensor/nalei_radar/launch/nalei_radar.launch

@@ -0,0 +1,11 @@
+<launch>
+<!-- can 口 -->
+  <arg name="dev_can" default=" can1" />    
+
+  <node pkg="nalei_radar" type="nalei_radar" name="nalei_radar" output="screen">
+    <!-- <param name="dev_can" value="$(arg dev_can)" /> -->
+    
+  </node>
+  
+</launch>
+

hinge_car_decision_nav_task_sensor/src/sensor/nalei_radar/package.xml

@@ -0,0 +1,74 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>nalei_radar</name>
+  <version>0.0.0</version>
+  <description>The nalei_radar package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="user@todo.todo">user</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/nalei_radar</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>can_msgs</build_depend>
+  <build_depend>pcl_ros</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>can_msgs</build_export_depend>
+  <build_export_depend>pcl_ros</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>sensor_msgs</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>can_msgs</exec_depend>
+  <exec_depend>pcl_ros</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

hinge_car_decision_nav_task_sensor/src/sensor/nalei_radar/src/nalei_radar_node.cpp

@@ -0,0 +1,102 @@
+#include <ros/ros.h>
+#include <can_msgs/Frame.h>
+#include <std_msgs/Int32.h>
+#include <vector>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <net/if.h>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+#include <linux/can.h>
+#include <linux/can/raw.h>
+#include <iostream>
+
+uint8_t can_60a_buf[8] = {0x00};
+int noofobjetcs = 0;
+uint8_t can_60b_buf[8] = {0x00};
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "sr73_lidar");
+    ros::NodeHandle nh;
+    ros::Publisher nalei_radar_pub = nh.advertise<std_msgs::Int32>("/nalei_radar_dis", 1);
+
+    int s, nbytes;
+    struct sockaddr_can addr;
+    struct ifreq ifr;
+    // 从CAN总线读取数据帧
+    struct can_frame frame;
+
+    s = socket(PF_CAN, SOCK_RAW, CAN_RAW); // 创建套接字
+    strcpy(ifr.ifr_name, "can1");
+    ioctl(s, SIOCGIFINDEX, &ifr); // 指定 can1设备
+    addr.can_family = AF_CAN;
+    addr.can_ifindex = ifr.ifr_ifindex;
+    bind(s, (struct sockaddr *)&addr, sizeof(addr)); // 将套接字与 can1 绑定
+
+    ros::Rate loop_rate(20);
+    // ros::Rate loop_rate(1);
+
+    while (ros::ok())
+    {
+        // int index=0;
+        int bytes = read(s, &frame, sizeof(frame));
+        std_msgs::Int32 dis_msg;
+
+        std::vector<int32_t> distance_array;
+
+        if (frame.can_id == 0x60a)
+        {
+            memcpy(can_60a_buf, &frame.data, 8);
+
+            uint8_t buf1 = can_60a_buf[0];
+            noofobjetcs = buf1; // 目标个数
+            // std::cout << "size" << noofobjetcs;
+
+            for (int i = 0; i < noofobjetcs; i++)
+            {
+                int bytes = read(s, &frame, sizeof(frame));
+                if (frame.can_id == 0x60b)
+                {
+
+                    memcpy(can_60b_buf, &frame.data, 8);
+
+                    int obj_id = 0;
+                    uint8_t buf1 = can_60b_buf[0];
+                    obj_id = buf1;
+
+                    int c_l_1 = can_60b_buf[1];
+                    int c_l_2 = can_60b_buf[2];
+
+                    c_l_1 = c_l_1 * 32;
+                    c_l_2 = c_l_2 >> 3;
+
+                    float dis_long2 = ((c_l_1 + c_l_2) * 0.2f - 500.0f) * 1000; // 毫米  纵向
+                    // float dis_lat2 = ((can_60b_buf[2] & 0x07) * 256 + can_60b_buf[3]) * 0.2 - 204.6;  横向
+                    // std::cout << dis_long2 << std::endl;
+
+                    // msg.data.push_back(static_cast<int32_t>(dis_long2));
+                    distance_array.push_back(dis_long2);
+                }
+            }
+
+            if (!distance_array.empty())
+            {
+                auto smallest = std::min_element(std::begin(distance_array), std::end(distance_array));
+                // std::cout << "min element is " << *smallest<< std::endl;
+                dis_msg.data = *smallest;
+                nalei_radar_pub.publish(dis_msg);
+            }
+            distance_array.clear(); // 清空 vector
+        }
+        // std::cout << "-------"<< std::endl;
+
+        ros::spinOnce();
+        loop_rate.sleep();
+    }
+    close(s);
+    return 0;
+}

hinge_car_decision_nav_task_sensor/src/sensor/outrigger/CMakeLists.txt

@@ -0,0 +1,211 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(outrigger)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## 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
+  std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## 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 also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES outrigger
+#  CATKIN_DEPENDS roscpp std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+set(LIBMODBUS_INCLUDE_DIRS /usr/local/include/modbus)
+set(LIBMODBUS_LIBRARIES /usr/local/lib/libmodbus.so)
+
+include_directories(${LIBMODBUS_INCLUDE_DIRS})
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/outrigger.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+add_executable(${PROJECT_NAME} src/outrigger_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+  ${LIBMODBUS_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_outrigger.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

hinge_car_decision_nav_task_sensor/src/sensor/outrigger/README.md

@@ -0,0 +1,2 @@
+# 左右支腿   modbus-tcp
+# 主机发，从机回

hinge_car_decision_nav_task_sensor/src/sensor/outrigger/launch/outrigger.launch

@@ -0,0 +1,25 @@
+<launch>
+  <arg name="dev_ip" default="192.168.3.23 " />    
+  <arg name="dev_port" default=" 10003" />    
+  <arg name="pub_topic_left" default=" outrigger_left" />    
+  <arg name="pub_topic_right" default=" outrigger_right" />    
+  <arg name="slave_id_left" default="3" />    
+  <arg name="slave_id_right" default="2" />    
+
+  <node pkg="outrigger" type="outrigger" name="outrigger_left" output="screen">
+    <param name="dev_ip" value="$(arg dev_ip)" />
+    <param name="dev_port" value="$(arg dev_port)" />
+    <param name="pub_topic" value="$(arg pub_topic_left)" />
+    <param name="slave_id" value="$(arg slave_id_left)" />
+    
+  </node>
+
+  <node pkg="outrigger" type="outrigger" name="outrigger_right" output="screen">
+    <param name="dev_ip" value="$(arg dev_ip)" />
+    <param name="dev_port" value="$(arg dev_port)" />
+    <param name="pub_topic" value="$(arg pub_topic_right)" />
+    <param name="slave_id" value="$(arg slave_id_right)" />
+    
+  </node>
+ 
+</launch>

hinge_car_decision_nav_task_sensor/src/sensor/outrigger/package.xml

@@ -0,0 +1,65 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>outrigger</name>
+  <version>0.0.0</version>
+  <description>The outrigger package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="user@todo.todo">user</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/outrigger</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

hinge_car_decision_nav_task_sensor/src/sensor/outrigger/src/outrigger_node.cpp

@@ -0,0 +1,105 @@
+#include <ros/ros.h>
+#include <std_msgs/String.h>
+#include <modbus/modbus.h>
+#include <iostream>
+#include <std_msgs/Float64.h>
+#include <std_msgs/Int8.h>
+#include <std_msgs/Int64.h>
+#include <std_msgs/Int16.h>
+#include <std_msgs/Bool.h>
+#include <std_msgs/Float32MultiArray.h>
+
+using std::cout;
+using std::dec;
+using std::endl;
+using std::hex;
+using std::string;
+
+uint16_t fb_registers[4];
+
+void printfb()
+{
+    std::cout << "==========================" << std::endl;
+    for (size_t i = 0; i < 4; ++i)
+    {
+        std::cout << std::hex << std::setw(4) << std::setfill('0') << fb_registers[i] << ' ';
+    }
+    std::cout << dec << std::endl;
+    for (size_t i = 0; i < 4; ++i)
+    {
+        std::cout << static_cast<short>(fb_registers[i]) << " ";
+    }
+    std::cout << std::endl;
+}
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "outrigger");
+    ros::NodeHandle nh;
+    ros::NodeHandle nh2("~");
+
+    string dev_ip;
+    int dev_port;
+    string pub_topic;
+    int slave_id;
+
+    nh2.getParam("dev_ip", dev_ip);
+    nh2.getParam("dev_port", dev_port);
+    nh2.getParam("pub_topic", pub_topic);
+    nh2.getParam("slave_id", slave_id);
+
+    ros::Publisher fb_dyp_pub = nh.advertise<std_msgs::Float32MultiArray>(pub_topic, 10);
+
+    modbus_t *mb;
+    int rc;
+
+    mb = modbus_new_tcp(dev_ip.c_str(), dev_port);
+    if (mb == NULL)
+    {
+        std::cout << "connect error" << std::endl;
+        return -1;
+    }
+    else
+    {
+        std::cout << "connect ok" << std::endl;
+    }
+
+    rc = modbus_set_slave(mb, slave_id); // slave_id  从站地址
+
+    rc = modbus_connect(mb);
+    if (rc == -1)
+    {
+        std::cout << "error 2" << std::endl;
+        modbus_free(mb);
+        return -1;
+    }
+
+    rc = modbus_set_debug(mb, false);
+
+    ros::Rate loop_rate(1);
+    while (ros::ok())
+    {
+        std_msgs::Float32MultiArray msg;
+
+        // rc = modbus_write_registers(mb,)   // 02 03 00 00 00 10 44 35 -> 02:从机地址  03读寄存器功能码  00 00 要读的寄存器起始地址为00   00 10要读的寄存器个数16  4435 从字节1到6的crc校验
+        rc = modbus_read_registers(mb, 00, 16, fb_registers);
+
+        for (size_t i = 0; i < 2; ++i)
+        {
+
+            msg.data.push_back(static_cast<short>(fb_registers[i]));
+        }
+
+        ros::Duration(0.2).sleep();
+        // std::cout << "################################### 2" << std::endl;
+
+        fb_dyp_pub.publish(msg);
+
+        ros::spinOnce();
+        loop_rate.sleep();
+    }
+    modbus_close(mb);
+    modbus_free(mb);
+
+    return 0;
+}

hinge_car_decision_nav_task_sensor/src/sensor/radar_dyp_modbus/CMakeLists.txt

@@ -0,0 +1,214 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(radar_dyp_modbus)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## 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
+  std_msgs
+  serial
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## 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 also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES radar_dyp_modbus
+ CATKIN_DEPENDS roscpp std_msgs serial
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+set(LIBMODBUS_INCLUDE_DIRS /usr/local/include/modbus)
+set(LIBMODBUS_LIBRARIES /usr/local/lib/libmodbus.so)
+
+include_directories(${LIBMODBUS_INCLUDE_DIRS})
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/radar_dyp_modbus.cpp
+# )
+## Add serial library
+# add_library(serial ${serial_SRCS})
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME} src/radar_dyp_RS485.cpp)
+add_executable(${PROJECT_NAME} src/radar_dyp_modbus.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(${PROJECT_NAME}
+   ${catkin_LIBRARIES}
+   ${LIBMODBUS_LIBRARIES}
+ )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_radar_dyp_modbus.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

hinge_car_decision_nav_task_sensor/src/sensor/radar_dyp_modbus/Readme.md

@@ -0,0 +1 @@
+#  电应普超声波雷达   直接连接AD10  modbus-rtu