无法找出BAD_ALLOC错误的来源 [英] Unable to figure out the source for the bad_alloc error

问题描述

当我尝试在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，可用于分布式日志记录

这篇关于无法找出BAD_ALLOC错误的来源的文章就介绍到这了，希望我们推荐的答案对大家有所帮助，也希望大家多多支持IT屋！