ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
1

Unable to figure out the source for the bad_alloc error

asked 2021-06-16 14:21:02 -0600

updated 2021-06-16 16:01:47 -0600

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 ...
(more)
edit retag flag offensive close merge delete

Comments

Can you please update your question with a copy and paste of the full error?

jayess gravatar image jayess  ( 2021-06-16 15:36:00 -0600 )edit

@jayess I have updated the post.

skpro19 gravatar image skpro19  ( 2021-06-16 15:55:05 -0600 )edit

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.

skpro19 gravatar image skpro19  ( 2021-06-16 16:04:21 -0600 )edit

1 Answer

Sort by » oldest newest most voted
1

answered 2021-06-17 00:31:28 -0600

updated 2021-06-17 00:32:39 -0600

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.

edit flag offensive delete link more

Comments

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.

skpro19 gravatar image skpro19  ( 2021-06-17 08:59:03 -0600 )edit
1

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/2...

Fetullah Atas gravatar image Fetullah Atas  ( 2021-06-17 11:25:08 -0600 )edit

Makes sense. Thanks!

skpro19 gravatar image skpro19  ( 2021-06-17 11:42:08 -0600 )edit

Question Tools

2 followers

Stats

Asked: 2021-06-16 14:21:02 -0600

Seen: 228 times

Last updated: Jun 17 '21