JYC/25RC_MID360_R2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

2

Browse Source
This commit is contained in:
hara 2024-11-28 20:22:00 +08:00
parent 8a804699bd
commit 0a5fb0df3f
12 changed files with 40 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/FAST_LIO_LOCALIZATION/PCD/M3.yaml
View File

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

7
src/FAST_LIO_LOCALIZATION/PCD/M4.yaml
View File

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

BIN
src/FAST_LIO_LOCALIZATION/PCD/m4.pgm
View File

Binary file not shown.

7
src/FAST_LIO_LOCALIZATION/PCD/m4.yaml
View File

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

BIN
src/FAST_LIO_LOCALIZATION/PCD/M4.pgm → src/FAST_LIO_LOCALIZATION/PCD/map1.pgm
View File

Binary file not shown.

7
src/FAST_LIO_LOCALIZATION/PCD/map1.yaml Normal file
View File

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

7
src/FAST_LIO_LOCALIZATION/launch/localization_MID360.launch
View File

@ -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/M4.yaml /map:=prior_map"/>
>>>>>>> 710b3fc3ab857cb452f1e5c38d1599436191b8de
<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">

5
src/FAST_LIO_LOCALIZATION/launch/move.launch Normal file
View File

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

4
src/FAST_LIO_LOCALIZATION/launch/nav.launch
View File

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

43
src/FAST_LIO_LOCALIZATION/src/controlpid.cpp
View File

@ -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) {
static const float min_distance = 0.45; // 最小安全距离，单位：米
bool obstacle_detected = false;
// 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;
}
}
// 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();
}
}
// if (obstacle_detected) {
// // 处理避障逻辑
// ROS_WARN("stop！！！");
// stopRobot();
// } else {
// // 继续正常运动
// continueMoving();
// }
// }
void stopRobot() {
geometry_msgs::Twist stop_cmd;
stop_cmd.linear.x = 0;

4
src/FAST_LIO_LOCALIZATION/src/mbot_linux_serial.cpp
View File

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

2
src/pcd2pgm/launch/run.launch
View File

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