2

src/FAST_LIO_LOCALIZATION/PCD/M3.yaml

@@ -1,7 +0,0 @@
-image: /home/cmrt/ws_livox/src/FAST_LIO_LOCALIZATION/PCD/M3.pgm
-resolution: 0.050000
-origin: [-18.653002, -10.328159, 0.000000]
-negate: 0
-occupied_thresh: 0.65
-free_thresh: 0.196
-

src/FAST_LIO_LOCALIZATION/PCD/M4.yaml

@@ -1,7 +0,0 @@
-image: /home/robofish/jyc/mid360/src/FAST_LIO_LOCALIZATION/PCD/M4.pgm
-resolution: 0.050000
-origin: [-22.123611, -17.241680, 0.000000]
-negate: 0
-occupied_thresh: 0.65
-free_thresh: 0.196
-

src/FAST_LIO_LOCALIZATION/PCD/m4.yaml

@@ -1,7 +0,0 @@
-image: /home/cmrt/ws_livox/src/FAST_LIO_LOCALIZATION/PCD/m4.pgm
-resolution: 0.100000
-origin: [-17.300000, -16.800000, -nan]
-negate: 0
-occupied_thresh: 0.65
-free_thresh: 0.196
-

src/FAST_LIO_LOCALIZATION/PCD/map1.yaml

@@ -0,0 +1,7 @@
+image: /home/cmrt/ws_livox/src/FAST_LIO_LOCALIZATION/PCD/map1.pgm
+resolution: 0.050000
+origin: [-13.175220, -3.068274, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

src/FAST_LIO_LOCALIZATION/launch/localization_MID360.launch

@@ -37,11 +37,8 @@
 
 	<!-- load 2d map -->
 	<arg name="2dmap" default="M4.yaml" />
-<<<<<<< HEAD
+
-	<node name = "map_server" pkg = "map_server" type = "map_server" args="$(find fast_lio_localization)/PCD/m4.yaml /map:=prior_map"/>
+	<node name = "map_server" pkg = "map_server" type = "map_server" args="$(find fast_lio_localization)/PCD/map1.yaml /map:=prior_map"/>
-=======
-	<node name = "map_server" pkg = "map_server" type = "map_server" args="$(find fast_lio_localization)/PCD/M4.yaml /map:=prior_map"/>
->>>>>>> 710b3fc3ab857cb452f1e5c38d1599436191b8de
 
 	<!-- pointscloud2 to laserscans -->
 	<include file="$(find fast_lio_localization)/launch/PointsCloud2toLaserscan.launch">

src/FAST_LIO_LOCALIZATION/launch/move.launch

@@ -0,0 +1,5 @@
+<launch>
+<node pkg = "fast_lio_localization" type = "flag" name = "flag" output="screen"/>
+<node pkg = "fast_lio_localization" type = "controlpid" name = "controlpid" output="screen"/>
+
+</launch>

src/FAST_LIO_LOCALIZATION/launch/nav.launch

@@ -1,7 +1,5 @@
 <launch>
 <include file="$(find livox_ros_driver2)/launch_ROS1/msg_MID360.launch"/>
 <include file="$(find fast_lio_localization)/launch/localization_MID360.launch"/>
-<node pkg = "fast_lio_localization" type = "flag" name = "flag" output="screen"/>
+
-<!-- <node pkg = "fast_lio_localization" type = "controlpid" name = "controlpid" output="screen"/> -->
-<node pkg = "fast_lio_localization" type = "publish_initial_pose.py" name = "publish_initial_pose" arg="0 0 0 0 0 0 "/>
 </launch>

src/FAST_LIO_LOCALIZATION/src/controlpid.cpp

@@ -47,7 +47,7 @@ public:
             {2.981, 1.60, -M_PI/2},
         };
         target_ = target_poses_[0];
-        laser_sub = nh_.subscribe<sensor_msgs::LaserScan>("/scan", 10, &PoseController::laser_callback, this);
+        //laser_sub = nh_.subscribe<sensor_msgs::LaserScan>("/scan", 10, &PoseController::laser_callback, this);
         pose_subscriber_ = nh_.subscribe("filtered_pose", 10, &PoseController::pose_callback, this);
         cmd_pid_publisher_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel_pid", 10);
         //position_id_subscriber_ = nh_.subscribe("/position_id", 10, &PoseController::position_id_callback, this);
@@ -134,31 +134,32 @@ private:
     //发送
     void serial_write3double_callback(const geometry_msgs::Twist::ConstPtr& msg) {
         //write3float(msg->linear.x, msg->linear.y, msg->angular.z);
-        write3float2(msg->linear.x, msg->linear.y, msg->angular.z);//lzcdxc
+        write3float2(msg->linear.x, msg->linear.y, 0//msg->angular.z);//lzcdxc
+        ROS_INFO("linear.x: %f, linear.y: %f, angular.z: %f", msg->linear.x, msg->linear.y, msg->angular.z);
     }
 
     //避障
-    void laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg) {
+//     void laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg) {
-    static const float min_distance = 0.45; // 最小安全距离，单位：米
+//     static const float min_distance = 0.45; // 最小安全距离，单位：米
-    bool obstacle_detected = false;
+//     bool obstacle_detected = false;
 
-    for (size_t i = 0; i < msg->ranges.size(); i=i+30) {
+//     for (size_t i = 0; i < msg->ranges.size(); i=i+30) {
-        float distance = msg->ranges[i];
+//         float distance = msg->ranges[i];
-        if (distance < min_distance && !std::isnan(distance)) {
+//         if (distance < min_distance && !std::isnan(distance)) {
-            obstacle_detected = true;
+//             obstacle_detected = true;
-            break;
+//             break;
-        }
+//         }
-    }
+//     }
 
-    if (obstacle_detected) {
+//     if (obstacle_detected) {
-        // 处理避障逻辑
+//         // 处理避障逻辑
-        ROS_WARN("stop！！！");
+//         ROS_WARN("stop！！！");
-        stopRobot();
+//         stopRobot();
-    } else {
+//     } else {
-        // 继续正常运动
+//         // 继续正常运动
-        continueMoving();
+//         continueMoving();
-    }
+//     }
-}
+// }
 void stopRobot() {
     geometry_msgs::Twist stop_cmd;
     stop_cmd.linear.x = 0;

src/FAST_LIO_LOCALIZATION/src/mbot_linux_serial.cpp

@@ -144,8 +144,8 @@ void write3float2(float x,float y,float yaw)
     Y.d = y;
     YAW.d = yaw;
 
-   unsigned char buf[15] = {0};//
+   unsigned char buf[16] = {0};//
-    buf[0]=0x11;
+    buf[0]=0xFF;
     buf[1]=0x09;
     buf[2]=0x00;
     buf[15]=0xFE;

src/pcd2pgm/launch/run.launch

@@ -2,7 +2,7 @@
 <launch>
 <node pkg="pcd2pgm" name="pcd2pgm" type="pcd2pgm" output="screen">
 <!-- 存放pcd文件的路径-->
-<param name="file_directory" value= "/home/robofish/jyc/mid360/src/FAST_LIO/PCD/" />
+<param name="file_directory" value= "/home/cmrt/ws_livox/src/FAST_LIO_LOCALIZATION/PCD/" />
 <!-- pcd文件名称-->
 <param name="file_name" value= "scans" />
 <!-- 选取的范围　最小的高度-->