PoseArray receiving data, not displaying in RViz
I am constructing a PoseArray message with a number of orientations (not from a sensor) and sending it to display on RViz. using rostopic echo /poseArrayTopic
, I see that all the messages are being received and has the correct values.
I believe the header is the issue, I have tried the following:
poseArray.header.stamp = cloud->header.stamp;
poseArray.header.frame_id = "/map";
, running the code with/without specifying a fixed frame, setting the stamp to ros::Time::now()
, none of which seemed to work.
Anyone got an idea?
Edit: from `rostopic echo /poseArrayTopic:
-
position:
x: 6.28499984741
y: 5.35500001907
z: 0.495000004768
orientation:
x: -0.0
y: 0.668825093651
z: -0.417154147527
w: 0.615349828393
-
position:
x: 6.31500005722
y: 5.35500001907
z: 0.495000004768
orientation:
x: -0.0
y: 0.237173257736
z: -0.912005751022
w: 0.334655578046
and etc.
my TF Tree: https://www.dropbox.com/s/4079bf2go1p...
Edit: I decided to attach my full node code here.
ros::Publisher poseArrayPub;
geometry_msgs::PoseArray poseArray;
void
cloud_cb (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>);
pcl::fromROSMsg (*cloud, *cloud2);
poseArray.poses.clear(); // Clear last block perception result
poseArray.header.stamp = ros::Time::now();
poseArray.header.frame_id = "/map";
ROS_INFO_STREAM("poseArray.header: frame=" << poseArray.header.frame_id);
// 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);
// The number assignment in the for loop is not correct.... Can't tell why....
for(size_t i = 0; i<normals->points.size(); ++i)
{
normals->points[i].x = cloud2->points[i].x;
normals->points[i].y = cloud2->points[i].y;
normals->points[i].z = cloud2->points[i].z;
geometry_msgs::PoseStamped pose;
geometry_msgs::Quaternion msg;
// extracting surface normals
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);
//adding pose to pose array
pose.pose.position.x = normals->points[i].x;
pose.pose.position.y = normals->points[i].y;
pose.pose.position.z = normals->points[i].z;
pose.pose.orientation = msg;
poseArray.poses.push_back(pose.pose);
}
poseArrayPub.publish(poseArray);
ROS_INFO("poseArray size: %i", poseArray.poses.size()); //this outputs 92161
}
int main (int argc, char** argv)
{
// Initialize ROS
ros::init (argc, argv, "normal_filter");
ros::NodeHandle nh;
ros::Rate loop_rate(60);
// Create a ROS subscriber for the input point cloud
ros::Subscriber sub = nh.subscribe ("/point_cloud_centers", 1, cloud_cb);
poseArrayPub = nh.advertise<geometry_msgs::PoseArray>("/normal_vectors", 1);
// Spin
ros::spin();
loop_rate.sleep();
}
Edit: the problem is in the numbers, as when I do a straightforward assignment like this: it runs fine.
for (int ...