Ask Your Question
0

Read costmap values from code

asked 2021-01-30 03:26:45 -0500

bach gravatar image

updated 2021-01-30 03:57:55 -0500

Hi,

I would like to read the values from the costmap generated by the robot localization system. This is what I'm doing now:

tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tf2_listener(tfBuffer);
costmap_2d::Costmap2DROS costmap_ros("local_costmap", tfBuffer);
costmap_2d::Costmap2D *costmap;


costmap = costmap_ros.getCostmap();

ROS_INFO("OK!");

unsigned int map_x1, map_x2, map_y1, map_y2, index;
double cost1, cost2;

costmap->worldToMap(STATIONS[0][0], STATIONS[0][1], map_x1, map_y1);
costmap->worldToMap(STATIONS[1][0], STATIONS[1][1], map_x2, map_y2);

cost1 = double(costmap->getCost(map_x1, map_y1));
cost2 = double(costmap->getCost(map_x2, map_y2));

But I get this error:

[ WARN] [1611998001.185217756, 4226.344000000]: Timed out waiting for transform from base_link to map to become available before running costmap, tf error: canTransform: target_frame map does not exist.. canTransform returned after 0.1 timeout was 0.1.

I search a lot, but without success. I see the global and local costmaps in RVIZ, but I don't know how to access their values from code.

Can someone help me please?

[EDIT]

I discovered in the source code of the constructor of Costmap2DROS these lines:

    // get our tf prefix
ros::NodeHandle prefix_nh;
std::string tf_prefix = tf::getPrefixParam(prefix_nh);

// get two frames
private_nh.param("global_frame", global_frame_, std::string("/map"));
private_nh.param("robot_base_frame", robot_base_frame_, std::string("base_link"));

// make sure that we set the frames appropriately based on the tf_prefix
global_frame_ = tf::resolve(tf_prefix, global_frame_);
robot_base_frame_ = tf::resolve(tf_prefix, robot_base_frame_);

ros::Time last_error = ros::Time::now();
std::string tf_error;
// we need to make sure that the transform between the robot base frame and the global frame is available
while (ros::ok() && !tf_.waitForTransform(global_frame_, robot_base_frame_, ros::Time(), ros::Duration(0.1), ros::Duration(0.01),&tf_error))

It seems the two frames to be trasformed are hard coded... They does not exist in my framework.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2021-05-02 13:07:09 -0500

updated 2021-05-11 08:05:05 -0500

You are getting tf error: canTransform: target_frame map does not exist.. canTransform returned after 0.1 timeout was 0.1 error because the transform between map -> base_link doesn't exist. You can run rosrun tf view_frames followed by evince frames.pdf to visualize your tf tree.

costmap_ros uses map and base_link as the default values for the global_frame and robot_base_frame parameters respectively.

Are you trying to localize the bot in the odom frame or map frame?

If you are trying to localize the bot in the odom frame, then you need to set the global_frame parameter to odom.

In case, you are trying to localize the bot in the map frame, then you need to make sure that the map -> base_link transform is being published. You can use gmapping to achieve that.

Also, make sure that the robot_base_frame parameter is set to base_link.

I hope this helps.

edit flag offensive delete link more

Your Answer

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

Add Answer

Question Tools

1 follower

Stats

Asked: 2021-01-30 03:26:45 -0500

Seen: 115 times

Last updated: May 11