position and orientation of the vector

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

Astronaut
302 ●177 ●206 ●214

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

piyushk
2861 ●31 ●49 ●76 http://cs.utexas.edu/~piyushk

Hello

I went through tf tutorial and some questions here http://answers.ros.org/question/39250/getting-the-position-and-the-pose-of-robot-using-tf_listener/) . 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;
    try{
      listener.lookupTransform("/base_link", "/door1",  
                               ros::Time(0), transform);
    }
    catch (tf::TransformException ex){
      ROS_ERROR("%s",ex.what());
    }

    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.

Thanks.

edit retag flag offensive close merge delete

add a comment

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

piyushk
2861 ●31 ●49 ●76 http://cs.utexas.edu/~piyushk

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){ 
  ROS_ERROR("%s",ex.what()); 
}

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

Comments

Sorry but where you calculate the distance here???

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

@Astronaut: see updated answer

piyushk ( 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 ( 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 ( 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 http://www.ros.org/wiki/tf

piyushk ( 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 ( 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 ( 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 ( 2012-08-24 14:27:04 -0500 )edit

see more comments

position and orientation of the vector

1 Answer

Comments

Question Tools

Stats

Related questions

position and orientation of the vector edit

1 Answer

Comments

Question Tools

Stats

Related questions

position and orientation of the vector