无法找出 bad_alloc 错误的来源

Question

当我尝试在 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

Answer 1

您的重名让您头疼，更准确地说是 costmap_ros = costmap_ros; 行。您有一个 输入参数 costmap_2d::Costmap2DROS* costmap_ros 以及一个 class 成员 costmap_2d::Costmap2DROS* costmap_ros 在 GlobalPlanner.行

costmap_ros = costmap_ros;

会将输入参数分配给自身而不是 class 成员，使 class 成员 未初始化 。在 initialize 函数内调用 costmap_ros_ = costmap_ros->getCostmap(); 时，您实际上会在 GlobalPlanner::makePlan 未初始化的 class 成员内使用输入参数。您可以通过 changing the line to

来修复它

this->costmap_ros = costmap_ros;

告诉它应该使用 class 成员而不是输入参数，但实际上我会 重命名变量 以具有唯一的名称。

如果您始终将变量声明为 const，就不会犯这样的错误：Test it here. Read about this so-called const-correctness here.

此外，在使用 ROS 时，最好使用 ROS logging macros ROS_DEBUG_STREAM(...), ROS_INFO_STREAM(...), ROS_WARN_STREAM(...), ROS_ERROR_STREAM(...), ROS_FATAL_STREAM(...) etc. 而不是 std::cout 来输出到控制台，因为这些宏是

更具表现力
时间戳
稍后登录inspection/debugging
不仅在控制台中显示，而且通过网络发送到 rosout 并可用于分布式日志记录

无法找出 bad_alloc 错误的来源

Unable to figure out the source for the bad_alloc error

c++

ros

bad-alloc