我试图通过使用下面的代码从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节点中显示它们。
答案 0 :(得分:0)
尝试对你的pointcloud和marker messaages使用相同的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
它适用于我的情况。