Kinect控制插件
在介绍Turtlebot的描述模型时，我们提到Turtleot上安装了一个Kinect双目摄像头， 并通过插件控制。这个插件是gazebo_plugins包中的一个动态库。 本文中我们详细解释这个插件的源码。
1. GazeboRosOpenniKinect定义
在gazebo_ros_openni_kinect.h 头文件中定义了GazeboRosOpenniKinect类，它有两个父类:DepthCameraPlugin( *.cc, *.hh )和GazeboRosCameraUtils( *.cpp, *.h)。 第一个父类是Gazebo原生的关于深度摄像头的插件，第二个父类则是gazebo_plugins包中一个关于摄像头的辅助类。 在GazeboRosCameraUtils中定义了一个ros::Handle类型的指针rosnode_，它是插件访问ROS系统API的对象。
1.1 关键成员变量
-
point_cloud_msg_是sensor_msgs包中的PointCloud2类型的消息用于描述点云数据。 它有一个N维点的集合，可能附加有发现、强度等信息。点数据以二进制大文件(Blob, binary large object)形式存储。数据格式则通过字段"fields"描述。
点云数据可以以类似图像的2维形式表示，也可以是1维的数组来表示。表示成2D图像形式的点云数据是由类似深度相机的传感器获得的。消息文件中定义了height和weight两个字段。 对于2D形式的数据，它们就表示点云数据的长宽。对于1D数组形式的数据，height是1，width表示点云数据的长度。
此外，PointCLoud2中还有其他一些字段描述点云数据的属性。is_begendian表示数据是否是大端存储的，point_step则描述了一个点数据的字节数，row_step是一行数据的字节数。 data字段则记录了实际的点数据，数据大小为row_step * height。如果数据中没有无效的点，我们认为点云数据是稠密的，用is_dense表示。
-
depth_image_msg_是sensor_msgs包中的Image类型的消息，这里用于描述深度图。 它有height和width描述图像的行列数量，encoding说明了像素的编码方式， 在头文件include/sensor_msgs/image_encoding.h中列举了可行的编码方式。 与PointCloud2类似，有字段is_bigendian表示数据的大小端存储方式，有字段step描述了一行数据的长度，字段data记录了实际的数据内容。
-
image_msg_是在父类GazeboRosCameraUtils中定义的sensor_msgs::Image类型的消息，用于描述Kinect采集的图像信息。
-
point_cloud_pub_, depth_image_pub_和image_pub_是三个消息发布器，分别用于发送point_cloud_msg_, depth_image_msg_和image_msg_。其中image_pub_是定义在父类GazeboRosUtils中的变量。
1.2 关键成员函数
- Load函数用于Gazebo加载插件的时候对系统进行初始化的操作。
- Advertise函数向ROS系统注册了三个发布器，用于发布点云数据、深度图像、和摄像头数据。
- OnNewDepthFrame函数用于更新点云数据和深度图像，并发送关于这两个主题的消息。
- OnNewImageFrame函数则用于记录Kinect采集的图像，并发送消息。
2. 插件初始化过程
在gazebo_ros_openni_kinect.cpp中的构造函数中， 将点云、深度图、深度信息三个计数器置零，并归零了更新时间。 这里的common::Time是Gazebo中定义的时间模块( *.cc, *.hh)。
GazeboRosOpenniKinect::GazeboRosOpenniKinect()
{
this->point_cloud_connect_count_ = 0;
this->depth_info_connect_count_ = 0;
this->depth_image_connect_count_ = 0;
this->last_depth_image_camera_info_update_time_ = common::Time(0);
}
Gazebo装载插件的时候，首先调用Load函数完成对插件系统的初始化操作。它有两个参数，_parent是一个指向与本插件相关联的模型的指针，_sdf则是指向本插件SDF元素的指针。 在该函数的一开始就先调用了父类的Load函数，完成对DepthCameraPlugin的初始化。
void GazeboRosOpenniKinect::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
{
DepthCameraPlugin::Load(_parent, _sdf);
接下来把DepthCameraPlugin中定义的一些成员变量拷贝到GazeboRosCameraUtils中定义的变量中。其实不是很明白为什么会有两个，觉得有些多余。
// copying from DepthCameraPlugin into GazeboRosCameraUtils
this->parentSensor_ = this->parentSensor;
this->width_ = this->width;
this->height_ = this->height;
this->depth_ = this->depth;
this->format_ = this->format;
this->camera_ = this->depthCamera;
在下面的代码中，检查加载插件时是否通过xml标签描述了imageTopicName和cameraInfoTopicName，如果没有描述，那么为GazeboRosCameraUtils的成员变量赋予默认名称。
// using a different default
if (!_sdf->HasElement("imageTopicName"))
this->image_topic_name_ = "ir/image_raw";
if (!_sdf->HasElement("cameraInfoTopicName"))
this->camera_info_topic_name_ = "ir/camera_info";
接着，从插件XML标签中获取点云主题名称，如果没有xml中没有描述就给一个默认值。
// point cloud stuff
if (!_sdf->HasElement("pointCloudTopicName"))
this->point_cloud_topic_name_ = "points";
else
this->point_cloud_topic_name_ = _sdf->GetElement("pointCloudTopicName")->Get();
类似的套路，还有深度图相关的主题名称。
// depth image stuff
if (!_sdf->HasElement("depthImageTopicName"))
this->depth_image_topic_name_ = "depth/image_raw";
else
this->depth_image_topic_name_ = _sdf->GetElement("depthImageTopicName")->Get();
if (!_sdf->HasElement("depthImageCameraInfoTopicName"))
this->depth_image_camera_info_topic_name_ = "depth/camera_info";
else
this->depth_image_camera_info_topic_name_ = _sdf->GetElement("depthImageCameraInfoTopicName")->Get();
下面导入点云的Cutoff配置
if (!_sdf->HasElement("pointCloudCutoff"))
this->point_cloud_cutoff_ = 0.4;
else
this->point_cloud_cutoff_ = _sdf->GetElement("pointCloudCutoff")->Get();
if (!_sdf->HasElement("pointCloudCutoffMax"))
this->point_cloud_cutoff_max_ = 5.0;
else
this->point_cloud_cutoff_max_ = _sdf->GetElement("pointCloudCutoffMax")->Get();
最后调用父类GazeboRosUtils的Load函数，并注册回调函数Advertise，在加载父类时就会调用。
load_connection_ = GazeboRosCameraUtils::OnLoad(boost::bind(&GazeboRosOpenniKinect::Advertise, this));
GazeboRosCameraUtils::Load(_parent, _sdf);
}
在Advertise中，实际上就是发布了三个主题。
void GazeboRosOpenniKinect::Advertise()
{
ros::AdvertiseOptions point_cloud_ao =
ros::AdvertiseOptions::create(
this->point_cloud_topic_name_,1,
boost::bind( &GazeboRosOpenniKinect::PointCloudConnect,this),
boost::bind( &GazeboRosOpenniKinect::PointCloudDisconnect,this),
ros::VoidPtr(), &this->camera_queue_);
this->point_cloud_pub_ = this->rosnode_->advertise(point_cloud_ao);
ros::AdvertiseOptions depth_image_ao =
ros::AdvertiseOptions::create< sensor_msgs::Image >(
this->depth_image_topic_name_,1,
boost::bind( &GazeboRosOpenniKinect::DepthImageConnect,this),
boost::bind( &GazeboRosOpenniKinect::DepthImageDisconnect,this),
ros::VoidPtr(), &this->camera_queue_);
this->depth_image_pub_ = this->rosnode_->advertise(depth_image_ao);
ros::AdvertiseOptions depth_image_camera_info_ao =
ros::AdvertiseOptions::create(
this->depth_image_camera_info_topic_name_,1,
boost::bind( &GazeboRosOpenniKinect::DepthInfoConnect,this),
boost::bind( &GazeboRosOpenniKinect::DepthInfoDisconnect,this),
ros::VoidPtr(), &this->camera_queue_);
this->depth_image_camera_info_pub_ = this->rosnode_->advertise(depth_image_camera_info_ao);
}
3. 更新操作
3.1 更新深度图和点云数据
这个插件有一个成员函数OnNewDepthFrame，用于更新系统的深度信息，它有5个参数。其中_image是一个指针是一个图像数据缓存，将用于填充点云消息point_cloud_msg_和深度图消息depth_image_msg_。 _width, _height, _depth和_format是关于图像帧的一些属性，但是好像没有用。在程序的一开始先检查一下系统的状态确认已经初始化了，而且具有合理的图像长宽属性。 并且用记录了深度图更新时间。
void GazeboRosOpenniKinect::OnNewDepthFrame(const float *_image,
unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format)
{
if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
return;
this->depth_sensor_update_time_ = this->parentSensor->LastMeasurementTime();
在上面代码片段中，获取最近一次测量时间所用的传感器对象parentSensor是在其父类DepthCameraPlugin中定义的DepthCameraSensorPtr类型的指针对象。这是Gazebo原生的传感器类型， 继承自CameraSensor(Sensor)。在插件的初始化过程中还将DepthCameraPlugin中的这个对象拷贝到了GazeboRosCameraUtils中的parentSensor_对象中，这就是一个SensorPtr类型的指针。
下面，通过parentSensor的IsActive判定传感器是否开启。若没有开启，则根据链接数量开启传感器。若已经开启，则填充点云数据和深度图数据。 最后通过函数PublishCameraInfo发布消息。
if (this->parentSensor->IsActive()) {
if (this->point_cloud_connect_count_ <= 0 && this->depth_image_connect_count_ <= 0 &&(*this->image_connect_count_) <= 0) {
this->parentSensor->SetActive(false);
} else {
if (this->point_cloud_connect_count_ > 0)
this->FillPointdCloud(_image);
if (this->depth_image_connect_count_ > 0)
this->FillDepthImage(_image);
}
} else {
if (this->point_cloud_connect_count_ > 0 || this->depth_image_connect_count_ <= 0)
// do this first so there is chance for sensor to run 1 frame after activate
this->parentSensor->SetActive(true);
}
PublishCameraInfo();
}
上面的更新函数中，调用了FillPointdCloud,FillDepthImage和PublishCameraInfo三个函数。下面，我们先来看一下PublishCameraInfo。其内容很简单， 就是先调用GazeboRosCameraUtils父类的PublishCameraInfo函数发布默认的摄像头信息。接着发布Kinect的摄像头信息。
void GazeboRosOpenniKinect::PublishCameraInfo()
{
ROS_DEBUG_NAMED("openni_kinect", "publishing default camera info, then openni kinect camera info");
GazeboRosCameraUtils::PublishCameraInfo();
if (this->depth_info_connect_count_ > 0) {
this->sensor_update_time_ = this->parentSensor_->LastMeasurementTime();
#if GAZEBO_MAJOR_VERSION >= 8
common::Time cur_time = this->world_->SimTime();
#else
common::Time cur_time = this->world_->GetSimTime();
#endif
if (this->sensor_update_time_ - this->last_depth_image_camera_info_update_time_ >= this->update_period_) {
this->PublishCameraInfo(this->depth_image_camera_info_pub_);
this->last_depth_image_camera_info_update_time_ = this->sensor_update_time_;
}
}
}
接下来，关注一下填充点云数据的函数FillPointdCloud。在这个函数的开始和结束的时候通过互斥锁lock_保证了构造点云数据消息point_cloud_msg_的原子性。 在函数总先对点云数据消息的header, width, height, ros_step进行了配置，然后通过辅助函数FillPointCloudHelper填充数据，最后通过point_cloud_pub_将点云数据消息发布出去。 FillPointCloudHelper函数有点长，主要是根据点云数据格式进行拷贝，这里先不详细介绍了。
void GazeboRosOpenniKinect::FillPointdCloud(const float *_src)
{
this->lock_.lock();
this->point_cloud_msg_.header.frame_id = this->frame_name_;
this->point_cloud_msg_.header.stamp.sec = this->depth_sensor_update_time_.sec;
this->point_cloud_msg_.header.stamp.nsec = this->depth_sensor_update_time_.nsec;
this->point_cloud_msg_.width = this->width;
this->point_cloud_msg_.height = this->height;
this->point_cloud_msg_.row_step = this->point_cloud_msg_.point_step * this->width;
///copy from depth to point cloud message
FillPointCloudHelper(this->point_cloud_msg_, this->height, this->width, this->skip_, (void*)_src );
this->point_cloud_pub_.publish(this->point_cloud_msg_);
this->lock_.unlock();
}
类似的，深度图数据的填充函数FillDepthImage也是使用相同的套路实现的。通过lock_对深度图消息depth_image_msg_加锁，并用辅助函数FillDepthImageHelper填充数据。 最后通过depth_image_pub_发布消息。
void GazeboRosOpenniKinect::FillDepthImage(const float *_src)
{
this->lock_.lock();
// copy data into image
this->depth_image_msg_.header.frame_id = this->frame_name_;
this->depth_image_msg_.header.stamp.sec = this->depth_sensor_update_time_.sec;
this->depth_image_msg_.header.stamp.nsec = this->depth_sensor_update_time_.nsec;
///copy from depth to depth image message
FillDepthImageHelper(this->depth_image_msg_, this->height, this->width, this->skip_, (void*)_src );
this->depth_image_pub_.publish(this->depth_image_msg_);
this->lock_.unlock();
}
3.2 更新图像
成员函数OnNewImageFrame用于更新图像，同样的它有5个参数。其中_image是一个指针是一个图像数据缓存，将用于填充图像消息image_msg_，这是一个定义在父类GazeboRosCameraUtils中的消息。 剩余的这些关于图像帧属性的参数，好像在函数中并没有用到。在函数中先检查系统已经正确被初始化了，并判定初始化过程中设定的图像行列长度是否合理。然后记录了更新时间。
void GazeboRosOpenniKinect::OnNewImageFrame(const float *_image,
unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format)
{
if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
return;
this->sensor_update_time_ = this->parentSensor_->LastMeasurementTime();
上述代码片段中，是通过在父类GazeboRosCameraUtils中定义的成员parentSensor_来获取测量时间的。在初始化过程中，就已经将DepthCameraPlugin中的parentSensor对象拷贝到了parentSensor_中了。
下面，仍旧是通过parentSensor的IsActive判定传感器是否开启。若没有开启，则根据链接数量开启传感器。若已经开启，则通过父类GazeboRosCameraUtils的PutCameraData函数填充图像消息image_msg_，并发布之。
if (this->parentSensor->IsActive()) {
if (this->point_cloud_connect_count_ <= 0 && this->depth_image_connect_count_ <= 0 && (*this->image_connect_count_) <= 0) {
this->parentSensor->SetActive(false);
} else {
if ((*this->image_connect_count_) > 0)
this->PutCameraData(_image);
}
} else {
if ((*this->image_connect_count_) > 0)
// do this first so there's chance for sensor to run 1 frame after activate
this->parentSensor->SetActive(true);
}
}