Ask Your Question
0

Closest point to a fixed frame

asked 2012-08-16 20:41:28 -0500

Astronaut gravatar image

updated 2012-08-17 01:32:38 -0500

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 ...
(more)
edit retag flag offensive close merge delete

Comments

You didn't use the "code"-button (the one with the 10101) or indent your code, so it was not displayed correctly and unreadable. I fixed that.

dornhege gravatar imagedornhege ( 2012-08-17 01:34:47 -0500 )edit

" code button"?? When I edit the code you mean? Any comment to my code??

Astronaut gravatar imageAstronaut ( 2012-08-17 01:38:15 -0500 )edit

Yes, during editing there is a button with something like 101010 on it that will format your posted code to be readable.

dornhege gravatar imagedornhege ( 2012-08-17 02:32:54 -0500 )edit

Ok. Thanks. But I think my code is ok, and doing what I want. I do not understand the comment of michikarg. Why my code is wrong?

Astronaut gravatar imageAstronaut ( 2012-08-17 02:39:16 -0500 )edit

I only changed the display of your code. I have not reviewed the contents.

dornhege gravatar imagedornhege ( 2012-08-17 02:44:47 -0500 )edit

AAh ok. Can you give me some advice or help about the content? Cause I think that my code is ok

Astronaut gravatar imageAstronaut ( 2012-08-17 02:48:10 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2012-08-16 20:59:03 -0500

michikarg gravatar image

You are posting the position of the door in reference to the /odom-frame which ist NOT the position of the robot but the place where the robot started. You should read through the ROS navigation pages where also the frames used for localization and navigation are explained ( http://ros.org/wiki/navigation/ ). Usually you should post the position of the door in reference to the /map-frame and then ask for the transformation between you robot base_link or base-footprint-frame (which you basically did) given your robot's base-frame is /base_link.

edit flag offensive delete link more

Comments

So the the robot start in some point of the map environment. And start moving. I just want to know the distance of the robot in every moment of time ( while he is moving in the map) to that door. I think that what my code is doing, Correct?So what should I change, which part of the code?

Astronaut gravatar imageAstronaut ( 2012-08-16 21:12:33 -0500 )edit

So I change this line in my launch file. <node pkg="tf" type="static_transform_publisher" name="door1" args="1 0 0 0 0 0 /world /door1 . No now the door frame is fixed on the world map frame . So base_link and world (map frame ) are the same now and can be related to some function like distance.

Astronaut gravatar imageAstronaut ( 2012-08-16 21:24:43 -0500 )edit

base_link and the map-frame should NOT be the same... the base_link frame is usually generated by your localization and represents the pose of your robot in the map. The map-frame is generated by your map server and is the origin of your map... Did you read through http://ros.org/wiki/navigation?

michikarg gravatar imagemichikarg ( 2012-08-16 21:32:50 -0500 )edit

cause of that I change the code. <node pkg="tf" type="static_transform_publisher" name="door1" args="1 0 0 0 0 0 /world /door1

Astronaut gravatar imageAstronaut ( 2012-08-16 21:40:45 -0500 )edit

cause of that I change the code. <node pkg="tf" type="static_transform_publisher" name="door1" args="1 0 0 0 0 0 /world /door1

Astronaut gravatar imageAstronaut ( 2012-08-16 21:41:14 -0500 )edit

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",

Astronaut gravatar imageAstronaut ( 2012-08-16 21:44:09 -0500 )edit

I dont see what is wrong. Can you point which part of the code is not correct?

Astronaut gravatar imageAstronaut ( 2012-08-16 21:45:10 -0500 )edit

It seems to me that you are still not familiar with the basics of navigation and tf. While ROS answers is meant to help you, you will still have to understand the concepts and debug your code yourself...

michikarg gravatar imagemichikarg ( 2012-08-16 22:24:13 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2012-08-16 20:41:28 -0500

Seen: 479 times

Last updated: Aug 17 '12