同时发布标记和点云

Question

我尝试使用以下代码从 ROS 节点同时发布点云和标记箭头。当我尝试在 rviz 中同时显示它们时，我看不到它们。

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <visualization_msgs/Marker.h>

ros::Publisher pub;
ros::Publisher arrow_pub;
void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{

// ** conversion of ros message to pcl
pcl::PointCloud<pcl::PointXYZRGB> output;
pcl::fromROSMsg(*input,output);

int h = 480;
int w = 640;
pcl::PointXYZRGB p[w][h];

for(int i=0;i<=w;i++)
{
  for(int j=0;j<=h;j++)
    {
      p[i][j] = output.at(i,j);
    }
 }

visualization_msgs::Marker arrow;

arrow.header.frame_id = "/camera_depth_frame";
arrow.header.stamp = ros::Time::now();

arrow.ns = "example";
arrow.id = 1;

arrow.type = visualization_msgs::Marker::ARROW;
arrow.action = visualization_msgs::Marker::ADD;

arrow.pose.position.x = p[55][55].x;
arrow.pose.position.y = p[55][55].y;
arrow.pose.position.z = p[55][55].z;

arrow.pose.orientation.x = 45;
arrow.pose.orientation.y = 0.0;
arrow.pose.orientation.z = 45;
arrow.pose.orientation.w = 1.0;

arrow.scale.x= 5;
arrow.scale.y= 0.1;
arrow.scale.z = 0.1;

arrow.color.g = 0.0f;
arrow.color.a = 1.0;
arrow.color.r = 0.0f;
arrow.color.b = 1.0f;

arrow.lifetime = ros::Duration();


// ** conversion of pcl messag to ros
sensor_msgs::PointCloud2 cloud;
pcl::toROSMsg(output,cloud);

pub.publish(cloud);
arrow_pub.publish(arrow);
}


int
main(int argc, char** argv)
{
    ros::init(argc, argv,"my_pcl_tutorial");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("input",1,cloud_cb);
    pub = nh.advertise<sensor_msgs::PointCloud2> ("output",1);
    arrow_pub = nh.advertise<visualization_msgs::Marker> ("out",1);
    ros::spin();
}

当我运行 rostopic echo out 和 output 时，我可以看到两个主题都由节点发布，但我无法在 ROS 节点内显示它们。

Answer 1

尝试对您的点云和标记消息使用相同的 header.frame_id，然后，在 RVIZ 中，选择正确的主题并将固定框架设置为您的 frame_id 值：

代码显示如下：

void publish(PointCloudPtr cloud)
{    
    cloud->header.frame_id = "/velodyne";
    pub_cloudObjects.publish (cloud);    
}


void add_marker( Eigen::Vector3f center, int number)
{
    visualization_msgs::Marker marker;
    // Set our shape type to be a cube, size & rgb
    uint32_t shape = visualization_msgs::Marker::CUBE;
    marker.ns = "basic_shapes";
    marker.id = number;
    marker.type = shape;
    marker.action = visualization_msgs::Marker::ADD;

    marker.pose.position.x = (float)center(0);
    marker.pose.position.y = (float)center(1);
    marker.pose.position.z = (float)center(2);
    marker.pose.orientation.x = 0.0;
    marker.pose.orientation.y = 0.0;
    marker.pose.orientation.z = 0.0;
    marker.pose.orientation.w = 1.0;

    marker.scale.x = 0.5;
    marker.scale.y = 0.5;
    marker.scale.z = 0.5;

    marker.color.r = 0.0f;
    marker.color.g = 1.0f;
    marker.color.b = 0.0f;
    marker.color.a = 1.0;

    marker.lifetime = ros::Duration();
    marker.header.frame_id = "/velodyne";
    marker.header.stamp = ros::Time::now();

    marker_pub.publish(marker);
}

然后在 RVIZ 中：

Global Options:
    Fixed Frame: /velodyne

它对我有效。

同时发布标记和点云

Publishing marker and point cloud at the same time

c++

ros