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
Your duplicate names are causing you that headache, more precisely the line costmap_ros = costmap_ros;. You have an input argument costmap_2d::Costmap2DROS* costmap_ros as well as a class member costmap_2d::Costmap2DROS* costmap_ros inside GlobalPlanner. The line
costmap_ros = costmap_ros;
will assign the input argument to itself instead of to the class member, leaving the class member uninitialised. When calling costmap_ros_ = costmap_ros->getCostmap(); inside the initialize function you will actually use the input argument while inside GlobalPlanner::makePlan the uninitialised class member.
You can to fix it by changing the line to
this->costmap_ros = costmap_ros;
to tell it that it should use the class member and not the input argument but actually I would rename the variables to have unique names.
You would not make such mistakes if you would declare your variables as const consistently: Test it here. Read about this so-called const-correctness here.
Additionally it is better to use the ROS logging macros ROS_DEBUG_STREAM(...), ROS_INFO_STREAM(...), ROS_WARN_STREAM(...), ROS_ERROR_STREAM(...), ROS_FATAL_STREAM(...) etc. instead of std::cout for output to console when working with ROS as these macros are
rosout and can be used for distributed logging