ROS RVIZ 点云图相关实现

最近使用ROS RVIZ实现点云图拼接，不过没有得到很好的效果，这里做个阶段总结。 ROS RVIZ是个功能强大的可视化工具，可以显示很多与机器人相关的信息。

先对ROS中的坐标简要说明，ROS的坐标通过/tf topic 来管理，tf是一个多叉树结构，每一个坐标通过一个frame id识别，使用代码来管理tf，所有坐标都是通过/tf由程序员自己管理。按照官方指南，tf通过broadcast , listen, add fixed frame。

broadcast：

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <sensor_msgs/PointCloud2.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "world_tf_broadcaster");
  ros::NodeHandle nh;
ros::Rate r(10);
  tf::TransformBroadcaster broadcaster;
  tf::Transform tfT;

  while(nh.ok()){
  tfT.setIdentity();
  broadcaster.sendTransform(tf::StampedTransform(tfT,ros::Time::now(), "ORB_SLAM/Camera", "kinect2_link"));                       //kinect2_rgb_optical_frame
 /*   broadcaster.sendTransform(
      tf::StampedTransform(
        tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.0, 0.0, 0.0)),
        ros::Time::now(),"ORB_SLAM/Camera", "kinect2_link"));*/
    //r.sleep();
  }

  ros::spin();
  return 0;
};

listen：

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "world_tf_listener");

  ros::NodeHandle node;

  tf::TransformListener listener;
  tf::StampedTransform transform;

  ros::Rate rate(10.0);
  while (node.ok()){
    
    try{
       // listener.waitForTransform("/world", "/kinect2_rgb_optical_frame", ros::Time(0), ros::Duration(3.0));
        listener.lookupTransform("/world", "/kinect2_rgb_optical_frame",
                           ros::Time(0), transform);
    }
    catch (tf::TransformException &ex) {
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
      continue;
    }

    rate.sleep();
  }
  return 0;
};

关于点云图，RVIZ中有两种格式，PointCloud2 与 PointCloud，具体关系没有搞清楚，但是这个是月PCL包相关的格式，是PCL与ROS互相支持的。点云图是通过获取已发布关于点云信息的TOPIC得到。

PointCloud：

#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
//#include <boost/foreach.hpp>
#include <tf/transform_listener.h>
#include <pcl_ros/transforms.h>


typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;

ros::Publisher tf_pub;
tf::TransformListener *tf_listener; 

void callback(const PointCloud::ConstPtr& pcl_in)
{
  PointCloud pcl_out;

  tf_listener->waitForTransform("/ORB_SLAM/World", (*pcl_in).header.frame_id, ros::Time::now(),      ros::Duration(5.0));
  pcl_ros::transformPointCloud("/ORB_SLAM/World", *pcl_in, pcl_out, *tf_listener);
  tf_pub.publish(pcl_out);

  //printf ("Cloud: width = %d, height = %d\n", pcl_in->width, pcl_in->height);
  //BOOST_FOREACH (const pcl::PointXYZ& pt, pcl_in->points)
    //printf ("\t(%f, %f, %f)\n", pt.x, pt.y, pt.z);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "sub_pcl");
  ros::NodeHandle nh;
  ros::Subscriber sub = nh.subscribe<PointCloud>("kinect2/qhd/points", 1, callback);
  tf_pub = nh.advertise<PointCloud> ("tf_points2", 1);
  
  tf_listener    = new tf::TransformListener();

  ros::spin();
}

PointCloud2：

#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
//#include <boost/foreach.hpp>
#include <tf/transform_listener.h>
#include <pcl_ros/transforms.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/PointCloud.h>

ros::Publisher tf_pub;
tf::TransformListener *tf_listener;

void 
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
  // Create a container for the data.
  sensor_msgs::PointCloud2 getinput, output;

  // Do data processing here...
  getinput = *input;
  output= *input;
tf_listener->waitForTransform("/ORB_SLAM/World", "kinect2_rgb_optical_frame", ros::Time::now(),      ros::Duration(5.0));
pcl_ros::transformPointCloud("/ORB_SLAM/World", getinput, output, *tf_listener);

  // Publish the data.
  tf_pub.publish (output);
}

int
main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "my_pcl_tutorial");
  ros::NodeHandle nh;

  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = nh.subscribe ("kinect2/qhd/points", 1, cloud_cb);

  // Create a ROS publisher for the output point cloud
  tf_pub = nh.advertise<sensor_msgs::PointCloud2> ("tf_points2", 1);

  tf_listener    = new tf::TransformListener();
  // Spin
  ros::spin ();
}

事实上，还有很多相关方面需要继续学习。

ROS RVIZ 点云图相关实现

猜你喜欢