Velodyne仿真插件的新生
到目前为止，我们都还一直在对Gazebo的官方教程进行扩展，主要是为了充分的了解Gazebo与ROS之间是如何配合工作的。官方教程中企图通过一组竖直的光束和一个旋转关节来模拟多线激光的扫描过程， 但是这种方式的仿真效率比较低，为了获得较为密集的点云，我们就需要一方面提高光束的更新频率另一方面降低扫描速度。提高光束的更新频率就意味着仿真系统中通信频率的提高， 这将在一定程度上降低仿真系统的更新速度。降低扫描速度意味着机器人需要在一个较低的速度下运动，否则在建图的时候我们还必需把扫描数据的产生时间考虑进来。
其实，Gazebo原生提供的ray传感器是支持多线激光的。通过它我们能够以一个较低的频率更新传感器数据，并且可以得到一个很高的点云密度，而不需要对机器人的运动速度做什么限制。 如此一来也就不需要一个旋转关节来模拟扫描过程了，所以出于后续接入Cartographer算法的考虑，我们需要对Velodyne的这个插件进行重构。
本文中，我们将先具体了解一下Gazebo原生的ray传感器，再对模型文件和插件类VelodynePlugin进行调整，最后介绍ray的扫描数据应当如何解析。
ray
├── scan 描述激光的扫描范围
├── range 描述每束激光的探测距离
└── noise 可选,描述测量噪声
1. Gazebo的ray传感器
在开始重构之前，让我们先具体了解一下SDF文件中ray传感器都有哪些字段以及它们的意义。 如右侧的树形结构所示，我们可以通过scan、range和noise三个方面来描述一个ray传感器，其中噪声项noise不是必需的。
scan
├── horizontal 描述水平扫描范围
│ ├── samples 描述雷达扫描一圈的采样光束数量
│ ├── resolution 分辨率,和samples一起决定实际的采样点数量
│ ├── min_angle 起始扫描角度
│ └── max_angle 终止扫描角度，大于等于min_angle
└── vertical 可选,描述竖直扫描范围,字段同horizontal
字段scan用于描述激光的扫描范围，它有horizontal和vertical两个字段。其中horizontal用于描述雷达的水平扫描方式，而描述竖直方向的vertical则不是必需的。 在用户没有提供vertical字段的情况下，Gazebo会认为所模拟的雷达是一种单线的激光。
字段horizontal和vertical的描述方式是一样的，它们都有四个字段samples、resolution、min_angle和max_angle。其中samples和resolution一起描述了雷达扫描一圈的采样点数量， 一般情况下分辨率都是1。如果分辨率小于1，Gazebo就会对扫描数据进行插值；如果大于1，就会对多个扫描值进行平均。而字段min_angle和max_angle约束了扫描范围，采样点将均匀的分布在这个范围里。
在之前的demo中只有horizontal字段，所以只能将之旋转90度安装，使用过程有诸多不便。我们完全可以把vertical字段也添加进来，令其samples字段为32，分辨率为1，来模拟32线的激光。 下面的代码片段分别是修改后模型文件的关于水平和竖直扫描范围的描述。 水平方向上，我们提高了采样点数量，扫描范围也扩大到了\([-\pi, \pi]\)。
|
|
在新的模型文件中，我们除了调整了上述的ray传感器的描述方式之外，还将唯一的转动关节改为fixed，不再提供任何自由度。 如果我们打开gazebo直接将修改后的模型拖进仿真场景中，就会看到如下密集的扫描效果。
 |
2. 重构后的VelodynePlugin
调整完模型文件之后，我们还需要相应的对仿真插件作出修改。如下面的代码片段所示， 我们对插件类VelodynePlugin的成员变量进行了精简，只保留了6个变量。 VelodynePlugin仍然继承自ModelPlugin，被我们当作模型插件。各个字段的意义参见其后注释，这里不再解释。
class VelodynePlugin : public ModelPlugin {
private:
std::unique_ptr<ros::NodeHandle> mRosNode; // ROS的节点句柄
ros::Publisher mRosPointCloudPub; // 用于发布激光点云的主题发布器
physics::WorldPtr mWorld; // 仿真世界对象
physics::ModelPtr mModel; // 仿真模型对象
gazebo::transport::NodePtr mGazeboNode; // Gazebo的通信节点
gazebo::transport::SubscriberPtr mLaserScanSub; // 订阅Gazebo激光扫描数据的订阅器
};
相应的，我们对Load函数修改如下。仍然保留了一开始对关节数量的查询，确保模型文件没有太大的问题。
public: virtual void Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
if (model->GetJointCount() == 0) {
std::cerr << "没有正确装载velodyne模型\n";
return;
}
std::cout << "douniwan" << std::endl;
然后，获取世界对象和模型对象，并分别用成员指针mWorld和mModel记录之。
this->mWorld = model->GetWorld();
this->mModel = model;
接着，创建一个ROS的节点句柄，并通过该句柄注册发布主题"/point_cloud_3d"。
this->mRosNode.reset(new ros::NodeHandle("~"));
this->mRosPointCloudPub = this->mRosNode->advertise<sensor_msgs::PointCloud2>("/point_cloud_3d", 10);
最后构建Gazebo通信节点，并在初始化之后注册接收雷达数据的回调函数。我们将在该回调函数中解析雷达数据，填充ROS点云消息并通过主题"/point_cloud_3d"发布出去。
this->mGazeboNode = gazebo::transport::NodePtr(new gazebo::transport::Node());
this->mGazeboNode->Init();
mLaserScanSub = this->mGazeboNode->Subscribe("~/my_velodyne/velodyne_hdl-32/top/sensor/scan", &VelodynePlugin::OnLaserScanMsg, this);
}
3. 激光数据的回调函数
下面的代码片段是接收到Gazebo的雷达扫描数据消息后的回调函数，该函数唯一的输入参数就是接收的扫描数据消息。在该函数的一开始，我们先从数据消息中解析出了雷达在世界坐标系下的位姿。
private: void OnLaserScanMsg(ConstLaserScanStampedPtr & msg) {
gazebo::msgs::Pose const & pose_msg = msg->scan().world_pose();
ignition::math::Pose3d pose = gazebo::msgs::ConvertIgn(pose_msg);
Eigen::Quaterniond qua(pose.Rot().W(), pose.Rot().X(), pose.Rot().Y(), pose.Rot().Z());
然后解析获取水平和数值方向上的扫描起始角度、终结角度、扫描角度步长、采样点数量。它们将用于遍历扫描数据，填充激光点云。
int count = msg->scan().count();
int vertical_count = msg->scan().vertical_count();
double angle_min = msg->scan().angle_min();
double angle_step = msg->scan().angle_step();
double range_min = msg->scan().range_min();
double range_max = msg->scan().range_max();
double vertical_angle_min = msg->scan().vertical_angle_min();
double vertical_angle_max = msg->scan().vertical_angle_max();
double vertical_step = msg->scan().vertical_angle_step();
接下来，我们创建一个临时的点云对象pc，并在一个双层for循环中遍历扫描数据。先从中获取探测到的距离range，并判定它在一个合理的范围内。
pcl::PointCloud<pcl::PointXYZ> pc;
for (int i = 0; i < count; i++) {
for (int j = 0; j < vertical_count; j++) {
double range = msg->scan().ranges(i + j * count);
if (std::isinf(range) || range > range_max || range < range_min)
continue;
如果数据合理，我们就根据起始角度、扫描角度步长以及遍历索引计算扫描点相对于传感器的俯仰角(pitch)和偏航角(yaw)。
double pitch = j * vertical_step + vertical_angle_min;
double yaw = i * angle_step + angle_min;
根据球坐标模型计算扫描点在传感器坐标系下的坐标，用临时变量point记录。
double cp = cos(pitch); double sp = sin(pitch);
double cy = cos(yaw); double sy = sin(yaw);
Eigen::Vector3d point(range * cp * cy, range * cp * sy, range * sp);
在成功获得扫描点坐标后，乘上传感器的全局位姿，得到扫描点在世界坐标系下的位姿。
point = qua.matrix() * point;
pc.push_back(pcl::PointXYZ(point[0] + pose.Pos().X(), point[1] + pose.Pos().Y(), point[2] + pose.Pos().Z()));
}
}
最后，利用包pcl_conversions将刚刚构建的点云数据填充到一个ROS的消息对象pc_msg中，并通过消息发布器mRosPointCloudPub发布出去。
sensor_msgs::PointCloud2 pc_msg;
pcl::toROSMsg(pc, pc_msg);
pc_msg.header.stamp = ros::Time(msg->time().sec(), msg->time().nsec());
pc_msg.header.frame_id = "base";
this->mRosPointCloudPub.publish(pc_msg);
}
4. 完
至此，关于插件的重构就介绍到这里。下面的两幅图分别是Gazebo和rviz的截图。在Gazebo世界中，我们放置了一个圆柱、立方体、球。在rviz订阅的点云数据中我们可以大致看出这三个几何体的轮廓。
 |
 |