Robotics StackExchange | Archived questions

publishing marker and point cloud same time

I was trying to publish point cloud and marker arrow same time from a ros node by using below code. When i try to see them both in rviz display i can see them both arrow is coming but when i put /output in the pointcloud2 display, it shows error. when i run rostopic echo output i can see the values, but inside rviz it shows error.

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

What is the problem am i doing something wrong relate to tf.

Asked by dinesh on 2016-07-11 08:47:27 UTC

Comments

It looks like you copy the data to the new point cloud but you don't copy the frame or header information. Does your published point cloud have the timestamp and the frame_id set in the header?

Asked by ahendrix on 2016-07-11 21:50:45 UTC

ok, finally it is showin arrow but not in right position where i wanted. for eg. i am trying to show the arrow starting from (xw,yw,zw) euclidean point so when i put arrow.pose.position.x = xw or xp ... the arrow position is not starting from any point in the point cloud.

Asked by dinesh on 2016-07-12 05:57:26 UTC

ok now i got the correct position of marker in the point cloud, after digging the working of commands in detail. problem solved now.

Asked by dinesh on 2016-07-13 03:21:15 UTC

Answers