transform a point from pointcloud

asked 2012-05-28 15:33:22 -0600

53 ●2 ●2 ●9

updated 2012-05-28 15:36:38 -0600

i want to set a goal at the side of a pointcloud. i have the x y z of the point from the pointcloud, but i need to transform it into the right frame. I went through the tf tutorials and I tried with a simple tf listener:

    geometry_msgs::PointStamped theGoal;
    tf::TransformListener listener;
    ros::NodeHandle node;
    ros::Rate rate(10.0);
    while (node.ok()){

     tf::StampedTransform transform;
         try{
            listener.lookupTransform("/move_base_simple/goal", "/camera/rgb/points",  ros::Time(0), transform);
         }
           catch (tf::TransformException ex){
            ROS_ERROR("%s",ex.what());
           } 


      }

but now i dont know how to set the msg for move_base_simple/goal

i also tried with transformPoint, but it only works with geometry_msgs::PointStamped, i tried with transformPointCloud, but then i just get another sensor_msgs/PointCloud2 pointcloud

i just want to transform one point so i can use it to set a goal, i would even multiply it myself if someone would tell me the angles of the difference between the two frames.

i m using ubuntu 10.04, ROS electric, pcl 1.1, robot platform turtlebot

edit retag flag offensive close merge delete

add a comment

5

answered 2012-05-28 15:54:06 -0600

Dan Lazewatsky
9115 ●79 ●103 ●160 http://dan.lazewatsky.com/

updated 2012-05-29 16:17:10 -0600

Get the point out of the point cloud, construct a PointStamped, then transform it using transformPoint. Here's a code snippet that will load the values from your point into a PointStamped, transform that point into target_frame, and put the resulting point into pt_transformed.

geometry_msgs::PointStamped pt;
geometry_msgs::PointStamped pt_transformed;
pt.header = myCloud->header;
pt.point.x = myCloud->points[1].x;
pt.point.y = myCloud->points[1].y;
pt.point.z = myCloud->points[1].z;

tf::TransformListener listener;
listener.transformPoint("target_frame", pt, pt_transformed);

However, when sending a goal, whatever is listening for the goal will likely transform it into whatever frame it needs.

edit flag offensive delete link

Comments

right now i am getting my point by calling : myCloud -> points[1].x and same for y and z. if then i want to set the coordinates for move_base_msgs::MoveBaseGoal goal i need to correct the values myself no?

Jerneja Mislej ( 2012-05-28 23:10:57 -0600 )edit

what do you mean with construct a PointStamped? do I just declare it like this? geometry_msgs::PointStamped theGoal; how can i transform my point with transformPoint if it only takes in geometry_msgs::PointStamped ?

Jerneja Mislej ( 2012-05-28 23:17:29 -0600 )edit

I've edited my answer with some code that will do what I described.

Dan Lazewatsky ( 2012-05-29 16:17:31 -0600 )edit

add a comment

1

answered 2012-06-05 10:36:58 -0600

Jerneja Mislej

53 ●2 ●2 ●9

hi, this solved my problem:

 tf::TransformListener listener;
 tf::StampedTransform transform;
 ros::NodeHandle node;
 const std::string target_frame = "base_link";
 const std::string original_frame = "camera_rgb_optical_frame";
 const ros::Time time = ros::Time(0);
 ros::Rate rate(10.0);
 while (node.ok()){
 try{
    listener.waitForTransform(target_frame, original_frame, time, ros::Duration(10.0));
    listener.lookupTransform(target_frame, original_frame, time, transform);
    break;
  }
 catch (tf::TransformException ex){
      ROS_ERROR("%s",ex.what());
 }
}

point = transform * point;

but when i put together the move to goal code, i realized Dan was right, in the code you can declare what ever frame you need and it works, just have to:

  goal.target_pose.header.frame_id = "camera_rgb_optical_frame";

edit flag offensive delete link

Comments

Hello Jerneja Mislej, Dan Lazewatsky

I am also doing the same thing, transformation from point of point cloud to world coordinate(/map frame). Please provide the solution(code snippet) for the tranformation.

puneet ( 2016-02-12 19:41:31 -0600 )edit

add a comment

0

answered 2012-05-30 02:31:40 -0600

Jerneja Mislej

53 ●2 ●2 ●9

updated 2012-05-30 07:28:56 -0600

thanks for the answer.

i tried like this:

   geometry_msgs::PointStamped pt;
   geometry_msgs::PointStamped pt_transformed;
   pt.header = cloud_cluster->header;
   pt.point.x = point_x;
   pt.point.y = point_y;
   pt.point.z = point_z;

   tf::TransformListener listener;
   const std::string target_frame = "base_link";
   const std::string original_frame = "camera_rgb_optical_frame";
   const ros::Time time = ros::Time::now();
   listener.waitForTransform(target_frame, original_frame, time, ros::Duration(10.0));
   listener.transformPoint(target_frame, time, pt, original_frame, pt_transformed);

but i keep getting that the frame doesnt exist, although is then listed in the list of existing frames. that is the reason i tried with waitForTRansform, but it doesnt seem to take notice of it.

i tried with different frames, ros times, but nothing seems to work.

i tried to change the code to :

    tf::TransformListener listener;
    tf::StampedTransform transform;
    const std::string target_frame = "base_link";
    const std::string original_frame = "camera_rgb_optical_frame";
    const ros::Time time = ros::Time::now();
    listener.waitForTransform(target_frame, original_frame, time, ros::Duration(10.0));
    listener.lookupTransform(target_frame, original_frame, time, transform);


    goal_point_x=transform * point_x;
    goal_point_y=transform * point_y;
    goal_point_z=transform * point_z;

but then it says that the two frames are not connected

i also tried transforming the pointcloud resized to one point, but it also says that the frames dont exist, the code:

       cloud_cluster -> points.resize(1);

       tf::TransformListener listener;
       sensor_msgs::PointCloud2 pointCloud2;
       pcl::toROSMsg (*cloud_cluster, pointCloud2);
       sensor_msgs::PointCloud pointCloud;
       sensor_msgs::PointCloud pointCloudInFrame;
       sensor_msgs::convertPointCloud2ToPointCloud(pointCloud2,pointCloud);


       listener.waitForTransform("map", "camera_rgb_optical_frame", ros::Time(0), ros::Duration(50.0));
        listener.transformPointCloud("map",ros::Time(0),pointCloud,"camera_rgb_optical_frame", pointCloudInFrame);
       sensor_msgs::PointCloud2 pointCloud2InFrame;
      sensor_msgs::convertPointCloudToPointCloud2(pointCloudInFrame,pointCloud2InFrame);
       pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster_in_frame (new pcl::PointCloud<pcl::PointXYZRGB>);
       pcl::fromROSMsg(pointCloud2InFrame,*cloud_cluster_in_frame);

edit flag offensive delete link

add a comment

transform a point from pointcloud

3 Answers

Comments

Comments

Question Tools

Stats

Related questions

transform a point from pointcloud edit

3 Answers

Comments

Comments

Question Tools

Stats

Related questions

transform a point from pointcloud