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

asked 2012-08-22 20:26:35 -0500

Astronaut gravatar image

updated 2014-01-28 17:13:25 -0500

ngrennan gravatar image


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 <ros ros.h="">

include <tf transform_listener.h="">

include "std_msgs/String.h"

include <vector>

include <ros package.h="">

include "geometry_msgs/PoseWithCovarianceStamped.h"

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;
                    listener.lookupTransform("/base_link", "/door1",  
                                ros::Time(0), transform);
                    catch (tf::TransformException ex){

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

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

     return 0;

Any help?


edit retag flag offensive reopen merge delete

Closed for the following reason question is not relevant or outdated by tfoote
close date 2015-04-07 00:09:10.054207