Open imuncle opened 4 years ago
因为商用激光雷达频率多在10~15Hz之间,当机器人快速运动时可能会出现激光雷达数据变形的情况,这会在一定程度上影响机器人的定位和路径规划。因此对激光雷达数据的纠正很关键。
我参考了激光雷达移动状态下的数据矫正这篇文章,其中基本上是一篇论文的中文翻译,论文也在这篇文章中给出了。
我根据论文中的内容使用ROS和OpenCV搭建了一个仿真环境,在这个仿真程序中,雷达数据为10Hz,里程计数据为100Hz,使用键盘控制机器人的前后左右旋转。雷达一圈采集360个点。纠正的大体思路如下:
因为激光雷达数据频率较低,但每一帧中有360个数据,所以激光雷达的每一个数据时间间隔是小于里程计的。首先我们需要构建一个队列存储最近20帧里程计数据,然后在激光雷达数据回调函数中,针对每一个数据的时间戳找到对应的里程计近似数据,最后将这些里程计位姿对齐到最后一帧里程计数据(因为原始数据中只有最后一个雷达数据是准确的。)
程序有四个文件,直接上代码,关键部分有注释:
// 作者:谢胜 // 功能:读取键盘输入(无需回车),可用于模拟程序中机器人控制的输入信号
class Keyboard { public: Keyboard() { kfd = 0; tcgetattr(kfd, &cooked); memcpy(&raw, &cooked, sizeof(struct termios)); raw.c_lflag &=~ (ICANON | ECHO); raw.c_cc[VEOL] = 1; raw.c_cc[VEOF] = 2; tcsetattr(kfd, TCSANOW, &raw); ufd.fd = kfd; ufd.events = POLLIN; } ~Keyboard() { tcsetattr(kfd, TCSANOW, &cooked); } uint8_t Read() { int num; uint8_t c; if((num = poll(&ufd, 1, 250)) < 0) { perror("poll():"); return 0; } else if(num > 0) { if(read(kfd, &c, 1) < 0) { perror("read():"); return 0; } } else { return 0; } return c; } private: struct pollfd ufd; struct termios raw; struct termios cooked; int kfd; };
* car.h ```cpp #ifndef CAR_H #define CAR_H #include "keyboard.h" #include <ros/ros.h> #include <thread> #include "common_msgs/CarPose.h" // 存储机器人位置的类(其实没怎么用它,基本多余) class CarPosition { public: CarPosition(){} CarPosition(float x, float y, float orientation) { x_ = x; y_ = y; orientation_ = orientation; } float x_; float y_; float orientation_; }; // 机器人控制类,实现了键盘控制机器人移动,并以100帧的频率发布里程计数据 class Car { public: Car(float x, float y, float orientation, int width, int height) { car_pose_pub_ = nh.advertise<common_msgs::CarPose>("car_pose", 1); position_.x_ = x; position_.y_ = y; position_.orientation_ = orientation; width_ = width; height_ = height; control_thread_ = std::thread(&Car::CanControl, this); keyboard_thread_ = std::thread(&Car::KeyboardRead, this); } ~Car() { if(control_thread_.joinable()) { control_thread_.join(); } if(keyboard_thread_.joinable()) { keyboard_thread_.join(); } } void RunForwardBack(float velocity) { position_.x_ += velocity * std::cos(position_.orientation_); position_.y_ += velocity * std::sin(position_.orientation_); } void RunLeftRight(float velocity) { position_.x_ -= velocity * std::sin(position_.orientation_); position_.y_ += velocity * std::cos(position_.orientation_); } void Rotate(float angular) { position_.orientation_ += angular; if(position_.orientation_ < 0) { position_.orientation_ += 2*M_PI; } if(position_.orientation_ > 2*M_PI) { position_.orientation_ -= 2*M_PI; } } void CanControl() { ros::Rate rate(100); while(ros::ok()) { car_pose_.stamp = ros::Time::now(); car_pose_.x = GetX(); car_pose_.y = GetY(); car_pose_.orientation = GetOrientation(); car_pose_pub_.publish(car_pose_); rate.sleep(); } } void KeyboardRead() { while(ros::ok()) { int key = keyboard.Read(); if(key == 'w') { RunForwardBack(0.1); } else if(key == 's') { RunForwardBack(-0.1); } else if(key == 'a') { RunLeftRight(0.1); } else if(key == 'd') { RunLeftRight(-0.1); } else if(key == 'q') { Rotate(0.05); } else if(key == 'e') { Rotate(-0.05); } } } CarPosition GetPosition() { return position_; } float GetX() { return position_.x_; } float GetY() { return position_.y_; } float GetOrientation() { return position_.orientation_; } private: CarPosition position_; int width_; int height_; Keyboard keyboard; std::thread control_thread_; std::thread keyboard_thread_; ros::NodeHandle nh; ros::Publisher car_pose_pub_; common_msgs::CarPose car_pose_; }; #endif
#include <ros/ros.h> #include "common_msgs/CarPose.h" #include "sensor_msgs/LaserScan.h" #include "car.h" #include <thread>
// 模拟雷达数据发布类 class LaserPub { public: LaserPub(): car(1, 1, 0, 0.6, 0.4), angle_min(0),angle_max(6.28),angle_increment(0.017453),time_increment(0.00028),scan_time(0.1),range_min(0.1),range_max(8), scan_index(0),scan_seq(1), map_width(8),map_height(5) { laser_pub = nh.advertise("laser_scan", 1); laser_scandata.ranges.resize(360); scanthread = std::thread(&LaserPub::Scan, this); } ~LaserPub() { if(scanthread.joinable()) { scanthread.join(); } } // 单次扫描,传入参数是本次扫描的序号(0~359) float ScanOnce(int index) { CarPosition car_pose = car.GetPosition(); float angle = carpose.orientation - angle_increment index; if(angle < 0) angle += 2M_PI; if(angle <= M_PI) { float x = (map_height - carpose.y) / std::tan(angle) + carpose.x; if(x >= 0 && x <= map_width) { return std::sqrt((x - carpose.x) (x - carpose.x) + (map_height - carpose.y) (map_height - carpose.y)); } else if(x < 0) { float y = (0 - carpose.x) std::tan(angle) + carpose.y; return std::sqrt((0 - carpose.x) (0 - carpose.x) + (y - carpose.y) (y - carpose.y)); } else if(x > map_width) { float y = (map_width - carpose.x) std::tan(angle) + carpose.y; return std::sqrt((map_width - carpose.x) (map_width - carpose.x) + (y - carpose.y) (y - carpose.y)); } } else { float x = (0 - carpose.y) / std::tan(angle) + carpose.x; if(x >= 0 && x <= map_width) { return std::sqrt((x - carpose.x) (x - carpose.x) + (0 - carpose.y) (0 - carpose.y)); } else if(x < 0) { float y = (0 - carpose.x) std::tan(angle) + carpose.y; return std::sqrt((0 - carpose.x) (0 - carpose.x) + (y - carpose.y) (y - carpose.y)); } else if(x > map_width) { float y = (map_width - carpose.x) std::tan(angle) + carpose.y; return std::sqrt((map_width - carpose.x) (map_width - carpose.x) + (y - carpose.y) (y - carpose.y)); } } } // 模拟雷达数据发布 void LaserDataPublish() { laser_scandata.angle_min = angle_min; laser_scandata.angle_max = angle_max; laser_scandata.angle_increment = angle_increment; laser_scandata.time_increment = time_increment; laser_scandata.scan_time = scan_time; laser_scandata.range_min = range_min; laser_scandata.range_max = range_max; laser_pub.publish(laser_scandata); } // 雷达扫描线程,频率10Hz void Scan() { ros::Rate rate(3600); while(ros::ok()) { if(scan_index == 0) { laser_scandata.header.seq = scan_seq; laser_scandata.header.stamp = ros::Time::now(); laser_scandata.header.frame_id = "map"; } laser_scandata.ranges[scan_index] = ScanOnce(scan_index); scan_index++; if(scan_index == 360) { LaserDataPublish(); scan_index = 0; } rate.sleep(); } } int scan_index; int scan_seq; private: ros::NodeHandle nh; ros::Subscriber car_pose_sub; ros::Publisher laser_pub; sensor_msgs::LaserScan laser_scandata; Car car; std::thread scanthread;
double angle_min; double angle_max; double angle_increment; double time_increment; double scan_time; double range_min; double range_max; int map_width; int map_height;
};
int main(int argc, char** argv) { ros::init(argc, argv, "laser_pub_node"); LaserPub laser_pub; ros::AsyncSpinner async_spinner(3); async_spinner.start(); ros::waitForShutdown(); return 0; }
* laser_correct.cpp ```cpp #include <ros/ros.h> #include <opencv2/opencv.hpp> #include "common_msgs/CarPose.h" #include "sensor_msgs/LaserScan.h" #include <Eigen/Eigen> #include <Eigen/Dense> #include <Eigen/Geometry> #include <Eigen/Eigenvalues> // 使用opencv将机器人位姿显示出来 class CarDraw { public: CarDraw() { width_ = 60; height_ = 40; vertex_.resize(4); } void Draw(cv::Mat &img) { cv::line(img, vertex_[0], vertex_[1], cv::Scalar(0,0,255), 2); cv::line(img, vertex_[1], vertex_[2], cv::Scalar(0,0,255), 2); cv::line(img, vertex_[2], vertex_[3], cv::Scalar(0,0,255), 2); cv::line(img, vertex_[3], vertex_[0], cv::Scalar(0,0,255), 2); } // 手写了坐标变换,可用Eigen库替换 void Rotate(float angle) { float cos_theta = std::cos(angle); float sin_theta = std::sin(angle); cv::Point2f vertex; vertex.x = width_/2; vertex.y = height_/2; vertex_[0].x = cos_theta * vertex.x - sin_theta * vertex.y; vertex_[0].y = sin_theta * vertex.x + cos_theta * vertex.y; vertex_[0].x += center_.x; vertex_[0].y += center_.y; vertex.x = -width_/2; vertex.y = height_/2; vertex_[1].x = cos_theta * vertex.x - sin_theta * vertex.y; vertex_[1].y = sin_theta * vertex.x + cos_theta * vertex.y; vertex_[1].x += center_.x; vertex_[1].y += center_.y; vertex.x = -width_/2; vertex.y = -height_/2; vertex_[2].x = cos_theta * vertex.x - sin_theta * vertex.y; vertex_[2].y = sin_theta * vertex.x + cos_theta * vertex.y; vertex_[2].x += center_.x; vertex_[2].y += center_.y; vertex.x = width_/2; vertex.y = -height_/2; vertex_[3].x = cos_theta * vertex.x - sin_theta * vertex.y; vertex_[3].y = sin_theta * vertex.x + cos_theta * vertex.y; vertex_[3].x += center_.x; vertex_[3].y += center_.y; vertex_[0].y = 500 - vertex_[0].y; vertex_[1].y = 500 - vertex_[1].y; vertex_[2].y = 500 - vertex_[2].y; vertex_[3].y = 500 - vertex_[3].y; } // 设置机器人中心位置 void SetCenter(cv::Point2f center) { center_ = center; } private: std::vector<cv::Point2f> vertex_; cv::Point2f center_; int width_; int height_; }; // 雷达数据纠正类 class LaserCorrect { public: LaserCorrect() { car_pose_sub_ = nh.subscribe("car_pose", 1, &LaserCorrect::CarPoseCallback, this); laser_scan_sub_ = nh.subscribe("laser_scan", 1, &LaserCorrect::LaserScanCallback, this); } // 里程计数据回调函数,更新机器人位置,并更新里程计缓存队列 void CarPoseCallback(common_msgs::CarPose car_pose) { car_pose_[0] = car_pose.x; car_pose_[1] = car_pose.y; car_pose_[2] = car_pose.orientation; car.SetCenter(cv::Point2f(car_pose.x*100, car_pose.y*100)); car.Rotate(car_pose.orientation); PosePushBuffer(car_pose); } // 雷达数据回调函数,在其中纠正雷达数据 void LaserScanCallback(sensor_msgs::LaserScan laser_scan) { laser_scan_data_ = laser_scan; cv::Mat map(500, 800, CV_8UC3, cv::Scalar(255,255,255)); for(size_t i = 0; i < laser_scan_data_.ranges.size(); i++) { float angle = GetRobotOrientation() - laser_scan_data_.angle_min - i * laser_scan_data_.angle_increment; if(angle < 0) angle += 2*M_PI; float sin_theta = std::sin(angle); float cos_theta = std::cos(angle); float x = laser_scan_data_.ranges[i] * cos_theta + GetRobotX(); float y = laser_scan_data_.ranges[i] * sin_theta + GetRobotY(); cv::circle(map, cv::Point(x*100, 500-y*100), 3, cv::Scalar(0,0,0),-1); } car.Draw(map); if(car_pose_buffer_.size() >= 20) LaserDataCorrect(laser_scan_data_, map); cv::imshow("map", map); cv::waitKey(1); } float GetRobotX() { return car_pose_[0]; } float GetRobotY() { return car_pose_[1]; } float GetRobotOrientation() { return car_pose_[2]; } void PosePushBuffer(common_msgs::CarPose pose) { if(car_pose_buffer_.size() == 20) { car_pose_buffer_.erase(car_pose_buffer_.begin()); } car_pose_buffer_.push_back(pose); } // 雷达数据纠正函数 void LaserDataCorrect(const sensor_msgs::LaserScan& laser_scan_msg, cv::Mat& img) { double laser_start_time_stamp = GetRosTime(laser_scan_msg.header.stamp); double laser_last_time_stamp = laser_start_time_stamp + laser_scan_msg.scan_time; Eigen::Vector3d final_odom_pose; // 获取雷达最后一帧数据对应的里程计位置 FindBasePoseWithLaserPoint(laser_last_time_stamp, final_odom_pose); for(int i = 0; i < laser_scan_msg.ranges.size(); ++i) { double theta_tmp = laser_scan_msg.angle_min + i * laser_scan_msg.angle_increment; double time_tmp = laser_start_time_stamp + i * laser_scan_msg.time_increment; Eigen::Vector3d pose_tmp, pose_tmp_aligned; Eigen::Vector3d base_pose_tmp; pose_tmp(0) = laser_scan_msg.ranges[i] * std::cos(-theta_tmp); pose_tmp(1) = laser_scan_msg.ranges[i] * std::sin(-theta_tmp); pose_tmp(2) = 1; // 找到当前雷达数据对应的里程计位置 FindBasePoseWithLaserPoint(time_tmp, base_pose_tmp); // 根据里程计数据对雷达数据进行纠正 PointCoordinateAlign(pose_tmp, base_pose_tmp, pose_tmp_aligned, final_odom_pose); cv::circle(img, cv::Point(pose_tmp_aligned(0)*100, 500-pose_tmp_aligned(1)*100), 3, cv::Scalar(255,0,0),-1); } } double GetRosTime(ros::Time time) { return (double)time.nsec/(1e9) + time.sec; } // 要注意里程计的角度是0~2*M_PI,注意过零处理和防止超出角度范围 void FindBasePoseWithLaserPoint(double time_stamp, Eigen::Vector3d& base_odom_pose) { int i = 0; for(i = 0; i < car_pose_buffer_.size() - 1; ++i) { if(time_stamp >= GetRosTime(car_pose_buffer_[i].stamp) && time_stamp < GetRosTime(car_pose_buffer_[i + 1].stamp)) break; } if(i == car_pose_buffer_.size() - 1) { Eigen::Vector3d delta_pose; delta_pose(0) = car_pose_buffer_[i].x - car_pose_buffer_[i-1].x; delta_pose(1) = car_pose_buffer_[i].y - car_pose_buffer_[i-1].y; delta_pose(2) = car_pose_buffer_[i].orientation - car_pose_buffer_[i-1].orientation; if(delta_pose(2) > M_PI) { delta_pose(2) -= 2*M_PI; } if(delta_pose(2) < -M_PI) { delta_pose(2) += 2*M_PI; } double delta_time = GetRosTime(car_pose_buffer_[i].stamp) - GetRosTime(car_pose_buffer_[i-1].stamp); double incre_time = time_stamp - GetRosTime(car_pose_buffer_[i].stamp); base_odom_pose(0) = car_pose_buffer_[i].x + delta_pose(0) * incre_time/delta_time; base_odom_pose(1) = car_pose_buffer_[i].y + delta_pose(1) * incre_time/delta_time; base_odom_pose(2) = car_pose_buffer_[i].orientation + delta_pose(2) * incre_time/delta_time; } else { Eigen::Vector3d delta_pose; delta_pose(0) = car_pose_buffer_[i+1].x - car_pose_buffer_[i].x; delta_pose(1) = car_pose_buffer_[i+1].y - car_pose_buffer_[i].y; delta_pose(2) = car_pose_buffer_[i+1].orientation - car_pose_buffer_[i].orientation; if(delta_pose(2) > M_PI) { delta_pose(2) -= 2*M_PI; } if(delta_pose(2) < -M_PI) { delta_pose(2) += 2*M_PI; } double delta_time = GetRosTime(car_pose_buffer_[i+1].stamp) - GetRosTime(car_pose_buffer_[i].stamp); double incre_time = time_stamp - GetRosTime(car_pose_buffer_[i].stamp); base_odom_pose(0) = car_pose_buffer_[i].x + delta_pose(0) * incre_time/delta_time; base_odom_pose(1) = car_pose_buffer_[i].y + delta_pose(1) * incre_time/delta_time; base_odom_pose(2) = car_pose_buffer_[i].orientation + delta_pose(2) * incre_time/delta_time; } if(base_odom_pose(2) > 2*M_PI) base_odom_pose(2) -= 2*M_PI; if(base_odom_pose(2) < 0) base_odom_pose(2) += 2*M_PI; } // 将雷达数据对齐至雷达最后一帧 void PointCoordinateAlign(Eigen::Vector3d& point_pose, Eigen::Vector3d& base_pose, Eigen::Vector3d& point_pose_aligned, Eigen::Vector3d& base_pose_aligned) { Eigen::Matrix3d tf_pose_to_pose_aligned; tf_pose_to_pose_aligned.setZero(); Eigen::Matrix2d tf_base_to_pose; tf_base_to_pose.setZero(); double delta_theta = base_pose_aligned(2); tf_base_to_pose(0,0) = std::cos(-delta_theta); tf_base_to_pose(0,1) = -std::sin(-delta_theta); tf_base_to_pose(1,0) = std::sin(-delta_theta); tf_base_to_pose(1,1) = std::cos(-delta_theta); Eigen::Vector2d delta_pose; delta_pose(0) = base_pose(0) - base_pose_aligned(0); delta_pose(1) = base_pose(1) - base_pose_aligned(1); // 将里程计位移变换到雷达坐标系 delta_pose = tf_base_to_pose * delta_pose; delta_theta = base_pose(2) - base_pose_aligned(2); if(delta_theta > M_PI) { delta_theta -= 2*M_PI; } if(delta_theta < -M_PI) { delta_theta += 2*M_PI; } // 两帧雷达数据之间雷达坐标系的变换矩阵 tf_pose_to_pose_aligned(0,0) = std::cos(delta_theta); tf_pose_to_pose_aligned(0,1) = -std::sin(delta_theta); tf_pose_to_pose_aligned(1,0) = std::sin(delta_theta); tf_pose_to_pose_aligned(1,1) = std::cos(delta_theta); tf_pose_to_pose_aligned(0,2) = delta_pose(0); tf_pose_to_pose_aligned(1,2) = delta_pose(1); tf_pose_to_pose_aligned(2,2) = 1; Eigen::Matrix3d tf_pose_to_base; tf_pose_to_base.setZero(); delta_theta = base_pose_aligned(2); tf_pose_to_base(0,0) = std::cos(delta_theta); tf_pose_to_base(0,1) = -std::sin(delta_theta); tf_pose_to_base(1,0) = std::sin(delta_theta); tf_pose_to_base(1,1) = std::cos(delta_theta); tf_pose_to_base(0,2) = base_pose_aligned(0); tf_pose_to_base(1,2) = base_pose_aligned(1); tf_pose_to_base(2,2) = 1; // 求得纠正后的在世界坐标系下的雷达数据 point_pose_aligned = tf_pose_to_base * tf_pose_to_pose_aligned * point_pose; } private: ros::NodeHandle nh; ros::Subscriber car_pose_sub_; ros::Subscriber laser_scan_sub_; float car_pose_[3]; std::vector<common_msgs::CarPose> car_pose_buffer_; Eigen::Matrix3d tf_laser_to_base_; CarDraw car; sensor_msgs::LaserScan laser_scan_data_; }; int main(int argc, char** argv) { ros::init(argc, argv, "laser_correct_node"); LaserCorrect laser_correct; ros::spin(); return 0; }
程序中使用到了一个自定义的msg:
time stamp float64 x float64 y float64 orientation
纠正效果:
其中黑色是原始数据,蓝色是纠正后的数据,可以看到纠正之后基本与场地实际边界重合,达到纠正目的。
因为商用激光雷达频率多在10~15Hz之间,当机器人快速运动时可能会出现激光雷达数据变形的情况,这会在一定程度上影响机器人的定位和路径规划。因此对激光雷达数据的纠正很关键。
我参考了激光雷达移动状态下的数据矫正这篇文章,其中基本上是一篇论文的中文翻译,论文也在这篇文章中给出了。
我根据论文中的内容使用ROS和OpenCV搭建了一个仿真环境,在这个仿真程序中,雷达数据为10Hz,里程计数据为100Hz,使用键盘控制机器人的前后左右旋转。雷达一圈采集360个点。纠正的大体思路如下:
因为激光雷达数据频率较低,但每一帧中有360个数据,所以激光雷达的每一个数据时间间隔是小于里程计的。首先我们需要构建一个队列存储最近20帧里程计数据,然后在激光雷达数据回调函数中,针对每一个数据的时间戳找到对应的里程计近似数据,最后将这些里程计位姿对齐到最后一帧里程计数据(因为原始数据中只有最后一个雷达数据是准确的。)
程序有四个文件,直接上代码,关键部分有注释:
ifndef KEYBOARD_H
define KEYBOARD_H
include
include
include
include
include
include <sys/poll.h>
include <std_msgs/UInt8.h>
class Keyboard { public: Keyboard() { kfd = 0; tcgetattr(kfd, &cooked); memcpy(&raw, &cooked, sizeof(struct termios)); raw.c_lflag &=~ (ICANON | ECHO); raw.c_cc[VEOL] = 1; raw.c_cc[VEOF] = 2; tcsetattr(kfd, TCSANOW, &raw); ufd.fd = kfd; ufd.events = POLLIN; } ~Keyboard() { tcsetattr(kfd, TCSANOW, &cooked); } uint8_t Read() { int num; uint8_t c; if((num = poll(&ufd, 1, 250)) < 0) { perror("poll():"); return 0; } else if(num > 0) { if(read(kfd, &c, 1) < 0) { perror("read():"); return 0; } } else { return 0; } return c; } private: struct pollfd ufd; struct termios raw; struct termios cooked; int kfd; };
endif
// 模拟雷达数据发布类 class LaserPub { public: LaserPub(): car(1, 1, 0, 0.6, 0.4), angle_min(0),angle_max(6.28),angle_increment(0.017453),time_increment(0.00028),scan_time(0.1),range_min(0.1),range_max(8), scan_index(0),scan_seq(1), map_width(8),map_height(5) { laser_pub = nh.advertise("laser_scan", 1);
laser_scandata.ranges.resize(360);
scanthread = std::thread(&LaserPub::Scan, this);
}
~LaserPub()
{
if(scanthread.joinable())
{
scanthread.join();
}
}
// 单次扫描,传入参数是本次扫描的序号(0~359)
float ScanOnce(int index)
{
CarPosition car_pose = car.GetPosition();
float angle = carpose.orientation - angle_increment index;
if(angle < 0)
angle += 2M_PI;
if(angle <= M_PI)
{
float x = (map_height - carpose.y) / std::tan(angle) + carpose.x;
if(x >= 0 && x <= map_width)
{
return std::sqrt((x - carpose.x) (x - carpose.x) + (map_height - carpose.y) (map_height - carpose.y));
}
else if(x < 0)
{
float y = (0 - carpose.x) std::tan(angle) + carpose.y;
return std::sqrt((0 - carpose.x) (0 - carpose.x) + (y - carpose.y) (y - carpose.y));
}
else if(x > map_width)
{
float y = (map_width - carpose.x) std::tan(angle) + carpose.y;
return std::sqrt((map_width - carpose.x) (map_width - carpose.x) + (y - carpose.y) (y - carpose.y));
}
}
else
{
float x = (0 - carpose.y) / std::tan(angle) + carpose.x;
if(x >= 0 && x <= map_width)
{
return std::sqrt((x - carpose.x) (x - carpose.x) + (0 - carpose.y) (0 - carpose.y));
}
else if(x < 0)
{
float y = (0 - carpose.x) std::tan(angle) + carpose.y;
return std::sqrt((0 - carpose.x) (0 - carpose.x) + (y - carpose.y) (y - carpose.y));
}
else if(x > map_width)
{
float y = (map_width - carpose.x) std::tan(angle) + carpose.y;
return std::sqrt((map_width - carpose.x) (map_width - carpose.x) + (y - carpose.y) (y - carpose.y));
}
}
}
// 模拟雷达数据发布
void LaserDataPublish()
{
laser_scandata.angle_min = angle_min;
laser_scandata.angle_max = angle_max;
laser_scandata.angle_increment = angle_increment;
laser_scandata.time_increment = time_increment;
laser_scandata.scan_time = scan_time;
laser_scandata.range_min = range_min;
laser_scandata.range_max = range_max;
laser_pub.publish(laser_scandata);
}
// 雷达扫描线程,频率10Hz
void Scan()
{
ros::Rate rate(3600);
while(ros::ok())
{
if(scan_index == 0)
{
laser_scandata.header.seq = scan_seq;
laser_scandata.header.stamp = ros::Time::now();
laser_scandata.header.frame_id = "map";
}
laser_scandata.ranges[scan_index] = ScanOnce(scan_index);
scan_index++;
if(scan_index == 360)
{
LaserDataPublish();
scan_index = 0;
}
rate.sleep();
}
}
int scan_index;
int scan_seq;
private:
ros::NodeHandle nh;
ros::Subscriber car_pose_sub;
ros::Publisher laser_pub;
sensor_msgs::LaserScan laser_scandata;
Car car;
std::thread scanthread;
};
int main(int argc, char** argv) { ros::init(argc, argv, "laser_pub_node"); LaserPub laser_pub; ros::AsyncSpinner async_spinner(3); async_spinner.start(); ros::waitForShutdown(); return 0; }
程序中使用到了一个自定义的msg:
纠正效果:
其中黑色是原始数据,蓝色是纠正后的数据,可以看到纠正之后基本与场地实际边界重合,达到纠正目的。