0
$\begingroup$

Rosanswers logo

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

Originally posted by skpro19 on ROS Answers with karma: 310 on 2021-06-16

Post score: 1


Original comments

Comment by jayess on 2021-06-16:
Can you please update your question with a copy and paste of the full error?

Comment by skpro19 on 2021-06-16:
@jayess I have updated the post.

Comment by skpro19 on 2021-06-16:
Things seem to be working fine in the 7th line of the console output - global_pose_.x: 0.0127673 global_pose_.y: -0.00185759. However, it throws an error when the same function is called from inside the makePlan function.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

your naming of variables seems troubled to me. You have a global variable name which is identical to function parameter;

costmap_2d::Costmap2DROS *costmap_ros;

and

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

You do realize both of them has costmap_ros ?, it could be that you are making use of function parameter costmap_ros in initialize() function.

I suggest yo to clear up the namings to avoid confusions, also try to avoid using raw pointers. a std::shared_ptr<> will suit better here.


Originally posted by Fetullah Atas with karma: 819 on 2021-06-17

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by skpro19 on 2021-06-17:
That solved the problem. Thank you so much. However, why would that lead to an error in the first place? When I use the variable costmap_ros anywhere in the code, shouldn'r it refer to the one used in the header file? And I had already initialized it.

Comment by Fetullah Atas on 2021-06-17:
You won’t want to have identical global variable and function parameter names. If you have absolutely no other option then use ::costmap_ros to refer to global. See a more detailed discussion on the matter;

https://stackoverflow.com/questions/22492318/how-to-refer-to-a-global-variable-which-has-the-same-name-as-a-local-variable-in

Comment by skpro19 on 2021-06-17:
Makes sense. Thanks!

$\endgroup$

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.