0
$\begingroup$

I am getting a bad_alloc error when I try to use the costmap_2d::Costmap2DROS::getRobotPose function inside the makePlan function. Interestingly, if I call the same function from inside the initialize function (instead from inside the makePlan function), no error pops up.

I am attaching the relevant functions from my source code file.

#include <pluginlib/class_list_macros.h>
#include <my_global_planner/global_planner.h>
#include <tf/tf.h>
#include <queue>
#include<math.h>
#include <cstdlib>
#include <ctime>
#include<visualization_msgs/Marker.h>

//register this planner as a BaseGlobalPlanner plugin
PLUGINLIB_EXPORT_CLASS(global_planner::GlobalPlanner, nav_core::BaseGlobalPlanner)

using namespace std;

namespace global_planner {

  GlobalPlanner::GlobalPlanner (){

  }

  GlobalPlanner::GlobalPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
    initialize(name, costmap_ros);

  }

  void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){

    nh_ = ros::NodeHandle{"~abcd"};
    
    costmap_ros  = costmap_ros;
    costmap_ros_ = costmap_ros->getCostmap(); 

    size_x = costmap_ros_->getSizeInCellsX(); 
    size_y = costmap_ros_->getSizeInCellsY();

    geometry_msgs::PoseStamped global_pose_;
    costmap_ros->getRobotPose(global_pose_);

    cout << "global_pose_.x: " << global_pose_.pose.position.x << " global_pose_.y: " << global_pose_.pose.position.y << endl;

    cout << "Sleeping for 3 seconds!" << endl;
    ros::Duration(3.0).sleep();

    cout << "global_frame: " << costmap_ros->getGlobalFrameID() << endl;

    global_plan_pub = nh_.advertise<nav_msgs::Path>("my_global_path", 1 );
    goal_marker_pub = nh_.advertise<visualization_msgs::Marker>("goal_markers", 10);

    marker_id_cnt = 0 ;

    update_map_bounds();


    //pose_sub = nh_.subscribe("/camera/depth_registered/points", 1000, &GlobalPlanner::initialpose_callback, this);
 
    //pose_sub = nh_.subscribe("/camera/depth_registered/points", 1000, boost::bind(&GlobalPlanner::initialpose_callback, this, _1));

  }

bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,  std::vector<geometry_msgs::PoseStamped>& plan ){

    //Djikstra's Algorithm
    __uint32_t mx_i, my_i, mx_f, my_f;

    print_world_params(start, goal, mx_i, my_i, mx_f, my_f);

    Point curr_point = Point{mx_i, my_i};

    Point goal_point = Point{goal.pose.position.x, goal.pose.position.y};

    bool reached = false;

    //srand((__uint32_t)time(0));

    cout << "Testing costmap_ros for consistency!" << endl;

    cout << "ros_costmap->getGlobalFrameID(): " << costmap_ros->getGlobalFrameID() << endl;
    cout << "Sleeping for 2 seconds!" << endl;
    ros::Duration(2.0).sleep();

    cout << "Trying to fetch global_pose of the robot!" << endl;
    geometry_msgs::PoseStamped global_pose_;
    costmap_ros->getRobotPose(global_pose_);

    cout << "global_pose.x: " << global_pose_.pose.position.x << " global_pose_.y: " << global_pose_.pose.position.y  << endl;
    
    
    //generate_next_goal();
    
    return true;

  }

I am also attaching the relevant header file -

/** include the libraries you need in your planner here */
/** for global path planner interface */
#include <ros/ros.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_2d/costmap_2d.h>
#include <nav_core/base_global_planner.h>
#include <geometry_msgs/PoseStamped.h>
#include <angles/angles.h>
#include <base_local_planner/world_model.h>
#include <base_local_planner/costmap_model.h>
#include <map>
#include<nav_msgs/Path.h>


#ifndef GLOBAL_PLANNER_CPP
#define GLOBAL_PLANNER_CPP



namespace global_planner {

    class GlobalPlanner : public nav_core::BaseGlobalPlanner {

        struct Point {
            
                __uint32_t x, y; 

                bool operator==(const Point &p1 ) const{   return ((p1.x == x) && (p1.y == y));  }   
                
                bool operator<(const Point &p1 ) const{    return ((p1.x < x) || (p1.x == x && p1.y < y) ) ;  }   

            };

        
        struct Cell {

            Point point; 
            __uint32_t cost_till_now;

            bool operator<(const Cell &c1) const {
                
                
                return (c1.cost_till_now < cost_till_now || (c1.cost_till_now == cost_till_now && c1.point < point));

            }
        
        };



        public:

            GlobalPlanner();
            
            GlobalPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
            void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
            bool makePlan(const geometry_msgs::PoseStamped& start,const geometry_msgs::PoseStamped& goal,std::vector<geometry_msgs::PoseStamped>& plan);

            

        private: 


            double heu(Point p1, Point p2);
            void update_planner_plan(std::vector<Point> &path_points, std::vector<geometry_msgs::PoseStamped> &plan, const geometry_msgs::PoseStamped &goal); 
            void publish_global_path(const std::vector<geometry_msgs::PoseStamped> &plan, const geometry_msgs::PoseStamped &goal);
            bool print_cell(const Cell &cell);
            bool make_straight_plan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,  std::vector<geometry_msgs::PoseStamped>& plan);
            void print_world_params(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, __uint32_t &mx_i, __uint32_t &my_i, __uint32_t &mx_f, __uint32_t &my_f);
            bool generate_straight_path(const Point &p1, const Point &p2);
            Point generate_next_goal();
            bool makePlanOne(const geometry_msgs::PoseStamped& start,const geometry_msgs::PoseStamped& goal,std::vector<geometry_msgs::PoseStamped>& plan);
            bool is_inside_robot_footprint(const Point &p);
            Point initialpose_callback(const geometry_msgs::PoseStamped::ConstPtr &msg);
            void update_map_bounds();


            costmap_2d::Costmap2D* costmap_ros_;
            costmap_2d::Costmap2DROS *costmap_ros;
            __uint32_t size_x, size_y;
            __uint32_t map_xi, map_xf, map_yi, map_yf;
            ros::Publisher global_plan_pub, goal_marker_pub;
            ros::Subscriber pose_sub;
            ros::NodeHandle nh_;
            int marker_id_cnt;
    };

};

#endif

EDIT ONE - Attaching the error message

`[ INFO] [1623877214.383936978, 92.753000000]: global_costmap: Using plugin "static"
[ INFO] [1623877214.399754683, 92.769000000]: Requesting the map...
[ INFO] [1623877214.609456022, 92.979000000]: Resizing costmap to 4000 X 4000 at 0.050000 m/pix
[ INFO] [1623877214.758182943, 93.127000000]: Received a 4000 X 4000 map at 0.050000 m/pix
[ INFO] [1623877214.758233616, 93.127000000]: Subscribing to updates
[ INFO] [1623877214.763301804, 93.132000000]: global_costmap: Using plugin "inflation"
global_pose_.x: 0.0127673 global_pose_.y: -0.00185759
Sleeping for 3 seconds!
[ WARN] [1623877216.034654901, 94.401000000]: Map update loop missed its desired rate of 1.0000Hz... the loop actually took 1.2430 seconds
global_frame: map
x_mx: 1691 x_mn: 2307
y_mn: 1692 y_mx: 2308
[ INFO] [1623877218.021871851, 96.385000000]: local_costmap: Using plugin "obstacles_laser"
[ INFO] [1623877218.026462101, 96.390000000]:     Subscribed to Topics: laser
[ INFO] [1623877218.038852471, 96.402000000]: local_costmap: Using plugin "inflation"
[ INFO] [1623877218.073365799, 96.437000000]: Created local_planner dwa_local_planner/DWAPlannerROS
[ INFO] [1623877218.080402759, 96.444000000]: Sim period is set to 0.05
[ WARN] [1623877218.083561377, 96.447000000]: Parameter max_trans_vel is deprecated (and will not load properly). Use max_vel_trans instead.
[ WARN] [1623877218.084195753, 96.447000000]: Parameter min_trans_vel is deprecated (and will not load properly). Use min_vel_trans instead.
[ WARN] [1623877218.084750167, 96.448000000]: Parameter max_rot_vel is deprecated (and will not load properly). Use max_vel_theta instead.
[ WARN] [1623877218.085114421, 96.449000000]: Parameter min_rot_vel is deprecated (and will not load properly). Use min_vel_theta instead.
[ INFO] [1623877219.082219761, 97.444000000]: Recovery behavior will clear layer 'obstacles'
[ INFO] [1623877219.087005348, 97.448000000]: Recovery behavior will clear layer 'obstacles'
[ INFO] [1623877219.112662972, 97.473000000]: odom received!
[ WARN] [1623877220.007202261, 98.365000000]: Map update loop missed its desired rate of 1.0000Hz... the loop actually took 1.2070 seconds
Inside the make_plan function!
Hello World!
(int)costmap_2d::LETHAL_OBSTACLE: 254
(int)costmap_2d::NO_INFORMATION: 255
(int)costmap_2d::FREE_SPACE: 0
Sleeping for 2 seconds!

Printing start pose and goal pose: 
w_i: (0.01278,-0.00185759) m_i: (2000,1999)
w_f: (-9.51565,0.0048027) m_f: (1809,2000)

Testing costmap_ros for consistency!
[move_base-1] process has died [pid 3322, exit code -11, cmd /home/skpro19/catkin_ws/devel/lib/move_base/move_base /odom:=odometry/filtered __name:=move_base __log:=/home/skpro19/.ros/log/d4e95946-cee5-11eb-8e7f-c8e2656b0bb1/move_base-1.log].
log file: /home/skpro19/.ros/log/d4e95946-cee5-11eb-8e7f-c8e2656b0bb1/move_base-1*.log
all processes on machine have died, roslaunch will exit
shutting down processing monitor...
... shutting down processing monitor complete
done`
Improve this question
$\endgroup$
2
  • $\begingroup$ What is the exact, verbatim error you're getting? Doesn't it have a line number?? $\endgroup$
    – Chuck ♦
    Commented Jun 16, 2021 at 21:13
  • $\begingroup$ @Chuck I have updated the post with the error message. The error is arising because of using the getRobotPose function inside the makePlan function. $\endgroup$
    – skpro19
    Commented Jun 16, 2021 at 21:23

1 Answer 1

Reset to default
1
$\begingroup$

I'm no export in C++, but this line:

  void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){

    nh_ = ros::NodeHandle{"~abcd"};
    
    costmap_ros  = costmap_ros;

(the last) you are setting the costmap_ros parameter to itself, so I don't think you are setting the costmap_ros of the object so when you get to makePlan, costmap_ros is pointing to something random. Try renaming the costmap_ros parameter in the function to cm_ros and change the line to costmap_ros = cm_ros;

Improve this answer
$\endgroup$
1
  • $\begingroup$ That solved the problem. Thanks! $\endgroup$
    – skpro19
    Commented Jun 17, 2021 at 16:39

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.