Checking a goal is in a valid location in a costmap

edit

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 navigation
• 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();
    }
}