# Checking a goal is in a valid location in a costmap

Hello, I have been attempting to write a ros package external of move_base and costmap2d that checks the feasibility of random points being goals for a robot. What I am doing is importing costmap_2d and Costmap2DROS to attempt to get a pointer to the /move_base/global_costmap with the getCostmap() function. Then I input pseudo random x y coordinates into the getCost() function and then evaluate that points viability. My platform is running 16.04 with kinetic. The process of launching is as so:

• Roscore
• Bring up the robot
• Launch my goal checking node.

The main issue is that I get stuck requesting the map. Before now I had to play games with remapping topics and getting the node into the correct namespace for subscribing to the global costmap topic and it seems rather hacky to me. I think I need to make all of this into a plugin for costmap_2d but ideally I want it to work how it is now as I haven't done that before. If anyone can answer some of these questions I would be elated.

• What do you think I should do?
• Is my approach just wrong?
• Do I need to build a plugin?
• How can I get the nampespaces to work better together?
• Why is it hanging at the request?

Launch File:

<launch>
<!-- map checker -->
<!-- highly questionable namespace usage -->
<group ns="/move_base">
<node pkg="investigator" name="map_check" type="map_check" output="screen">
<!-- ?I shouldn't have to do this? -->
<remap from="/move_base/map_check/map" to="/move_base/global_costmap"/>
</node>
</group>
</launch>


Output:

started roslaunch server http://198.43.32.9:38759/

SUMMARY
========

PARAMETERS
* /rosdistro: kinetic
* /rosversion: 1.12.14

NODES
/move_base/
map_check (investigator/map_check)

ROS_MASTER_URI=http://198.43.32.9:11311

process[move_base/map_check-1]: started with pid [21613]
[ INFO] [1585756791.433410051]: Using plugin "static_layer"
[ INFO] [1585756791.446673931]: Requesting the map...
^C[move_base/map_check-1] killing on exit


Code:

#include "ros/ros.h"
#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_2d/costmap_2d.h>
#include <tf/transform_listener.h>
#include <iostream>
#include <stdlib.h>
#include <time.h>
//bool check_point() {
//}
int main(int argc, char **argv) {
ros::init(argc, argv, "map_check");
ros::NodeHandle node;
std::string map_name = "/global_costmap"; //Is this wrong remap should overide this
tf::TransformListener tf;
costmap_2d::Costmap2D* map;
costmap_2d::Costmap2DROS *costmap = new costmap_2d::Costmap2DROS(map_name, tf); //Wrong?
map = costmap->getCostmap();
srand(time(NULL));
ros::Rate loop_rate(2);//giving it time to check
int cost;
while (ros::ok()) {
int x = rand() % 5;
int y = rand() % 5;
cost = map->getCost(x,y);
std::cout<<"Point: ("<<x<<", "<<y<<") Cost: "<<cost<<"\n";
loop_rate.sleep();
}
}

edit retag close merge delete

Iam wondering where you load the map of the environment. The costmaps are abstractions of the map usually served by a map_server. No map should equals to empty/worhtless costmaps or maybe even some sort of error. Iam not sure what my ros output shows but "requesting map..." seems to be the last point in your startup run, I believe to remember that it should remark something about a "found/served/loaded map", and some map data before continuing. I dont see any map serving node in the launchfile either. move_base seem to require a map for its global planner/global_costmap link text

( 2020-04-04 05:37:40 -0500 )edit
1

Just wondering if you have seen the make_plan service of move_base which could be used to check for valid goals. For your problem: Why don't you just subscribe to the "/move_base/global_costmap/costmap"-topic to read the map? This should avoid any hangs you have and is more ROS-like.

( 2020-04-05 02:03:58 -0500 )edit

So what happened here is that the map is subscribed through a service, where you generate a pointer to the costmap object. I wish it was as easy as subscribing but if you want to access the costmap2d functions and use them with the costmaps generated by movebase you have to do these operations in movebase itself. This is because the costmap2dRos object instantiates with the init call and uses whatever you provide there as the name for your node as the namespace for the map name you provide. So in short by declaring the node as map_checker and giving a map name of /global_costmap I automatically subscribe to /map_checker/global_costmap. In my launch file you can see my hacky attempts to remap and force a subscription to the correct map.

I will definitely check out the make_plan service It looks like it has a path as an output Is ...(more)

( 2020-04-06 09:21:36 -0500 )edit
1

I just wanted to add my 2 cents here as I have done what you are attempting to do. I believe what you would want to look at is footprintCost from base_local_planner, which will check to see if the robot (from the robot's footprint which you can get from your costmap pointer) can be at a certain point without hitting an obstacle.

( 2020-04-16 11:27:12 -0500 )edit

Thanks for the heads up, this approach seems like it would work great for what I am doing.

( 2020-04-17 15:47:38 -0500 )edit

Sort by » oldest newest most voted

I ended up using make_plan. I spent some time getting get cost too work and eventually it did but not well. I then used make plan and it is a much better way to do this. Get_cost failed due to the fact it would test points outside of the map causing a core dump. To get around this I instead used getPlan a nav_msgs service and set track_unknown_space in costmap common params to true and allow_unknown to false in navfn params.

more