#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/RegionOfInterest.h>
#include <sensor_msgs/LaserScan.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <std_msgs/String.h>
#include <std_srvs/Empty.h>
#include <vector>
#include <cmath>
#include <tf2/LinearMath/Matrix3x3.h>
#include <deque>
#include <nav_msgs/Odometry.h> // 添加Odometry消息
nav_msgs::OccupancyGrid map_msg;
cv::Mat map_cropped;
cv::Mat map_temp;
cv::Mat map_match;
sensor_msgs::RegionOfInterest map_roi_info;
std::vector<cv::Point2f> scan_points;
std::vector<cv::Point2f> best_transform;
ros::ServiceClient clear_costmaps_client;
std::string base_frame;
std::string odom_frame;
std::string laser_frame;
std::string laser_topic;
float lidar_x = 250, lidar_y = 250, lidar_yaw = 0;
float deg_to_rad = M_PI / 180.0;
int cur_sum = 0;
int clear_countdown = -1;
int scan_count = 0;
// 全局：保存上一次滤波后的状态
double s_x = 0.0, s_y = 0.0, s_yaw = 0.0;
bool s_init = false;
double alpha; // 在 main 里通过 param 读取
// 添加全局发布器
ros::Publisher robot_pose_pub; // 用于发布机器人位姿
// 初始姿态回调函数
void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
{
try {
static tf2_ros::Buffer tfBuffer;
static tf2_ros::TransformListener tfListener(tfBuffer);
// 1. 查询从 base_frame 到 laser_frame 的转换
geometry_msgs::TransformStamped transformStamped =
tfBuffer.lookupTransform(base_frame, laser_frame, ros::Time(0), ros::Duration(1.0));
// 2. 创建一个 stamped pose 用于转换
geometry_msgs::PoseStamped base_pose, laser_pose;
base_pose.header = msg->header;
base_pose.pose = msg->pose.pose;
// 3. 将 base_frame 的位姿转换为 laser_frame 的位姿
tf2::doTransform(base_pose, laser_pose, transformStamped);
// 4. 从转换后的消息中提取位置和方向信息
double x = msg->pose.pose.position.x;
double y = msg->pose.pose.position.y;
tf2::Quaternion q(
laser_pose.pose.orientation.x,
laser_pose.pose.orientation.y,
laser_pose.pose.orientation.z,
laser_pose.pose.orientation.w);
// 5. 将四元数转换为yaw角度
tf2::Matrix3x3 m(q);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);
// 6. 将地图坐标转换为裁切后的地图坐标
lidar_x = (x - map_msg.info.origin.position.x) / map_msg.info.resolution - map_roi_info.x_offset;
lidar_y = (y - map_msg.info.origin.position.y) / map_msg.info.resolution - map_roi_info.y_offset;
// 7. 设置yaw角度
lidar_yaw = -yaw;
clear_countdown = 30;
}
catch (tf2::TransformException &ex)
{
ROS_WARN("无法获取从 %s 到 %s 的转换: %s", base_frame.c_str(), laser_frame.c_str(), ex.what());
}
}
void crop_map();
void processMap();
void mapCallback(const nav_msgs::OccupancyGrid::ConstPtr& msg)
{
map_msg = *msg;
crop_map();
processMap();
}
void crop_map()
{
// 显示地图信息
std_msgs::Header header = map_msg.header;
nav_msgs::MapMetaData info = map_msg.info;
//用来统计地图有效区域的变量
int xMax,xMin,yMax,yMin ;
xMax=xMin= info.width/2;
yMax=yMin= info.height/2;
bool bFirstPoint = true;
// 把地图数据转换成图片
cv::Mat map_raw(info.height, info.width, CV_8UC1, cv::Scalar(128)); // 灰色背景
for(int y = 0; y < info.height; y++)
{
for(int x = 0; x < info.width; x++)
{
int index = y * info.width + x;
// 直接使用map_msg.data中的值
map_raw.at<uchar>(y, x) = static_cast<uchar>(map_msg.data[index]);
// 统计有效区域
if(map_msg.data[index] == 100)
{
if(bFirstPoint)
{
xMax = xMin = x;
yMax = yMin = y;
bFirstPoint = false;
continue;
}
xMin = std::min(xMin, x);
xMax = std::max(xMax, x);
yMin = std::min(yMin, y);
yMax = std::max(yMax, y);
}
}
}
// 计算有效区域的中心点坐标
int cen_x = (xMin + xMax)/2;
int cen_y = (yMin + yMax)/2;
// 按照有效区域对地图进行裁剪
int new_half_width = abs(xMax - xMin)/2 + 50;
int new_half_height = abs(yMax - yMin)/2 + 50;
int new_origin_x = cen_x - new_half_width;
int new_origin_y = cen_y - new_half_height;
int new_width = new_half_width*2;
int new_height = new_half_height*2;
if(new_origin_x < 0) new_origin_x = 0;
if((new_origin_x + new_width) > info.width) new_width = info.width - new_origin_x;
if(new_origin_y < 0) new_origin_y = 0;
if((new_origin_y + new_height) > info.height) new_height = info.height - new_origin_y;
cv::Rect roi(new_origin_x, new_origin_y, new_width, new_height);
cv::Mat roi_map = map_raw(roi).clone();
map_cropped = roi_map;
// 地图的裁减信息
map_roi_info.x_offset = new_origin_x;
map_roi_info.y_offset = new_origin_y;
map_roi_info.width = new_width;
map_roi_info.height = new_height;
geometry_msgs::PoseWithCovarianceStamped init_pose;
init_pose.pose.pose.position.x = 0.0;
init_pose.pose.pose.position.y = 0.0;
init_pose.pose.pose.position.y = 0.0;
init_pose.pose.pose.orientation.x = 0.0;
init_pose.pose.pose.orientation.y = 0.0;
init_pose.pose.pose.orientation.z = 0.0;
init_pose.pose.pose.orientation.w = 1.0;
geometry_msgs::PoseWithCovarianceStamped::ConstPtr init_pose_ptr(new geometry_msgs::PoseWithCovarianceStamped(init_pose));
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;
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;
scan_points.push_back(cv::Point2f(x, y));
}
angle += msg->angle_increment;
}
if(scan_count == 0)
scan_count ++;
while (ros::ok())
{
if (!map_cropped.empty())
{
// 计算三种情况下的雷达点坐标数组
std::vector<cv::Point2f> transform_points, clockwise_points, counter_points;
int max_sum = 0;
float best_dx = 0, best_dy = 0, best_dyaw = 0;
for (const auto& point : scan_points)
{
// 情况一：原始角度
float rotated_x = point.x * cos(lidar_yaw) - point.y * sin(lidar_yaw);
float rotated_y = point.x * sin(lidar_yaw) + point.y * cos(lidar_yaw);
transform_points.push_back(cv::Point2f(rotated_x + lidar_x, lidar_y - rotated_y));
// 情况二：顺时针旋转1度
float clockwise_yaw = lidar_yaw + deg_to_rad;
rotated_x = point.x * cos(clockwise_yaw) - point.y * sin(clockwise_yaw);
rotated_y = point.x * sin(clockwise_yaw) + point.y * cos(clockwise_yaw);
clockwise_points.push_back(cv::Point2f(rotated_x + lidar_x, lidar_y - rotated_y));
// 情况三：逆时针旋转1度
float counter_yaw = lidar_yaw - deg_to_rad;
rotated_x = point.x * cos(counter_yaw) - point.y * sin(counter_yaw);
rotated_y = point.x * sin(counter_yaw) + point.y * cos(counter_yaw);
counter_points.push_back(cv::Point2f(rotated_x + lidar_x, lidar_y - rotated_y));
}
// 计算15种变换方式的匹配值
std::vector<cv::Point2f> offsets = {{0,0}, {1,0}, {-1,0}, {0,1}, {0,-1}};
std::vector<std::vector<cv::Point2f>> point_sets = {transform_points, clockwise_points, counter_points};
std::vector<float> yaw_offsets = {0, deg_to_rad, -deg_to_rad};
for (int i = 0; i < offsets.size(); ++i)
{
for (int j = 0; j < point_sets.size(); ++j)
{
int sum = 0;
for (const auto& point : point_sets[j])
{
float px = point.x + offsets[i].x;
float py = point.y + offsets[i].y;
if (px >= 0 && px < map_temp.cols && py >= 0 && py < map_temp.rows)
{
sum += map_temp.at<uchar>(py, px);
}
}
if (sum > max_sum)
{
max_sum = sum;
best_dx = offsets[i].x;
best_dy = offsets[i].y;
best_dyaw = yaw_offsets[j];
}
}
}
// 更新雷达位置和角度
lidar_x += best_dx;
lidar_y += best_dy;
lidar_yaw += best_dyaw;
// 判断匹配循环是否可以终止
if(check(lidar_x , lidar_y , lidar_yaw))
{
break;
}
}
}
if(clear_countdown > -1)
clear_countdown --;
if(clear_countdown == 0)
{
std_srvs::Empty srv;
clear_costmaps_client.call(srv);
}
}
std::deque<std::tuple<float, float, float>> data_queue;
const size_t max_size = 10;
bool check(float x, float y, float yaw)
{
if(x == 0 && y == 0 && yaw == 0)
{
data_queue.clear();
return true;
}
// 添加新数据
data_queue.push_back(std::make_tuple(x, y, yaw));
// 如果队列超过最大大小，移除最旧的数据
if (data_queue.size() > max_size)
{
data_queue.pop_front();
}
// 如果队列已满，检查第一个和最后一个元素
if (data_queue.size() == max_size)
{
auto& first = data_queue.front();
auto& last = data_queue.back();
float dx = std::abs(std::get<0>(last) - std::get<0>(first));
float dy = std::abs(std::get<1>(last) - std::get<1>(first));
float dyaw = std::abs(std::get<2>(last) - std::get<2>(first));
// 如果所有差值的绝对值都小于5，清空队列退出循环
if (dx < 5 && dy < 5 && dyaw < 5*deg_to_rad)
{
data_queue.clear();
return true;
}
}
return false;
}
cv::Mat createGradientMask(int size)
{
cv::Mat mask(size, size, CV_8UC1);
int center = size / 2;
for (int y = 0; y < size; y++)
{
for (int x = 0; x < size; x++)
{
double distance = std::hypot(x - center, y - center);
int value = cv::saturate_cast<uchar>(255 * std::max(0.0, 1.0 - distance / center));
mask.at<uchar>(y, x) = value;
}
}
return mask;
}
void processMap()
{
if (map_cropped.empty()) return;
map_temp = cv::Mat::zeros(map_cropped.size(), CV_8UC1);
cv::Mat gradient_mask = createGradientMask(101); // 创建一个101x101的渐变掩模
for (int y = 0; y < map_cropped.rows; y++)
{
for (int x = 0; x < map_cropped.cols; x++)
{
if (map_cropped.at<uchar>(y, x) == 100) // 障碍物
{
int left = std::max(0, x - 50);
int top = std::max(0, y - 50);
int right = std::min(map_cropped.cols - 1, x + 50);
int bottom = std::min(map_cropped.rows - 1, y + 50);
cv::Rect roi(left, top, right - left + 1, bottom - top + 1);
cv::Mat region = map_temp(roi);
int mask_left = 50 - (x - left);
int mask_top = 50 - (y - top);
cv::Rect mask_roi(mask_left, mask_top, roi.width, roi.height);
cv::Mat mask = gradient_mask(mask_roi);
cv::max(region, mask, region);
}
}
}
}
void pose_tf()
{
if (scan_count == 0) return;
if (map_cropped.empty() || map_msg.data.empty()) return;
// 一阶低通滤波状态
static bool s_init = false;
static double s_x = 0.0, s_y = 0.0, s_yaw = 0.0;
static tf2_ros::Buffer tfBuffer;
static tf2_ros::TransformListener tfListener(tfBuffer);
// 1. 计算原始位姿
double raw_x = (lidar_x + map_roi_info.x_offset) * map_msg.info.resolution
+ map_msg.info.origin.position.x;
double raw_y = (lidar_y + map_roi_info.y_offset) * map_msg.info.resolution
+ map_msg.info.origin.position.y;
double raw_yaw = -lidar_yaw;
// 2. 低通滤波
if (!s_init) {
s_x = raw_x; s_y = raw_y; s_yaw = raw_yaw;
s_init = true;
} else {
double dy = raw_yaw - s_yaw;
while (dy > M_PI) dy -= 2.0 * M_PI;
while (dy < -M_PI) dy += 2.0 * M_PI;
s_x = alpha * raw_x + (1.0 - alpha) * s_x;
s_y = alpha * raw_y + (1.0 - alpha) * s_y;
s_yaw = s_yaw + alpha * dy;
}
// 3. 构造 map -> base_link 变换
tf2::Transform map_to_base;
tf2::Quaternion q; // ← 把 q 提到此处，使后续都能访问
q.setRPY(0, 0, s_yaw);
map_to_base.setOrigin(tf2::Vector3(s_x, s_y, 0.0));
map_to_base.setRotation(q);
// 发布 map -> base_link
geometry_msgs::TransformStamped map_to_base_msg;
map_to_base_msg.header.stamp = ros::Time::now();
map_to_base_msg.header.frame_id = "map";
map_to_base_msg.child_frame_id = base_frame;
map_to_base_msg.transform.translation.x = s_x;
map_to_base_msg.transform.translation.y = s_y;
map_to_base_msg.transform.translation.z = 0.0;
map_to_base_msg.transform.rotation = tf2::toMsg(q); // 现在 q 可见
static tf2_ros::TransformBroadcaster br;
br.sendTransform(map_to_base_msg);
// 发布机器人里程计
nav_msgs::Odometry robot_odom;
robot_odom.header.stamp = ros::Time::now();
robot_odom.header.frame_id = "map";
robot_odom.child_frame_id = base_frame;
robot_odom.pose.pose.position.x = s_x;
robot_odom.pose.pose.position.y = s_y;
robot_odom.pose.pose.position.z = 0.0;
robot_odom.pose.pose.orientation = tf2::toMsg(q);
robot_pose_pub.publish(robot_odom);
// 7. 查询 odom -> laser_frame
geometry_msgs::TransformStamped odom_to_laser;
try {
odom_to_laser = tfBuffer.lookupTransform(odom_frame, laser_frame, ros::Time(0));
}
catch (tf2::TransformException &ex) {
ROS_WARN("%s", ex.what());
return;
}
// 8. 计算 map -> odom
tf2::Transform odom_to_laser_tf2, map_to_laser;
tf2::fromMsg(odom_to_laser.transform, odom_to_laser_tf2);
// 注意此处用 s_y 而不是 x_y
map_to_laser.setOrigin(tf2::Vector3(s_x, s_y, 0.0));
map_to_laser.setRotation(q);
tf2::Transform map_to_odom = map_to_laser * odom_to_laser_tf2.inverse();
// 9. 发布 map -> odom
geometry_msgs::TransformStamped map_to_odom_msg;
map_to_odom_msg.header.stamp = ros::Time::now();
map_to_odom_msg.header.frame_id = "map";
map_to_odom_msg.child_frame_id = odom_frame;
map_to_odom_msg.transform = tf2::toMsg(map_to_odom);
br.sendTransform(map_to_odom_msg);
}
int main(int argc, char** argv)
{
setlocale(LC_ALL,"");
ros::init(argc, argv, "lidar_loc");
// 读取参数
ros::NodeHandle private_nh("~");
private_nh.param<std::string>("base_frame", base_frame, "base_footprint");
private_nh.param<std::string>("odom_frame", odom_frame, "odom");
private_nh.param<std::string>("laser_frame", laser_frame, "scan");
private_nh.param<std::string>("laser_topic", laser_topic, "base_scan");
private_nh.param("filter_alpha", alpha, 0.1);
ros::NodeHandle nh;
ros::Duration(1).sleep();
// 初始化发布器 - 用于机器人位姿
robot_pose_pub = nh.advertise<nav_msgs::Odometry>("lidar_odom", 10);
ros::Subscriber map_sub = nh.subscribe("map", 1, mapCallback);
ros::Subscriber scan_sub = nh.subscribe(laser_topic, 1, scanCallback);
ros::Subscriber initial_pose_sub = nh.subscribe("initialpose", 1, initialPoseCallback);
clear_costmaps_client = nh.serviceClient<std_srvs::Empty>("move_base/clear_costmaps");
ros::Rate rate(30); // tf发送频率
while (ros::ok())
{
pose_tf();
ros::spinOnce();
rate.sleep();
}
return 0;
}为什么我使用这个激光定位算法并且加入滤波后，机器人轨迹还是会有跳变或者起伏，请你帮我全面分析一下
文章描述了在ros环境中，使用rviz和map_server时遇到的地图位置偏移问题。问题根源在于.yaml文件中origin参数不正确。通过调整origin参数至[-7.00000,-11.00000,0.000000]，并重启rviz，成功解决了地图偏移的问题。此外，建议在rviz中使用Axes和2DPoseEstimate工具辅助校准。
扫一扫
3234
到【灌水乐园】发言
