README.md

@@ -1,3 +1,2 @@
 # 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(controlpid src/controlpid.cpp src/mbot_linux_serial.cpp)
+add_executable(send src/send.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(controlpid ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES})
+target_link_libraries(send ${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(controlpid PRIVATE ${PYTHON_INCLUDE_DIRS})
+target_include_directories(send PRIVATE ${PYTHON_INCLUDE_DIRS})
 add_dependencies(fastlio_mapping fast_lio_generate_messages_cpp)

src/FAST_LIO_LOCALIZATION/PCD/map1.yaml

@@ -1,7 +0,0 @@
-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_localization)/PCD/scans.pcd" />
+	<arg name="map" default="$(find fast_lio)/PCD/scans.pcd" />
 	<!-- loalization-->
     <node pkg="fast_lio_localization" type="global_localization.py" name="global_localization" output="screen" />
 
@@ -36,9 +36,8 @@
 	</include> -->
 
 	<!-- load 2d 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"/>
+	<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"/>
 
 	<!-- pointscloud2 to laserscans -->
 	<include file="$(find fast_lio_localization)/launch/PointsCloud2toLaserscan.launch">

src/FAST_LIO_LOCALIZATION/launch/move.launch

@@ -1,5 +0,0 @@
-<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,5 +0,0 @@
-<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.8
+    LOCALIZATION_TH = 0.95
 
     # FOV(rad), modify this according to your LiDAR type
     FOV = 6.28

src/FAST_LIO_LOCALIZATION/src/flag.cpp

@@ -20,7 +20,7 @@ int main(int argc, char** argv){
     tf::StampedTransform transform_listener;
     
     try{
-      listener.lookupTransform("map", "body",  
+      listener.lookupTransform("robot_foot_init", "body",  
                                ros::Time(0), transform_listener);
     }
     catch (tf::TransformException ex){
@@ -44,6 +44,9 @@ 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,24 +138,6 @@ 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/FAST_LIO_LOCALIZATION/src/send.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,45 +131,9 @@ 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);
-        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);
+        write3float(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_;
@@ -182,9 +146,7 @@ void continueMoving() {
     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/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/ws_livox/src/FAST_LIO_LOCALIZATION/PCD/" />
+<param name="file_directory" value= "/home/cmrt/mid360/ws_livox/src/FAST_LIO/PCD/" />
 <!-- pcd文件名称-->
 <param name="file_name" value= "scans" />
 <!-- 选取的范围　最小的高度-->