无法找出BAD_ALLOC错误的来源
当我尝试在
makePlan函数内使用costmap_2d::Costmap2DROS::getRobotPose函数时，收到bad_alloc错误。有趣的是，如果我从initialize函数内部(而不是从makePlan函数内部)调用相同的函数，则不会弹出错误。
我正在附加source代码文件中的相关函数。
#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;
}
我还附上了相关的header文件-
/** 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
解决方案
您的重复名称让您头疼，更准确地说是线路costmap_ros = costmap_ros;。您有一个输入参数costmap_2d::Costmap2DROS* costmap_ros以及类成员GlobalPlanner。行
costmap_ros = costmap_ros;
将输入参数分配给自身而不是类成员，从而使类成员未初始化。当在initialize函数内调用costmap_ros_ = costmap_ros->getCostmap();时，您实际上将在GlobalPlanner::makePlan未初始化的类成员内使用输入参数。
您可以通过changing the line to
this->costmap_ros = costmap_ros;
告诉它应该使用类成员而不是输入参数，但实际上我会重命名变量以使其具有唯一的名称。
如果您一致地将变量声明为const：Test it here，您就不会犯这样的错误。阅读有关此所谓const-正确性here的内容。
此外，在使用ROS时，最好使用ROS logging macrosROS_DEBUG_STREAM(...), ROS_INFO_STREAM(...), ROS_WARN_STREAM(...), ROS_ERROR_STREAM(...), ROS_FATAL_STREAM(...)etc.而不是std::cout输出到控制台，因为这些宏是
- 更具表现力
- 时间戳
- 记录以备日后检查/调试
- 不仅显示在控制台中，而且通过网络发送到
rosout，可用于分布式日志记录
相关文章