distance from a baselink vector to some fixed frame on the map [closed]

Hello

Im trying to get the distance between my 4 baselink vectors and the fixed frame on the map. I assumed that my robot is a rectangle with dimension 60x90 cm and the base link is in the center of that rectangle. I made the transformation and just want the know the distance between those 4 baselink vectors( vectors between the 4 corners of the rectangle) and some fixed frame (point in the map). I have this code but dont know is I include all details wright and also how to calculate that minimal distance between the 4 vectors and the point in the map.

This is my node

include "geometry_msgs/Quaternion.h"

int main(int argc, char** argv){

ros::init(argc, argv, "my_tf_listener");
ros::NodeHandle node;
tf::TransformListener transform_listener;

while (node.ok())
{
tf::StampedTransform transform;
try{
ros::Time(0), transform);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}

tf::Stamped<tf::Pose> corner1(
tf::Pose(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.30, -0.45, 0.0)),
tf::Stamped<tf::Pose> corner2(
tf::Pose(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.30, 0.45, 0.0)),
tf::Stamped<tf::Pose> corner3(
tf::Pose(tf::Quaternion(0, 0, 0, 1), tf::Vector3(-0.30, -0.45, 0.0)),
tf::Stamped<tf::Pose> corner4(
tf::Pose(tf::Quaternion(0, 0, 0, 1), tf::Vector3(-0.30, 0.45, 0.0)),

tf::Stamped<tf::Pose> transformed_corner_1;
transform_listener.transformPose("map", corner1, transformed_corner_1);
tf::Stamped<tf::Pose> transformed_corner_2;
transform_listener.transformPose("map", corner2, transformed_corner_2);
tf::Stamped<tf::Pose> transformed_corner_3;
transform_listener.transformPose("map", corner3, transformed_corner_3);
tf::Stamped<tf::Pose> transformed_corner_4;
transform_listener.transformPose("map", corner4, transformed_corner_4);

double vect_a = transformed_corner_1.getOrigin();
double vect_b = transformed_corner_2.getOrigin();
double vect_c = transformed_corner_3.getOrigin();
double vect_d = transformed_corner_4.getOrigin();

ROS_INFO(" vect_a =%f", vect_a,   " vect_b =%f", vect_b," vect_c =%f",
vect_c," vect_d =%f", vect_d,);
rate.sleep();

}
return 0;
};


Any help?

Thanks

edit retag reopen merge delete