CMU-Exploration 代码笔记
总揽
仿真中重要的部分就是包含差速车的仿真(包含激光雷达等),地形分析、局部路径规划、路径更踪等;
sensor_scan_generation包
#include <math.h>
#include <time.h>
#include <stdio.h>
#include <stdlib.h>
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
using namespace std;
pcl::PointCloud<pcl::PointXYZ>::Ptr laserCloudIn(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZ>::Ptr laserCLoudInSensorFrame(new pcl::PointCloud<pcl::PointXYZ>());
double robotX = 0;
double robotY = 0;
double robotZ = 0;
double roll = 0;
double pitch = 0;
double yaw = 0;
bool newTransformToMap = false;
nav_msgs::Odometry odometryIn;
ros::Publisher *pubOdometryPointer = NULL;
tf::StampedTransform transformToMap;
tf::TransformBroadcaster *tfBroadcasterPointer = NULL;
ros::Publisher pubLaserCloud;
//回调函数
void laserCloudAndOdometryHandler(const nav_msgs::Odometry::ConstPtr& odometry,
const sensor_msgs::PointCloud2ConstPtr& laserCloud2)
{
laserCloudIn->clear();
laserCLoudInSensorFrame->clear();
pcl::fromROSMsg(*laserCloud2, *laserCloudIn);
odometryIn = *odometry;
transformToMap.setOrigin(
tf::Vector3(odometryIn.pose.pose.position.x, odometryIn.pose.pose.position.y, odometryIn.pose.pose.position.z));
transformToMap.setRotation(tf::Quaternion(odometryIn.pose.pose.orientation.x, odometryIn.pose.pose.orientation.y,
odometryIn.pose.pose.orientation.z, odometryIn.pose.pose.orientation.w));
int laserCloudInNum = laserCloudIn->points.size();
pcl::PointXYZ p1;
tf::Vector3 vec;
//把所有点转到sensor坐标系上?
//这里/registered_scan是投影到map坐标系里的
for (int i = 0; i < laserCloudInNum; i++)
{
p1 = laserCloudIn->points[i];
vec.setX(p1.x);
vec.setY(p1.y);
vec.setZ(p1.z);
vec = transformToMap.inverse() * vec;
p1.x = vec.x();
p1.y = vec.y();
p1.z = vec.z();
laserCLoudInSensorFrame->points.push_back(p1);
}
odometryIn.header.stamp = laserCloud2->header.stamp;
odometryIn.header.frame_id = "map";
odometryIn.child_frame_id = "sensor_at_scan";
pubOdometryPointer->publish(odometryIn);
transformToMap.stamp_ = laserCloud2->header.stamp;
transformToMap.frame_id_ = "map";
transformToMap.child_frame_id_ = "sensor_at_scan";
tfBroadcasterPointer->sendTransform(transformToMap);
sensor_msgs::PointCloud2 scan_data;
pcl::toROSMsg(*laserCLoudInSensorFrame, scan_data);
scan_data.header.stamp = laserCloud2->header.stamp;
scan_data.header.frame_id = "sensor_at_scan";
pubLaserCloud.publish(scan_data);
}
int main(int argc, char** argv)
{
//ros初始化
ros::init(argc, argv, "sensor_scan");
ros::NodeHandle nh;
ros::NodeHandle nhPrivate = ros::NodeHandle("~");
/************创建message_filters类************
message_filters类是 ROS(机器人操作系统)中的一个功能包,用于实现多个传感器数据或消息的时间同步。
可以方便地对不同话题发布的消息进行时间戳的同步,以确保数据在处理时具有一致的时间对齐
************************
即可以直接回调函数对两个及以上话题收到的消息在同一时间处理;
LOAM/LIOSAM等都是把数据存vector里遍历找时间戳相同的,人工时间对齐
*/
message_filters::Subscriber<nav_msgs::Odometry> subOdometry;
message_filters::Subscriber<sensor_msgs::PointCloud2> subLaserCloud;
/***********创建message_filters类************
*
* sync_policies::ApproximateTime 方法允许话题之间的时间辍存在一定的偏差
* sync_policies::ExactTime 表示必须时间完全一致,可能会导致频率下降
* syncPolicy(100) 100是queue size,队列长度,默认为10,
*/
typedef message_filters::sync_policies::ApproximateTime<nav_msgs::Odometry, sensor_msgs::PointCloud2> syncPolicy;
typedef message_filters::Synchronizer<syncPolicy> Sync;
boost::shared_ptr<Sync> sync_;
subOdometry.subscribe(nh, "/state_estimation", 1);
subLaserCloud.subscribe(nh, "/registered_scan", 1);
sync_.reset(new Sync(syncPolicy(100), subOdometry, subLaserCloud));
sync_->registerCallback(boost::bind(laserCloudAndOdometryHandler, _1, _2));
//值得学习,ros::Publisher类只有指针pubOdometryPointer是全局变量
//这样不用创建ros::Publisher的全局变量
ros::Publisher pubOdometry = nh.advertise<nav_msgs::Odometry> ("/state_estimation_at_scan", 5);
pubOdometryPointer = &pubOdometry;
tf::TransformBroadcaster tfBroadcaster;
tfBroadcasterPointer = &tfBroadcaster;
pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>("/sensor_scan", 2);
ros::spin();
return 0;
}
这个功能包只是运行了;对自主导航、规划没有太大用处
并没有功能包收/sensor_scan和/state_estimation_at_scan消息
这个功能包的目的是接收map坐标系下的点云数据
然后把这个数据转到sensor坐标系下,猜测是用来检测仿真是否正常的