ros nodelet 使用

Posted 2020-10-26 每天一点积累

ros nodelet能够加快高吞吐量程序运行速度比如点云

基本入门程序可以看

http://wiki.ros.org/nodelet/Tutorials/Porting%20nodes%20to%20nodelets

http://wiki.ros.org/nodelet

什么是nodelet，nodelet有什么用处

https://answers.ros.org/question/230972/what-is-a-nodelet/

 

代码框架：

class pcl_process_class
{
class MyPointCloudGeneratorNodelet : public nodelet::Nodelet
{
  // Subscriptions
  boost::shared_ptr<image_transport::ImageTransport> it_;
  image_transport::CameraSubscriber sub_depth_;
  int queue_size_;

  // Publications
  boost::mutex connect_mutex_;
  typedef sensor_msgs::PointCloud2 PointCloud;
  ros::Publisher pub_point_cloud_;

  image_geometry::PinholeCameraModel model_;

  virtual void onInit();

  void connectCb();

  void depthCb(const sensor_msgs::ImageConstPtr& depth_msg,
               const sensor_msgs::CameraInfoConstPtr& info_msg);
};

void MyPointCloudGeneratorNodelet::onInit()
{
  ros::NodeHandle& nh         = getNodeHandle();
  it_.reset(new image_transport::ImageTransport(nh));
  queue_size_ = 5;
  // Read parameters

  // Monitor whether anyone is subscribed to the output
  ros::SubscriberStatusCallback connect_cb = boost::bind(&MyPointCloudGeneratorNodelet::connectCb, this);
  // Make sure we don\'t enter connectCb() between advertising and assigning to pub_point_cloud_
  boost::lock_guard<boost::mutex> lock(connect_mutex_);
  pub_point_cloud_ = nh.advertise<PointCloud>("points", 1, connect_cb, connect_cb);
  ROS_INFO("onInit");

}

// Handles (un)subscribing when clients (un)subscribe
void MyPointCloudGeneratorNodelet::connectCb()
{
  boost::lock_guard<boost::mutex> lock(connect_mutex_);
  if (pub_point_cloud_.getNumSubscribers() == 0)
  {
    sub_depth_.shutdown();
  }
  else if (!sub_depth_)
  {
    image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
    sub_depth_ = it_->subscribeCamera("image_rect", queue_size_, &MyPointCloudGeneratorNodelet::depthCb, this, hints);
  }
  ROS_INFO("connectCb");

}

void MyPointCloudGeneratorNodelet::depthCb(const sensor_msgs::ImageConstPtr& depth_msg,
                                   const sensor_msgs::CameraInfoConstPtr& info_msg)
{

      pcl_process_class mpcl_process_class;
      mpcl_process_class.pcl_process(depth_msg);
      PointCloud pointCloud;
      pcl::toROSMsg(*mpcl_p ointCloud);
}

} // namespace depth_image_proc

// Register as nodelet
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(vision_obstacles_avoidance::MyPointCloudGeneratorNodelet,nodelet::Nodelet);

nodelet.launch

<launch>
  <node pkg="nodelet" type="nodelet" name="standalone_nodelet"  args="manager" output="screen"/>

  <node pkg="nodelet" type="nodelet" name="MyPointCloudGeneratorNodelet" args="load vision_obstacles_avoidance/MyPointCloudGeneratorNodelet standalone_nodelet" output="screen">
   <remap from="image_rect" to="/camera/depth_registered/hw_registered/image_rect_raw"/>   
   <remap from="points" to="/point_cloud"/>   
   <remap from="/camera/depth_registered/hw_registered/camera_info" to="/camera/depth_registered/camera_info"/>   
  </node>
</launch>

nodelet比较好用尤其是在使用pointcloud时候，由于ros node之间采用tcpros标准来传输数据，点云要经过压缩解压缩过程所以很慢，但是nodelet是直接使用原来数据，类似指针，但是只能在同一个机器下才有用。

 

class MyPointCloudGeneratorNodelet : public nodelet::Nodelet{  // Subscriptions  boost::shared_ptr<image_transport::ImageTransport> it_;  image_transport::CameraSubscriber sub_depth_;  int queue_size_;
  // Publications  boost::mutex connect_mutex_;  typedef sensor_msgs::PointCloud2 PointCloud;  ros::Publisher pub_point_cloud_;
  image_geometry::PinholeCameraModel model_;
  virtual void onInit();
  void connectCb();
  void depthCb(const sensor_msgs::ImageConstPtr& depth_msg,               const sensor_msgs::CameraInfoConstPtr& info_msg);};
void MyPointCloudGeneratorNodelet::onInit(){  ros::NodeHandle& nh         = getNodeHandle();  it_.reset(new image_transport::ImageTransport(nh));  queue_size_ = 5;  // Read parameters
  // Monitor whether anyone is subscribed to the output  ros::SubscriberStatusCallback connect_cb = boost::bind(&MyPointCloudGeneratorNodelet::connectCb, this);  // Make sure we don\'t enter connectCb() between advertising and assigning to pub_point_cloud_  boost::lock_guard<boost::mutex> lock(connect_mutex_);  pub_point_cloud_ = nh.advertise<PointCloud>("points", 1, connect_cb, connect_cb);  ROS_INFO("onInit");
}
// Handles (un)subscribing when clients (un)subscribevoid MyPointCloudGeneratorNodelet::connectCb(){  boost::lock_guard<boost::mutex> lock(connect_mutex_);  if (pub_point_cloud_.getNumSubscribers() == 0)  {    sub_depth_.shutdown();  }  else if (!sub_depth_)  {    image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());    sub_depth_ = it_->subscribeCamera("image_rect", queue_size_, &MyPointCloudGeneratorNodelet::depthCb, this, hints);  }  ROS_INFO("connectCb");
}
void MyPointCloudGeneratorNodelet::depthCb(const sensor_msgs::ImageConstPtr& depth_msg,                                   const sensor_msgs::CameraInfoConstPtr& info_msg){
  pcl_process_class mpcl_process_class;  mpcl_process_class.pcl_process(depth_msg);  PointCloud pointCloud;  pcl::toROSMsg(*mpcl_p ointCloud);}
} // namespace depth_image_proc
// Register as nodelet#include <pluginlib/class_list_macros.h>PLUGINLIB_EXPORT_CLASS(vision_obstacles_avoidance::MyPointCloudGeneratorNodelet,nodelet::Nodelet);