ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

RViz does not display topics from a separate node

asked 2014-08-07 11:05:27 -0600

xuningy gravatar image

updated 2014-08-07 12:10:45 -0600

I posted in another thread about RViz not displaying PoseArray, turns out the issues is not just PoseArray but my other displays in the same node too. Thread here: http://answers.ros.org/question/189041

The PC2 topic is also not displaying.

Just a bit of context: It is taking in a point cloud "point_cloud_centers" and calculating normals PoseArray, as well as outputting a normal filtered point cloud (limited by z). The point cloud was displaying fine until i added the normals PoseArray to be published in RViz, which has a fixed frame "/map" given by another node (which is already running at the same time as /point_cloud_centers" is given by that same node)

Also, both published topics are receiving the data, as given by rostopic echo /normal_filtered_pcl and rostopic echo /normal_vectors. My pc cloud is about 90,000 points if that matters at all. Thanks!

Edit: Thanks to bvbdort to point out I didn't attach my declarations. They've been added to the top (forgot this was right underneath the include statements.)

ros::Publisher pub;
ros::Publisher poseArrayPub;
geometry_msgs::PoseArray poseArray; // particles as PoseArray (preallocated)

void normalCallback (const sensor_msgs::PointCloud2ConstPtr& cloud)
{

  sensor_msgs::PointCloud2 output_normals;
  sensor_msgs::PointCloud2 cloud_normals;
  sensor_msgs::PointCloud2 cloud_filtered;
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2 (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_pass (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_filtered2 (new pcl::PointCloud<pcl::PointXYZRGBNormal>);   

  // Start making result

  pcl::fromROSMsg (*cloud, *cloud2);

  poseArray.header.stamp = ros::Time::now();
  poseArray.header.frame_id = cloud2->header.frame_id; //EDITED
  ROS_INFO_STREAM("poseArray.header: frame=" << poseArray.header.frame_id); //Outputs "/map"

  // estimate normals
  pcl::NormalEstimation<pcl::PointXYZ, pcl::PointNormal> ne;
  ne.setInputCloud(cloud2);
  ne.setKSearch (24);
  pcl::PointCloud<pcl::PointNormal>::Ptr normals (new pcl::PointCloud<pcl::PointNormal>);
  ne.compute(*normals);



  /******************Display filtered cloud based on height********/
  pcl::toROSMsg (*normals, output_normals);
  pcl::concatenateFields (*cloud, output_normals, cloud_normals);

  pcl::fromROSMsg (cloud_normals, *cloud_pass);



   // Create the filtering object
    pcl::PassThrough<pcl::PointXYZRGBNormal> pass;
    pass.setInputCloud (cloud_pass);
    pass.setFilterFieldName ("z");
    pass.setFilterLimits (0.0, 1.0);
    pass.filter (*cloud_filtered2);


    pcl::toROSMsg (*cloud_filtered2, cloud_filtered);

    ROS_INFO("points: %lu\n", cloud_filtered2->points.size());

    // Publish the data
    pub.publish (cloud_filtered);
  /***************************************************************/



  /***********************publish normal vectors************************/
  for(size_t i = 0; i<normals->points.size(); ++i)
  {

    // Declare goal output pose
    geometry_msgs::PoseStamped pose;
    geometry_msgs::Quaternion msg;

    tf::Vector3 axis_vector(normals->points[i].normal[0], normals->points[i].normal[1], normals->points[i].normal[2]);
    tf::Vector3 up_vector(0.0, 0.0, 1.0);

    tf::Vector3 right_vector = axis_vector.cross(up_vector);
    right_vector.normalized();
    tf::Quaternion q(right_vector, -1.0*acos(axis_vector.dot(up_vector)));
    q.normalize();
    tf::quaternionTFToMsg(q, msg);

    pose.pose.position.x = cloud2->points[i].x;
    pose.pose.position.y = cloud2->points[i].y;
    pose.pose.position.z = cloud2->points[i].z;

    pose.pose.orientation = msg;

    poseArray.poses.push_back(pose.pose);

  }

    poseArrayPub.publish(poseArray);
    ROS_INFO("poseArray size: %i", poseArray.poses.size());
    /***************************************************************/

}

int main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "normal_filter");
  ros::NodeHandle nh;

  ros::Rate loop_rate(10);
  // Create a ROS subscriber for the input point cloud
  ros::Subscriber ...
(more)
edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

answered 2014-08-07 13:19:21 -0600

xuningy gravatar image

updated 2014-08-07 13:19:43 -0600

SOLVED. Had -nan values somewhere in my 90,000 points.

 if (isinf(pose.pose.orientation.x) || isnan(pose.pose.orientation.x) ||
      isinf(pose.pose.orientation.y) || isnan(pose.pose.orientation.y) || 
      isinf(pose.pose.orientation.z) || isnan(pose.pose.orientation.z) ||
      isinf(pose.pose.orientation.w) || isnan(pose.pose.orientation.w) ){
      ROS_WARN("Point %d [%0.5f, %0.5f, %0.5f | %0.5f, %0.5f, %0.5f, %0.5f] ", i, pose.pose.position.x, pose.pose.position.y, pose.pose.position.z, pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w);
      continue;
}
edit flag offensive delete link more
0

answered 2014-08-07 11:45:59 -0600

bvbdort gravatar image

posearray definition is missing

geometry_msgs::PoseArray poseArray;

Increase size of buffer in publisher

poseArrayPub = nh.advertise<geometry_msgs::PoseArray>("/normal_vectors", 1000, 1);

Try to find frame id of pointcould and use the same frame from poseArray

ROS_INFO_STREAM("frame=" << cloud_filtered.header.frame_id);

poseArray.header.frame_id="/camera_depth_optical_frame";  // check this
edit flag offensive delete link more

Comments

Hi, Thanks for pointing the first point out -- I had it in my code but was trying to avoid the #include messages in copy pasting. I did the next two suggestions, none worked. Neither topics are displaying.

xuningy gravatar image xuningy  ( 2014-08-07 12:11:59 -0600 )edit

Question Tools

1 follower

Stats

Asked: 2014-08-07 11:05:27 -0600

Seen: 1,141 times

Last updated: Aug 07 '14