move_base/global_costmap/costmap subscriber not receiving data (but RVIZ and rostopic do)

asked 2022-12-21 11:00:10 -0500

zebfour gravatar image

updated 2022-12-26 10:17:47 -0500

Mike Scheutzow gravatar image

Hello,

I am experiencing a problem with ROS Melodic (Ubuntu 18.04, x86):

I am have installed the Tiago packages and I am moving the robot in a simulated environment using the move_base actions. I am trying to obtain the OccupancyGrid message published by the topic move_base/global_costmap/costmap but my subscriber does not seem to be working (I have coded a ROS_INFO call inside the callback and the message never shows up).

In particular, I have written a map_saver class, which has a method save_map() I use to subscribe to the topic and save the OccupancyGrid inside a member variable. I need the map to be saved before I can go on with my computations, but in practice the map is never saved (I never exit the while loop in the main reported below).

Here is the code of the main():

int main(int argc, char** argv) {
    ros::init(argc, argv, "map_elab");
    ros::NodeHandle n;
    ros::Rate r(10);

    map_saver ms(n);
    ROS_INFO("map_saver object instantiated");
    ms.save_map();
    while (!ms.map_is_available() && ros::ok()) { //NEVER EXITING THIS LOOP
        r.sleep();
        ros::spinOnce;
    }
    //---once the map is saved, do something with it---

Here is the skeleton of the map_saver class:

class map_saver {
    private:
    ros::NodeHandle _n;
    nav_msgs::OccupancyGrid _global_map;
    ros::Subscriber _map_subscriber;
    bool _map_is_saved = false;

    public:
    map_saver(ros::NodeHandle n)
        :_n(n) {}
    //...
    void save_map()
    {
        /* STEP 1: move to a waypoint at the end of the corridor using move_base*/
        //...
        /* STEP 2: subscribe to topic /move_base/global_map and save the global map */
        _map_subscriber = _n.subscribe("move_base/global_costmap/costmap", 5, &map_saver::save_map_callback, this);
    }
    void save_map_callback(const nav_msgs::OccupancyGrid::ConstPtr& msg) 
    {
        //we need to make sure the callback is called exactly once!
        ROS_INFO("save_map callback is saving the map...");
        _global_map = *msg;
        _map_subscriber.shutdown();
        _map_is_saved = true;
        ROS_INFO("save_map callback deactivated. Map is saved.");
    }
    const nav_msgs::OccupancyGrid& get_map()
    {return _global_map;}

    bool map_is_available()
    {return _map_is_saved;};

I have no clue as why this is happening, since both RViz and rostopic echo return a proper OccupancyGrid object. What am I getting wrong?

edit retag flag offensive close merge delete

Comments

The code could be stalling in STEP 1 if your ros node is not spinning; I would add a ROS_INFO to see if it even reaches the subscribe statement. If the subscribe is being called, use rostopic info /move_base/global_costmap/costmap to see if your ros node is listed as being subscribed.

Mike Scheutzow gravatar image Mike Scheutzow  ( 2022-12-22 07:19:33 -0500 )edit

FYI, It's not great practice to shut down the subscriber from within its own callback. A better alternative is to shut it down from within map_is_available() right before it returns true

kacaroll gravatar image kacaroll  ( 2022-12-22 16:11:31 -0500 )edit

@mike-scheutzow thanks for your advice but the node is indeeed subscribed @kacaroll why is that?

zebfour gravatar image zebfour  ( 2022-12-26 09:42:00 -0500 )edit

I abandoned that design and started over with a new one but I keep experiencing similar problems (and/or inexplicable out of memory exceptions) when instancing nodes from within classes... does ROS behave correctly with OOP or is it something I should pay attention to?

zebfour gravatar image zebfour  ( 2022-12-26 09:44:52 -0500 )edit

While roscpp is largely OOP, there are some limitations e.g. you can only call ros::init() once. If you compile the code you show us, does it work? I suspect it will work, which means it is some other code that is causing the problem. We can't debug code you don't show us.

Mike Scheutzow gravatar image Mike Scheutzow  ( 2022-12-26 10:15:23 -0500 )edit

thank you Mike Scheutzow. I have published a new question because my code has changed in a relevant way since then

zebfour gravatar image zebfour  ( 2022-12-28 04:24:48 -0500 )edit