first commit

README.md

@@ -1,2 +1,3 @@
 # mid360
 
+rostopic pub /goal_pose fast_lio_localization/GoalPose "{x: 2.0, y: 2.0, angle: 0.5, max_speed: 10.0, tolerance: 0.1, rotor: true}"

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/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

@@ -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" />
 
@@ -36,8 +36,9 @@
 	</include> -->
 
 	<!-- load 2d map -->
-	<arg name="2dmap" default="M3.yaml" />
-	<node name = "map_server" pkg = "map_server" type = "map_server" args="$(find fast_lio_localization)/PCD/M3.yaml /map:=prior_map"/>
+	<arg name="2dmap" default="M4.yaml" />
+
+	<node name = "map_server" pkg = "map_server" type = "map_server" args="$(find fast_lio_localization)/PCD/map1.yaml /map:=prior_map"/>
 
 	<!-- 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

@@ -0,0 +1,5 @@
+<launch>
+<include file="$(find livox_ros_driver2)/launch_ROS1/msg_MID360.launch"/>
+<include file="$(find fast_lio_localization)/launch/localization_MID360.launch"/>
+
+</launch>

src/FAST_LIO_LOCALIZATION/scripts/global_localization.py

@@ -216,7 +216,7 @@ if __name__ == '__main__':
 
     # The threshold of global localization,
     # only those scan2map-matching with higher fitness than LOCALIZATION_TH will be taken
-    LOCALIZATION_TH = 0.95
+    LOCALIZATION_TH = 0.8
 
     # FOV(rad), modify this according to your LiDAR type
     FOV = 6.28

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,45 @@ 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, 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) {
+//     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 +182,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[16] = {0};//
+    buf[0]=0xFF;
+    buf[1]=0x09;
+    buf[2]=0x01;
+    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>
-
+extern 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);

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/cmrt/mid360/ws_livox/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" />
 <!-- 选取的范围　最小的高度-->