0
$\begingroup$

Rosanswers logo

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/4079bf2go1po59k/frames.pdf

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 i = 0; i < 3; ++i) {
    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = 0.0;
    pose.pose.position.y = 0.0;
    pose.pose.position.z = i;
    pose.pose.orientation.x = 0.0;
    pose.pose.orientation.y = 0.0;
    pose.pose.orientation.z = 0.0;
    pose.pose.orientation.w = 1.0;
    poseArray.poses.push_back(pose.pose);
  }

Edit: It's the number of messages im sending at once. The algorithm loops through the entire pc, which contains over 90,000 points. When I set the max limit to half of the point cloud, it works fine. There may be a limit...


Originally posted by xuningy on ROS Answers with karma: 101 on 2014-08-05

Post score: 1

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

poseArray.header.frame_id = "/map" will only work if the /map frame exists, i.e, if you are localizing your device/robot to some /map frame. poseArray.header.frame_id actually represents from where this pose is measured. So if I want to publish a pose five meters in front of my robot's head (which is indicated by a frame_id of "camera_rgb_optical_frame), I would send a pose with position.z = 5.0 and set poseArray.header.frame_id = "/camera_rgb_optical_frame".

Try setting the frame_id to to a frame you know exists, and also setting the fixed frame to this as well... Can't really say much more without more info (what frames exists, are you using a robot/device, ...)

EDIT: I tried running this code and it worked for me, with only running openni_launch openni.launch on a kinect plugged into my computer. Perhaps you can give it a shot, or change poseArray.header.frame_id = "/camera_rgb_optical_frame":

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import PoseArray, Pose

if __name__ == '__main__':
    rospy.init_node('pose_array')
    r = rospy.Rate(60.0)
    pub = rospy.Publisher("/poseArrayTopic", PoseArray)
    while not rospy.is_shutdown():
        poseArray = PoseArray()
        poseArray.header.stamp = rospy.Time.now()
        poseArray.header.frame_id = "/camera_rgb_optical_frame"
        for i in range(1, 5):
            somePose = Pose()
            somePose.position.x = 0.0
            somePose.position.y = 0.0
            somePose.position.z = i

            somePose.orientation.x = 0.0
            somePose.orientation.y = 0.0
            somePose.orientation.z = 0.0
            somePose.orientation.w = 1.0

            poseArray.poses.append(somePose)

        pub.publish(poseArray)
        r.sleep()

Originally posted by ahubers with karma: 301 on 2014-08-05

This answer was ACCEPTED on the original site

Post score: 2


Original comments

Comment by xuningy on 2014-08-05:
Hi, thanks for the answer. To no avail, I have tried it with a known, published "/map" (my fixed frame), which I can see from my /tf tree. The poses are not coming from a sensor source. They are set by a set of calculations by code, which is displayed (correctly) using "rostopic echo /poseArrayTopic" as mentioned in the original post. It is not displaying in RViz.

Could there be other reasons?

Comment by ahubers on 2014-08-05:
Can you post a snippet of your results from "rostopic echo /poseArrayTopic" and also maybe a screenshot of your tf tree (from running "rosrun tf view_frames", then "evince frames.pdf")?

Comment by xuningy on 2014-08-06:
I've added the topic results and frames.pdf to my original post. Let me know what else might help! Thanks!

Comment by xuningy on 2014-08-06:
So I've tried your script in your edit, it works perfectly for me also. Though one thing to notice is that in RViz, you can see (in the display panels) that you are receiving messages. I do not have that displaying in my RViz for my code. A second follow up question is, I have the PoseArray as a separate node than my main node, which provides 2 sources of information: a point cloud, and the fixed frame. Could this have contributed? should I move the entire piece of code into my main node?

Comment by xuningy on 2014-08-06:
Added full code above.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.