同時にマーカーとポイントクラウドを公開する

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

その後、RVIZでは、正しいトピックを選択して、frame_id値として固定枠を設定し、あなたのpointcloudとマーカーmessaagesに同じheader.frame_idを使用してみてください：

コードのショーは、このようにかなったこと：

RVIZで

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); 
} 

、その後：

Global Options: 
    Fixed Frame: /velodyne 

それは私の場合で働いていました。