terminate called after throwing an instance of 'std::length_error' what(): vector::_M_range_insert

[ChunqiuYang]

感谢分享这么优秀的工作！最近在深度的测试过程中发现了一个bug，在跑ipc.launch和map_rc.launch模拟器时，当规划好几条路径之后，程序会报如下的错误，然后程序崩溃，想问一下，你们在跑该版本代码的时候也会有同样的问题出现吗？

[KayatoDQY]

Hello, we occasionally encounter the problem you mentioned above, but the specific cause of the problem is not clear. Can you find the cause? Have you solved the above problem?

[KayatoDQY]

Issue: std::length_error exception in PlannerClass::LocalPcCallback

Hello @ChunqiuYang , have you solved this problem now? Below are the problems I found and the solutions, which may not be correct, but can be used as a reference.

Problem

The reason for this error is that the std::length_error exception is thrown when the _M_range_insert method of std::vector is called in the PlannerClass::LocalPcCallback function. This indicates that when trying to insert elements into the vector, the maximum capacity of the vector is exceeded.

Analysis

My analysis is that local_pc_ is a std::vector<Eigen::Vector3d>, and elements are inserted into local_pc_ each time when traversing all points in static_map_. If the number of points in static_map_ is very large, a large number of memory allocation operations will result. Therefore, before inserting elements into the vector local_pc_, use the reserve method to pre-allocate enough space to reduce the number of memory reallocations.

Modified Code

The following is my modified code. After the modification, I tested it many times and did not find the above error:

void PlannerClass::LocalPcCallback(const sensor_msgs::PointCloud2ConstPtr& msg)
{
    local_pc_mutex_.lock();
    try {
        ros::Time now = ros::Time::now();
        local_pc_.clear();
        pcl::PointCloud<pcl::PointXYZ> cloud;
        pcl::fromROSMsg(*msg, cloud);

        vec_cloud_.push_back(cloud);
        if (vec_cloud_.size() > 10) vec_cloud_.pop_front();
        static_cloud_.reset(new pcl::PointCloud<pcl::PointXYZ>());
        for (int i = 0; i < vec_cloud_.size(); i++) *static_cloud_ += vec_cloud_[i];

        pcl::CropBox<pcl::PointXYZ> cb;
        cb.setMin(Eigen::Vector4f(odom_p_.x() - (map_upp_.x()-0.5), odom_p_.y() - (map_upp_.y()-0.5), 0.2, 1.0));
        cb.setMax(Eigen::Vector4f(odom_p_.x() + (map_upp_.x()-0.5), odom_p_.y() + (map_upp_.y()-0.5), map_upp_.z(), 1.0));
        cb.setInputCloud(static_cloud_);
        cb.filter(*static_cloud_);

        pcl::VoxelGrid<pcl::PointXYZ> vf;
        pcl::PointCloud<pcl::PointXYZ>::Ptr cur_cloud_ds(new pcl::PointCloud<pcl::PointXYZ>());
        Eigen::Vector3f pos = odom_p_.cast<float>();
        vf.setLeafSize(0.2, 0.2, 0.2);
        vf.setInputCloud(static_cloud_);
        vf.filter(static_map_);

        local_pc_.reserve(static_map_.points.size());
        for (auto &point: static_map_.points) {
            local_pc_.emplace_back(point.x, point.y, point.z);
        }

        static int obs_count = 0;
        local_astar_->SetCenter(Eigen::Vector3d(odom_p_.x(), odom_p_.y(), 0.0));
        local_astar_->setObsVector(local_pc_, expand_fix_);
        std::vector<Eigen::Vector3d> remain_path;
        remain_path.insert(remain_path.begin(), follow_path_.begin()+astar_index_, follow_path_.end());
        if (local_astar_->CheckPathFree(remain_path) == false) replan_flag_ = true;

        log_times_[0] = (ros::Time::now() - now).toSec() * 1000.0;
    } catch (const std::exception &e) {
        std::cerr << "Exception in LocalPcCallback: " << e.what() << std::endl;
    }
    local_pc_mutex_.unlock();
}

[ChunqiuYang]

Issue: std::length_error exception in PlannerClass::LocalPcCallback

Hello @ChunqiuYang , have you solved this problem now? Below are the problems I found and the solutions, which may not be correct, but can be used as a reference.

Problem

The reason for this error is that the std::length_error exception is thrown when the _M_range_insert method of std::vector is called in the PlannerClass::LocalPcCallback function. This indicates that when trying to insert elements into the vector, the maximum capacity of the vector is exceeded.

Analysis

My analysis is that local_pc_ is a std::vector<Eigen::Vector3d>, and elements are inserted into local_pc_ each time when traversing all points in static_map_. If the number of points in static_map_ is very large, a large number of memory allocation operations will result. Therefore, before inserting elements into the vector local_pc_, use the reserve method to pre-allocate enough space to reduce the number of memory reallocations.

Modified Code

The following is my modified code. After the modification, I tested it many times and did not find the above error:

void PlannerClass::LocalPcCallback(const sensor_msgs::PointCloud2ConstPtr& msg)
{
    local_pc_mutex_.lock();
    try {
        ros::Time now = ros::Time::now();
        local_pc_.clear();
        pcl::PointCloud<pcl::PointXYZ> cloud;
        pcl::fromROSMsg(*msg, cloud);

        vec_cloud_.push_back(cloud);
        if (vec_cloud_.size() > 10) vec_cloud_.pop_front();
        static_cloud_.reset(new pcl::PointCloud<pcl::PointXYZ>());
        for (int i = 0; i < vec_cloud_.size(); i++) *static_cloud_ += vec_cloud_[i];

        pcl::CropBox<pcl::PointXYZ> cb;
        cb.setMin(Eigen::Vector4f(odom_p_.x() - (map_upp_.x()-0.5), odom_p_.y() - (map_upp_.y()-0.5), 0.2, 1.0));
        cb.setMax(Eigen::Vector4f(odom_p_.x() + (map_upp_.x()-0.5), odom_p_.y() + (map_upp_.y()-0.5), map_upp_.z(), 1.0));
        cb.setInputCloud(static_cloud_);
        cb.filter(*static_cloud_);

        pcl::VoxelGrid<pcl::PointXYZ> vf;
        pcl::PointCloud<pcl::PointXYZ>::Ptr cur_cloud_ds(new pcl::PointCloud<pcl::PointXYZ>());
        Eigen::Vector3f pos = odom_p_.cast<float>();
        vf.setLeafSize(0.2, 0.2, 0.2);
        vf.setInputCloud(static_cloud_);
        vf.filter(static_map_);

        local_pc_.reserve(static_map_.points.size());
        for (auto &point: static_map_.points) {
            local_pc_.emplace_back(point.x, point.y, point.z);
        }

        static int obs_count = 0;
        local_astar_->SetCenter(Eigen::Vector3d(odom_p_.x(), odom_p_.y(), 0.0));
        local_astar_->setObsVector(local_pc_, expand_fix_);
        std::vector<Eigen::Vector3d> remain_path;
        remain_path.insert(remain_path.begin(), follow_path_.begin()+astar_index_, follow_path_.end());
        if (local_astar_->CheckPathFree(remain_path) == false) replan_flag_ = true;

        log_times_[0] = (ros::Time::now() - now).toSec() * 1000.0;
    } catch (const std::exception &e) {
        std::cerr << "Exception in LocalPcCallback: " << e.what() << std::endl;
    }
    local_pc_mutex_.unlock();
}

hello @KayatoDQY, thank you for your response. So far, I haven't pinpointed the exact location of the bug, but I have roughly narrowed down the issue to std::string or std::vector. I'll try the modified code you suggested later.