RViz does not display topics from a separate node
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 "pointcloudcenters" 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 /pointcloudcenters" 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 sub = nh.subscribe ("/point_cloud_centers", 10, normalCallback);
// Create a ROS publisher for the output point cloud
pub = nh.advertise<sensor_msgs::PointCloud2> ("/normal_filtered_pcl", 10, 1);
poseArrayPub = nh.advertise<geometry_msgs::PoseArray>("/normal_vectors", 10, 1);
// Spin
ros::spin();
}
Asked by xuningy on 2014-08-07 11:05:27 UTC
Answers
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
Asked by bvbdort on 2014-08-07 11:45:59 UTC
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.
Asked by xuningy on 2014-08-07 12:11:59 UTC
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;
}
Asked by xuningy on 2014-08-07 13:19:21 UTC
Comments