Ask Your Question
1

PoseArray receiving data, not displaying in RViz

asked 2014-08-05 14:55:37 -0500

xuningy gravatar image

updated 2014-08-06 15:01:00 -0500

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 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
2

answered 2014-08-05 16:00:37 -0500

ahubers gravatar image

updated 2014-08-06 13:20:20 -0500

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()
edit flag offensive delete link more

Comments

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?

xuningy gravatar imagexuningy ( 2014-08-05 17:24:42 -0500 )edit

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")?

ahubers gravatar imageahubers ( 2014-08-05 17:31:36 -0500 )edit

I've added the topic results and frames.pdf to my original post. Let me know what else might help! Thanks!

xuningy gravatar imagexuningy ( 2014-08-06 08:27:55 -0500 )edit

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?

xuningy gravatar imagexuningy ( 2014-08-06 14:15:00 -0500 )edit

Added full code above.

xuningy gravatar imagexuningy ( 2014-08-06 14:36:23 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2014-08-05 14:55:37 -0500

Seen: 4,402 times

Last updated: Aug 06 '14