Ask Your Question

position and orientation of the vector

asked 2012-08-21 14:39:30 -0500

Astronaut gravatar image

updated 2012-08-22 09:52:29 -0500


I went through tf tutorial and some questions here . I assume that my robot is an rectangle so want to know the position of each 4 vectors of my rectangle.

In my code to get the distance between the fixed point in the map and the center of the robot, center of the base link ( which is the centar of that rectangle I did not used a pose. I used this code. And think is ok.

while (node.ok()){
    tf::StampedTransform transform;
      listener.lookupTransform("/base_link", "/door1",  
                               ros::Time(0), transform);
    catch (tf::TransformException ex){

    double x = transform.getOrigin().x();
    double y = transform.getOrigin().y();
    double dist = sqrt(x*x + y*y);

So , but I need to calculate the distance between the 4 vectors (which are formed by the 4 corner points of the rectangle) to that fixed point. So how to get that vectors fist, with the help of my previous code and than the distance.


edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted

answered 2012-08-22 09:51:56 -0500

updated 2012-08-23 06:05:15 -0500

The best way to solve your problem is to use the transformation directly itself. You know the transformation between base_link and door1 in your code, so you can specify the points in the base_link and transform them into the door1 coordinate frame (untested code below)

EDIT: Didn't realize you needed just the distances. Distances can be computed in either frame of reference. In the example below we'll use the door1 frame of reference.

tf::StampedTransform transform; 
try { 
  listener.lookupTransform("/base_link", "/door1", ros::Time(0), transform); 
} catch (tf::TransformException ex){ 

tf::Vector3 pt1(robot_length/2, robot_breadth/2, 0);
tf::Vector3 pt2(robot_length/2, -robot_breadth/2, 0);
tf::Vector3 pt3(-robot_length/2, -robot_breadth/2, 0);
tf::Vector3 pt4(-robot_length/2, robot_breadth/2, 0);

tf::Transform tf_base_link_to_door = transform.Inverse();

tf::Vector3 door1_pt(0, 0, 0);
tf::Vector3 required_pt1 = tf_base_link_to_door * pt1;
float dist_pt1 = required_pt1.distance(door1_pt);
tf::Vector3 required_pt2 = tf_base_link_to_door * pt2;
float dist_pt2 = required_pt2.distance(door1_pt);
tf::Vector3 required_pt3 = tf_base_link_to_door * pt3;
float dist_pt3 = required_pt3.distance(door1_pt);
tf::Vector3 required_pt4 = tf_base_link_to_door * pt4;
float dist_pt4 = required_pt4.distance(door1_pt);
edit flag offensive delete link more


Sorry but where you calculate the distance here???

Astronaut gravatar imageAstronaut ( 2012-08-22 16:23:11 -0500 )edit

@Astronaut: see updated answer

piyushk gravatar imagepiyushk ( 2012-08-23 06:05:54 -0500 )edit

Yes, I need the distances between the four vectors( create from the 4 corners of the rectangle) and the fixed object(door) in the map. So in this case the four distances are dist_pt1, dist_pt2, dist_pt3 and dist_pt4, wright??

Astronaut gravatar imageAstronaut ( 2012-08-23 14:40:24 -0500 )edit

But dont understand. The door has fixed coordinate in the map frame, for example (1,1,0). So what means here tf::Vector3 door1_pt(0, 0, 0); ??? float dist_pt1..4 gives me the distance between that door in the map and the robot (robot is moving in the map). Yes?

Astronaut gravatar imageAstronaut ( 2012-08-23 15:12:59 -0500 )edit

@Astronaut: The transform from base link to door converts all points to a frame of reference that originates at door1, hence I've used 0,0,0 in the code above. And yes, the distance calculated should be what you need. I recommend reading a bit more about tf at

piyushk gravatar imagepiyushk ( 2012-08-23 18:20:24 -0500 )edit

Ok. Understand But need the distance between the line segment (formed by the 4 corners of the recatngle ) and the door. So that distances are dist_pt1, dist_pt2, dist_pt3 and dist_pt4 from the line segment to the door yes??

Astronaut gravatar imageAstronaut ( 2012-08-23 18:44:06 -0500 )edit

@Astronaut: The distances are to the 4 corner that make up the robot, if that is what you mean by line segments.

piyushk gravatar imagepiyushk ( 2012-08-24 06:45:09 -0500 )edit

I mean i need the distance between the door( a fixed frame in the map) and the 4 line segments (made by the 4 corners of the robot). So i have to know all those 4 distances and find the minimum between all 4 . Understand?

Astronaut gravatar imageAstronaut ( 2012-08-24 14:27:04 -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


Asked: 2012-08-21 14:39:30 -0500

Seen: 1,559 times

Last updated: Aug 23 '12