Revision history [back]

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)

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 required_pt1 = tf_base_link_to_door * pt1;
tf::Vector3 required_pt2 = tf_base_link_to_door * pt2;
tf::Vector3 required_pt3 = tf_base_link_to_door * pt3;
tf::Vector3 required_pt4 = tf_base_link_to_door * pt4;

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);