Merge branch 'main' of ssh://gitea.qutcmrt.top:222/hara/mid360 into main

src/FAST_LIO_LOCALIZATION/CMakeLists.txt

@@ -88,11 +88,11 @@ catkin_package(
 
 add_executable(fastlio_mapping1 src/laserMapping.cpp include/ikd-Tree/ikd_Tree.cpp src/preprocess.cpp)
 add_executable(flag src/flag.cpp)
-add_executable(send src/send.cpp src/mbot_linux_serial.cpp)
+add_executable(controlpid src/controlpid.cpp src/mbot_linux_serial.cpp)
 target_link_libraries(fastlio_mapping1 ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES})
 target_link_libraries(flag ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES})
-target_link_libraries(send ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES})
+target_link_libraries(controlpid ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES})
 target_include_directories(fastlio_mapping1 PRIVATE ${PYTHON_INCLUDE_DIRS})
 target_include_directories(flag PRIVATE ${PYTHON_INCLUDE_DIRS})
-target_include_directories(send PRIVATE ${PYTHON_INCLUDE_DIRS})
+target_include_directories(controlpid PRIVATE ${PYTHON_INCLUDE_DIRS})
 add_dependencies(fastlio_mapping fast_lio_generate_messages_cpp)

src/FAST_LIO_LOCALIZATION/PCD/M3.yaml

@@ -0,0 +1,7 @@
+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/launch/localization_MID360.launch

@@ -16,7 +16,7 @@
 
 
 	<!-- <arg name="map" default="/home/rm/ws_sentry/src/FAST_LIO/PCD(305_10-20)/scans.pcd" /> -->
-	<arg name="map" default="$(find fast_lio)/PCD/scans.pcd" />
+	<arg name="map" default="$(find fast_lio_localization)/PCD/scans.pcd" />
 	<!-- loalization-->
     <node pkg="fast_lio_localization" type="global_localization.py" name="global_localization" output="screen" />
 

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);
         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);
@@ -131,9 +131,44 @@ private:
         cmd.angular.z = control_yaw;
         cmd_pid_publisher_.publish(cmd);//发送pid
     }
+    //发送
     void serial_write3double_callback(const geometry_msgs::Twist::ConstPtr& msg) {
-        write3float(msg->linear.x, msg->linear.y, msg->angular.z);
+        //write3float(msg->linear.x, msg->linear.y, msg->angular.z);
+        write3float2(msg->linear.x, msg->linear.y, msg->angular.z);//lzcdxc
     }
+
+    //避障
+    void laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg) {
+    static const float min_distance = 0.45; // 最小安全距离，单位：米
+    bool obstacle_detected = false;
+
+    for (size_t i = 0; i < msg->ranges.size(); i=i+30) {
+        float distance = msg->ranges[i];
+        if (distance < min_distance && !std::isnan(distance)) {
+            obstacle_detected = true;
+            break;
+        }
+    }
+
+    if (obstacle_detected) {
+        // 处理避障逻辑
+        ROS_WARN("stop！！！");
+        stopRobot();
+    } else {
+        // 继续正常运动
+        continueMoving();
+    }
+}
+void stopRobot() {
+    geometry_msgs::Twist stop_cmd;
+    stop_cmd.linear.x = 0;
+    stop_cmd.angular.z = 0;
+    cmd_pid_publisher_.publish(stop_cmd);
+}
+
+void continueMoving() {
+    // 继续向目标移动的逻辑
+}
     ros::NodeHandle nh_;
     ros::Subscriber pose_subscriber_;
     ros::Subscriber goal_subscriber_;
@@ -146,7 +181,9 @@ private:
     std::vector<TargetPose> target_poses_;
     PIDController x_controller_, y_controller_, yaw_controller_;
     TargetPose target_;
+    ros::Subscriber laser_sub;
     int robot_mode_;
+    float min_distance = 0.5; // 最小安全距离，单位：米
 };
 
 int main(int argc, char **argv) {

src/FAST_LIO_LOCALIZATION/src/flag.cpp

@@ -20,7 +20,7 @@ int main(int argc, char** argv){
     tf::StampedTransform transform_listener;
     
     try{
-      listener.lookupTransform("robot_foot_init", "body",  
+      listener.lookupTransform("map", "body",  
                                ros::Time(0), transform_listener);
     }
     catch (tf::TransformException ex){
@@ -44,9 +44,6 @@ int main(int argc, char** argv){
     pose.pose.orientation.z = robot_oriation_z;  
     pose.pose.orientation.w = robot_oriation_w;  
     pose_publisher.publish(pose);
-    transform_broadcaster.setOrigin( tf::Vector3(2,2, 0.0) );
-    transform_broadcaster.setRotation( tf::Quaternion(0, 0, 0,1) );
-    //broadcaster.sendTransform(tf::StampedTransform(transform_broadcaster, ros::Time::now(), "map", "flag"));
     
 
     rate.sleep();

src/FAST_LIO_LOCALIZATION/src/mbot_linux_serial.cpp

@@ -138,6 +138,24 @@ void write3float(float x,float y,float yaw)
     // 通过串口下发数据
     boost::asio::write(sp, boost::asio::buffer(buf));
 
+}
+void write3float2(float x,float y,float yaw)
+{   X.d  = x;//mm/s
+    Y.d = y;
+    YAW.d = yaw;
+
+   unsigned char buf[15] = {0};//
+    buf[0]=0x11;
+    buf[1]=0x09;
+    buf[2]=0x00;
+    buf[15]=0xFE;
+    for(int i=0;i<4;i++){
+        buf[i+3]=X.data[i];
+        buf[i+7]=Y.data[i];
+        buf[i+11]=YAW.data[i];
+    }
+    boost::asio::write(sp, boost::asio::buffer(buf));
+
 }
 std::string string2hex(const std::string& input)
 {

src/FAST_LIO_LOCALIZATION/src/mbot_linux_serial.h

@@ -8,7 +8,7 @@
 #include <nav_msgs/Odometry.h>
 #include <boost/asio.hpp>
 #include <geometry_msgs/Twist.h>
-
+void write3float2(float x,float y,float yaw);
 extern void serialInit();
 extern void writeSpeed(double Left_v, double Right_v,unsigned char ctrlFlag);
 extern void write3float(float x, float y, float yaw);