Hello,

I posted my issues some days ago, but while trying to solve them I encountered a problem different enough to deserve its own topic.

I'm working on Melodic (Ubuntu 18.04, x86) and I have written the following code, which will work on TIAGo in a simulated environment. Please don't question the logic behind what I'm doing: this is just a somewhat minimal test to ask if I'm getting something wrong about the basics of ROS and/or the use of OOP while dealing with ROS.

#include <string>
#include <vector>
#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <tf/transform_listener.h>
#include <tf/message_filter.h>
#include <message_filters/subscriber.h>

#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud.h>

#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Point32.h>

#include <laser_geometry/laser_geometry.h> //from laserscan to pointcloud

#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/GetMap.h>

#include <move_base_msgs/MoveBaseAction.h>
/**
* This class saves the map and returns it.
*/
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)
    {}
    /**
    * Move to position (hardcoded) at the end of the corridor, then save the occupancy grid.
    */
    void save_map()
    {
        /* STEP 1: move to a waypoint at the end of the corridor */
        actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> ac("move_base", true);
        //wait for the spawn of the action server
        while(!ac.waitForServer(ros::Duration(5.0))){
            ROS_INFO("Waiting for the move_base action server to come up");
        }
        move_base_msgs::MoveBaseGoal goal;
        goal.target_pose.header.frame_id = "map";
        goal.target_pose.header.stamp = ros::Time::now();
        //use hardcoded pose
        goal.target_pose.pose.position.x = 8; //test it!!
        goal.target_pose.pose.orientation.w = 0.375;

        ROS_INFO("save_map is trying to reach the waypoint");
        ac.sendGoal(goal);
        ac.waitForResult();
        if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
            ROS_INFO("save_map reached waypoint");
        else {
            ROS_ERROR("save_map could not reach the waypoint, aborting...");
            ros::shutdown();
        } 
        /* STEP 2: obtain the map from the service static_map */

        ros::service::waitForService("static_map");
        nav_msgs::GetMap srv;
        ros::ServiceClient client = _n.serviceClient<nav_msgs::GetMap>("static_map");
        if(client.call(srv)) 
        {
            _global_map = srv.response.map;
            _map_is_saved = true;
            ROS_INFO("map obtained from GetMap service");
        }
        else
        {
            ROS_ERROR("could not get map from GetMap service");
        }
    }

    const nav_msgs::OccupancyGrid& get_map()
    {return _global_map;}

    bool map_is_available()
    {return _map_is_saved;};

};

/**
* This class is responsible for listening to transforms and waiting for pointcloud data to be published.
* The callback is used to update the pointcloud member variable
* 
*/
class pointcloud_subscriber
{
    private:
    ros::NodeHandle _n;
    sensor_msgs::PointCloud _pointcloud;
    laser_geometry::LaserProjection _projector;
    tf::TransformListener _listener;
    message_filters::Subscriber<sensor_msgs::LaserScan> _laser_sub;
    tf::MessageFilter<sensor_msgs::LaserScan> _laser_notifier;

    const std::string LASERSCAN_TOPIC = "scan_raw";
    const std::string ABS_FRAME = "map";
    const std::string LASER_FRAME = "base_laser_link";

    public:
    pointcloud_subscriber(ros::NodeHandle n) 
        :_n(n), 
        _laser_sub(_n, LASERSCAN_TOPIC, 5),
        _laser_notifier(_laser_sub,_listener, ABS_FRAME, 5)
    {
        ROS_INFO("entering pointcloud_subscriber constructor...");
        _laser_notifier.registerCallback(boost::bind(&pointcloud_subscriber::save_pointcloud, this, _1));
        _laser_notifier.setTolerance(ros::Duration(0.01));
    }
    void save_pointcloud(const sensor_msgs::LaserScan::ConstPtr& msg)
    {   
        //obtain the laserscan & turn it into a PointCloud in the map reference frame
        sensor_msgs::PointCloud p;
        ROS_INFO("Starting laserscan transform...");
        try{
            _projector.transformLaserScanToPointCloud(ABS_FRAME, *msg, p, _listener, msg->range_max);
        }
        catch(tf2::TransformException& e) {
            ROS_ERROR("%s", e.what());
            sleep(1);
        }
        this->_pointcloud = p;
    }
    sensor_msgs::PointCloud get_latest_pointcloud() {return _pointcloud;}
};


int main(int argc, char** argv) {
    ros::init(argc, argv, "tiago_move_server");
    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()) {
        r.sleep();
        ros::spinOnce;
    }
    //---once the map is saved, we may elaborate the laserscans based on it---

    ROS_INFO("map is saved, now starting the filtering of the laserscan");

    pointcloud_subscriber ps(n); //std::bad_alloc??????
    ROS_INFO("subscriber created");

    /* use the subscriber...*/
    return 0;
}

What I'm trying to do is summarized as follows:

• Move in a predefined pose
• Instantiate a map_saver object and use it to call a service and save the static map from that pose
• Instantiate a pointcloud_subscriber object which subscribes to the laserscan of TIAGo and dynamically transforms it into a pointcloud

Then I should use the pointcloud_subscriber object to obtain the pointcloud member variable and perform some elaborations on it.

As I said in the title, testing this code in my setup results in either

...
[ INFO] [1672220974.314407375, 4309.922000000]: map is saved, now starting the filtering of the laserscan
terminate called after throwing an instance of 'std::bad_alloc'
  what():  std::bad_alloc
Aborted (core dumped)

or

[ INFO] [1672220977.194391197, 4311.343000000]: map is saved, now starting the filtering of the laserscan
terminate called after throwing an instance of 'std::length_error'
  what():  basic_string::_M_create
Aborted (core dumped)

which I think are originated in the call to the pointcloud_subscriber constructor. To be clear, I do have enough RAM for my task in my setup (I have about 6 GB free prior to starting the node; the map is a 1984x1984 OccupancyGrid and each laserscan has about 500 readings).

I have already tested the map service and the laserscan topic separately using rosservice and rostopic. I have also experimented with passing the nodeHandle by reference instead of by copy but the result is the same.

Could you point out any design flaw you see in my code? I suspect this problem has to do with how ROS manages objects and object members, but I'm honestly very puzzled...