ROS_Robot/jie_ware
1
0
Fork 0
mirror of https://github.com/6-robot/jie_ware.git synced 2025-09-15 12:59:05 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Compare commits

base: ROS_Robot:35e827a6de318564c1f9883190044871f4d83a14
Branches Tags
ROS_Robot:main
...
compare: ROS_Robot:f777dccb2a33f28803dc62dd1284042cd09ceee9
Branches Tags
ROS_Robot:main

2 Commits
35e827a6de ... f777dccb2a

Author SHA1 Message Date
Robot
f777dccb2a new video link 2025-06-12 23:45:11 +08:00
Robot
60f7457c18 fix: Resolve issue in transform 2025-06-12 23:39:24 +08:00
2 changed files with 40 additions and 5 deletions
Show Stats Expand all files Collapse all files

2
README.md
View File

@ -6,6 +6,8 @@ Bilibili: [《一种简单易用的激光雷达定位方法》](https://www.bili
Youtube: [《一种简单易用的激光雷达定位方法》](https://www.youtube.com/watch?v=0JqGX8lKRu0)
Bilibili: [《代价地图清除》](https://www.bilibili.com/video/BV1kwzqYyEe7/)
Youtube: [《代价地图清除》](https://www.youtube.com/watch?v=giHf_PY4EmY)
Bilibili: [《去除ROS导航中的激光雷达噪点》](https://www.bilibili.com/video/BV1LFjBzREQu)
Youtube: [《去除ROS导航中的激光雷达噪点》](https://www.youtube.com/watch?v=98GF6_zN_IA)
基础：[《ROS 快速入门教材》](https://www.bilibili.com/video/BV1BP4y1o7pw/)
扩展：[《ROS 导航，除了 DWA 和 TEB 还有没有其他选择？》](https://www.bilibili.com/video/BV1nQR4YsESM/)

43
src/lidar_loc.cpp
View File

@ -160,17 +160,50 @@ void crop_map()
initialPoseCallback(init_pose_ptr);
}
bool check(float x, float y, float yaw);
void scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
scan_points.clear();
double angle = msg->angle_min;
static tf2_ros::Buffer tfBuffer;
static tf2_ros::TransformListener tfListener(tfBuffer);
// 从激光雷达到底盘的转换
geometry_msgs::TransformStamped transformStamped;
try {
transformStamped = tfBuffer.lookupTransform(base_frame, laser_frame,ros::Time(0));
}
catch (tf2::TransformException &ex) {
ROS_WARN("%s", ex.what());
return;
}
for (size_t i = 0; i < msg->ranges.size(); ++i)
{
if (msg->ranges[i] >= msg->range_min && msg->ranges[i] <= msg->range_max)
{
float x = msg->ranges[i] * cos(angle) / map_msg.info.resolution;
float y = -msg->ranges[i] * sin(angle) / map_msg.info.resolution;
// 1. 首先在激光雷达坐标系下计算点的坐标
float x_laser = msg->ranges[i] * cos(angle);
float y_laser = -msg->ranges[i] * sin(angle);
// 2. 创建要转换的点
geometry_msgs::PointStamped point_laser;
point_laser.header.frame_id = laser_frame;
point_laser.header.stamp = msg->header.stamp;
point_laser.point.x = x_laser;
point_laser.point.y = y_laser;
point_laser.point.z = 0.0;
// 3. 转换到底盘坐标系
geometry_msgs::PointStamped point_base;
tf2::doTransform(point_laser, point_base, transformStamped);
// 4. 转换为栅格地图坐标并存储
float x = point_base.point.x / map_msg.info.resolution;
float y = point_base.point.y / map_msg.info.resolution;
scan_points.push_back(cv::Point2f(x, y));
}
angle += msg->angle_increment;
@ -375,13 +408,13 @@ void pose_tf()
q.setRPY(0, 0, yaw_ros);
// 5. 计算 base_footprint 在 map 中的位置
double base_x = x_meters;
double base_y = y_meters;
double base_x = -x_meters;
double base_y = -y_meters;
// 6. 查询 odom 到 base_frame 的变换
geometry_msgs::TransformStamped odom_to_base;
try {
odom_to_base = tfBuffer.lookupTransform(odom_frame, laser_frame, ros::Time(0));
odom_to_base = tfBuffer.lookupTransform(odom_frame, base_frame, ros::Time(0));
}
catch (tf2::TransformException &ex) {
ROS_WARN("%s", ex.what());