# transform a point from pointcloud

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 close merge delete

Sort by » oldest newest most voted

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

more

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?

( 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 ?

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

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

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

hi, this solved my problem:

 tf::TransformListener listener;
tf::StampedTransform transform;
ros::NodeHandle node;
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";

more

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.

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

i tried like this:

   geometry_msgs::PointStamped pt;
geometry_msgs::PointStamped pt_transformed;
pt.point.x = point_x;
pt.point.y = point_y;
pt.point.z = point_z;

tf::TransformListener listener;
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 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);

more