Closest point to a fixed frame

edit

Hello

Im trying to write a node that gives me the distance to a fixed frame of some object in my map. So for example a door. So I just want to analyse the position of the robot (based on laser scan) in the relation to this object. So I passed the TF tutorial. But not sure if the code is Ok. Any help?

So this is my code

#include <ros/ros.h>
#include <tf/transform_listener.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf_listener");

  ros::NodeHandle node;
  tf::TransformListener listener;

  ros::Rate rate(10.0);
  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);
    ROS_INFO("%f", dist);

    rate.sleep();
  }
  return 0;
};

And just labelled the door on my launch file like this

<node pkg="tf" type="static_transform_publisher" name="door1" args="1 0 0 0 0 0 /odom /door1 10"/>

So I think this part of the code give me the the distance to the door. Cause my laser is placed on the base_link. So it give me the distance between the door and the base link in every time step

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);
    ROS_INFO("%f", dist);

    rate.sleep();

Yes??? This is the visualisation code

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "geometry_msgs/Pose.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "tf/message_filter.h"
#include "message_filters/subscriber.h"
#include "visualization_msgs/Marker.h"
/**
 * This tutorial demonstrates simple receipt of messages over the ROS system.
 */


class amcl_pose_listener {

public:

  ros::NodeHandle n_;

  //Message filters for subscribed sensor msgs

  message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped> pose_sub_;  //Subscriber to a published pose msg (from a localiser such as AMCL, etc)

  ros::Publisher traj_pub_, line_to_door_pub_;  //Publish an trajectory

  //The trajectory
  visualization_msgs::Marker trajectory_points_, line_to_door_points_;

  amcl_pose_listener(ros::NodeHandle n): 

  pose_sub_(n_, "amcl_pose", 100)       //Subscriber to a pose msg "amcl_pose" (from AMCL)
  {    
    //Set call back function for pose data
    pose_sub_.registerCallback(boost::bind(&amcl_pose_listener::poseCallBack, this, _1));

    //Publish a trajectory
    traj_pub_ = n.advertise<visualization_msgs::Marker>("robot_fixed_pose_trajectory", 100);
    line_to_door_pub_ = n.advertise<visualization_msgs::Marker>("wheelchair_to_door_trajectory", 100);

    line_to_door_points_.points.resize(2);

  }

  //Pose data callback
  void poseCallBack(const geometry_msgs::PoseWithCovarianceStampedConstPtr& data_in)
  {
    //Init the trajectory history
    trajectory_points_.header.frame_id = "map";
    trajectory_points_.header.stamp = ros::Time::now();
    trajectory_points_.ns = "robot_fixed_pose_trajectory";
    trajectory_points_.type = visualization_msgs::Marker::LINE_STRIP;
    trajectory_points_.action = visualization_msgs::Marker::ADD;
    trajectory_points_.id = 0;
    //Green colour
    trajectory_points_.color.g = 1.0f;
    trajectory_points_.color.a = 1.0;
    trajectory_points_.scale.x = 0.1;   //Sets the width of the LINE_STRIP
    trajectory_points_.scale.y = 0.1;   //Ignored if marker type is LINE_STRIP

    //Init the trajectory history
    line_to_door_points_.header.frame_id = "map";
    line_to_door_points_.header.stamp = ros::Time::now();
    line_to_door_points_.ns = "line_to_door";
    line_to_door_points_.type = visualization_msgs::Marker::LINE_STRIP;
    line_to_door_points_.action = visualization_msgs::Marker::MODIFY ...