publishing marker and point cloud same time

asked 2016-07-11 08:47:27 -0500

dinesh gravatar image

updated 2016-07-11 20:42:02 -0500

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.

edit retag flag offensive close merge delete

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?

ahendrix gravatar image ahendrix  ( 2016-07-11 21:50:45 -0500 )edit

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.

dinesh gravatar image dinesh  ( 2016-07-12 05:57:26 -0500 )edit

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

dinesh gravatar image dinesh  ( 2016-07-13 03:21:15 -0500 )edit