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 "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 ...