如何解决无法找出 bad_alloc 错误的来源
当我尝试在 bad_alloc 函数中使用 costmap_2d::Costmap2DROS::getRobotPose 函数时出现 makePlan 错误。有趣的是，如果我从 initialize 函数内部（而不是从 makePlan 函数内部）调用相同的函数，则不会弹出错误。
#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",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_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,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,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,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 中有一个输入参数 costmap_2d::Costmap2DROS* costmap_ros 以及一个类成员 GlobalPlanner。线
costmap_ros = costmap_ros;
会将输入参数分配给自身而不是类成员，从而使类成员未初始化。在 costmap_ros_ = costmap_ros->getCostmap(); 函数内部调用 initialize 时，您实际上将在 GlobalPlanner::makePlan 内部使用未初始化的类成员的输入参数。
您可以通过 changing the line to
this->costmap_ros = costmap_ros;
告诉它应该使用类成员而不是输入参数，但实际上我会重命名变量以获得唯一的名称。
如果您始终将变量声明为 const：Test it here，您就不会犯这样的错误。了解这种所谓的 const-正确性 here。
此外，在使用 ROS 时最好使用 ROS logging macros ROS_DEBUG_STREAM(...),ROS_INFO_STREAM(...),ROS_WARN_STREAM(...),ROS_ERROR_STREAM(...),ROS_FATAL_STREAM(...) etc. 而不是 std::cout 来输出到控制台，因为这些宏是
- 更具表现力
- 时间戳
- 记录以供日后检查/调试
- 不仅显示在控制台中，还通过网络发送到
rosout并可用于分布式日志记录
版权声明：本文内容由互联网用户自发贡献，该文观点与技术仅代表作者本人。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容， 请发送邮件至 dio@foxmail.com 举报，一经查实，本站将立刻删除。
