Ubuntu 20 segmentation fault

[brunopinto900]

Hello,

I launched ROS with this "roslaunch plan_manage kino_replan.launch"

When i give a goal with the 2D navigation goal plugin in RVIZ, its printed, on the terminal, the following:

Triggered!
[TRIG]: from WAIT_TARGET to GEN_NEW_TRAJ
[kino replan]: -----------------------
start: -19 0 0, 0 0 0, 0 0 0
goal: -10.9901 -0.233514 1, 0 0 0
terminate called after throwing an instance of 'std::bad_alloc'
what(): std::bad_alloc
Stack trace (most recent call last):
#8 Object "[0xffffffffffffffff]", at 0xffffffffffffffff, in
#7 Object "/home/bruno/fast_plan_ws/devel/lib/libpath_searching.so", at 0x7f460ba8e570, in
#6 Object "/home/bruno/fast_plan_ws/devel/lib/plan_manage/fast_planner_node", at 0x7f460bb208f9, in Eigen::internal::throw_std_bad_alloc()
#5 Object "/lib/x86_64-linux-gnu/libstdc++.so.6", at 0x7f460b13a798, in __cxa_throw
#4 Object "/lib/x86_64-linux-gnu/libstdc++.so.6", at 0x7f460b13a4e6, in std::terminate()
#3 Object "/lib/x86_64-linux-gnu/libstdc++.so.6", at 0x7f460b13a47b, in
#2 Object "/lib/x86_64-linux-gnu/libstdc++.so.6", at 0x7f460b12e950, in
#1 Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7f460ad45858, in abort
#0 Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7f460ad660fb, in gsignal
Segmentation fault (Signal sent by the kernel [(nil)])
[fast_planner_node-1] process has died [pid 13388, exit code -11, cmd /home/bruno/fast_plan_ws/devel/lib/plan_manage/fast_planner_node /odom_world:=/state_ukf/odom /sdf_map/odom:=/state_ukf/odom /sdf_map/cloud:=/pcl_render_node/cloud /sdf_map/pose:=/pcl_render_node/camera_pose /sdf_map/depth:=/pcl_render_node/depth __name:=fast_planner_node __log:=/home/bruno/.ros/log/62741276-f6d0-11eb-9763-089798ca460b/fast_planner_node-1.log].
log file: /home/bruno/.ros/log/62741276-f6d0-11eb-9763-089798ca460b/fast_planner_node-1*.log

What could be wrong?
I have Eigen 3.3.9

[anyway-blows]

I met the same problem. Finally, I found

int KinodynamicAstar::timeToIndex(double time)
{
  int idx = floor((time - time_origin_) * inv_time_resolution_);
}

this fun should have a return value.

However when I fixed this problem, I met another problem.

opt.set_min_objective(BsplineOptimizer::costFunction, this);

when the code arrived where the above code line is, the bspline_opt crashed for no reasons. Now i haven't fixed this problem.
@brunopinto900

[anyway-blows]

these functions EDTEnvironment::evaluateEDTWithGrad and EDTEnvironment::interpolateTrilinear the signature show that they have a return value, however the definition has no return value, which causes the crash.

[iamrajee]

@brunopinto900 @anyway-blows
I'm facing a similar error. kino_replan.launch crashes with the message "open set empty, no path!". I'm on ubuntu 20 ros noetic. I've made all the other necessary modifications(C++ Version & Eigen). Any lead on what might be causing the issue?

I realised that my code went into the below snippet and continued(L189) thereafter. That is why it's unable to reach to this line(L278), where new pro_node is pushed to open_set_.

// Check if in close set
        Eigen::Vector3i pro_id = posToIndex(pro_pos);
        int pro_t_id = timeToIndex(pro_t);
        PathNodePtr pro_node = dynamic ? expanded_nodes_.find(pro_id, pro_t_id) : expanded_nodes_.find(pro_id);
        if (pro_node != NULL && pro_node->node_state == IN_CLOSE_SET)
        {
          if (init_search)
            std::cout << "close" << std::endl;
          continue;
        }

Also, I have included return value to avoid the Eigen:bad_alloc error in KinodynamicAstar::timeToIndex Function

int KinodynamicAstar::timeToIndex(double time)
{
  int idx = floor((time - time_origin_) * inv_time_resolution_);
  return idx; //new <==
}

Thanks!

[anyway-blows]

@iamrajee Thare are two more bugs in fast-planner codes, which are functions interpolateTrilinear and evaluateEDTWithGrad.

Their declarations both declaim a return value: L69-L72
but their definition miss a return value:
L84-L105
L107-L118

their returns are unnecessary, so I changed them to return void.

wish to be useful to you.

[iamrajee]

@anyway-blows You're right, thank you for the insights. The weird part is that I have also tested the same code of Fast_Planner on ubuntu18.04 Melodic, where it works without any issue or error. It's only noetic where I'm getting this error(open set empty, no path!). Still not sure what might be causing this issue.

[DavidePatria]

@iamrajee have you been able to run it on noetic?

[iamrajee]

Nopes! I eventually shifted to melodic -

[eliabntt]

try #85 try this comment

[ba1Sta]

@iamrajee Thare are two more bugs in fast-planner codes, which are functions interpolateTrilinear and evaluateEDTWithGrad.

Their declarations both declaim a return value: L69-L72 but their definition miss a return value: L84-L105 L107-L118

their returns are unnecessary, so I changed them to return void.

wish to be useful to you.

Hi @iamrajee , thanks for your sharing. You previouse comments are valuable! I follow your steps and successfully find the problem in kinodynamic_astar.cpp. After that, I encountered the same problem: open set empty, no path.... Finally, after following the instructions from @anyway-blows . I successfully figure that out. Here is my modification, hope others will be able figure that out as well.

1. In the edt_environment.h, replace "pair<double, Eigen::Vector3d>" with void in both line 69 and line 71. After modification:

/**
* This file is part of Fast-Planner.
*
* Copyright 2019 Boyu Zhou, Aerial Robotics Group, Hong Kong University of Science and Technology, <uav.ust.hk>
* Developed by Boyu Zhou <bzhouai at connect dot ust dot hk>, <uv dot boyuzhou at gmail dot com>
* for more information see <https://github.com/HKUST-Aerial-Robotics/Fast-Planner>.
* If you use this code, please cite the respective publications as
* listed on the above website.
*
* Fast-Planner is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Fast-Planner is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with Fast-Planner. If not, see <http://www.gnu.org/licenses/>.
*/



#ifndef _EDT_ENVIRONMENT_H_
#define _EDT_ENVIRONMENT_H_

#include <Eigen/Eigen>
#include <geometry_msgs/PoseStamped.h>
#include <iostream>
#include <ros/ros.h>
#include <utility>

#include <plan_env/obj_predictor.h>
#include <plan_env/sdf_map.h>

using std::cout;
using std::endl;
using std::list;
using std::pair;
using std::shared_ptr;
using std::unique_ptr;
using std::vector;

namespace fast_planner {
class EDTEnvironment {
private:
  /* data */
  ObjPrediction obj_prediction_;
  ObjScale obj_scale_;
  double resolution_inv_;
  double distToBox(int idx, const Eigen::Vector3d& pos, const double& time);
  double minDistToAllBox(const Eigen::Vector3d& pos, const double& time);

public:
  EDTEnvironment(/* args */) {
  }
  ~EDTEnvironment() {
  }

  SDFMap::Ptr sdf_map_;

  void init();
  void setMap(SDFMap::Ptr map);
  void setObjPrediction(ObjPrediction prediction);
  void setObjScale(ObjScale scale);
  void getSurroundDistance(Eigen::Vector3d pts[2][2][2], double dists[2][2][2]);
  void interpolateTrilinear(double values[2][2][2], const Eigen::Vector3d& diff,
                                                     double& value, Eigen::Vector3d& grad);
  void evaluateEDTWithGrad(const Eigen::Vector3d& pos, double time,
                                                    double& dist, Eigen::Vector3d& grad);
  double evaluateCoarseEDT(Eigen::Vector3d& pos, double time);
  void getMapRegion(Eigen::Vector3d& ori, Eigen::Vector3d& size) {
    sdf_map_->getRegion(ori, size);
  }

  typedef shared_ptr<EDTEnvironment> Ptr;
};

}  // namespace fast_planner

#endif

2. In the edt_environment.cpp, replace "pair<double, Eigen::Vector3d>" with void in both line 84 and line 107. After modification:

/**
* This file is part of Fast-Planner.
*
* Copyright 2019 Boyu Zhou, Aerial Robotics Group, Hong Kong University of Science and Technology, <uav.ust.hk>
* Developed by Boyu Zhou <bzhouai at connect dot ust dot hk>, <uv dot boyuzhou at gmail dot com>
* for more information see <https://github.com/HKUST-Aerial-Robotics/Fast-Planner>.
* If you use this code, please cite the respective publications as
* listed on the above website.
*
* Fast-Planner is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Fast-Planner is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with Fast-Planner. If not, see <http://www.gnu.org/licenses/>.
*/



#include <plan_env/edt_environment.h>

namespace fast_planner {
/* ============================== edt_environment ==============================
 */
void EDTEnvironment::init() {
}

void EDTEnvironment::setMap(shared_ptr<SDFMap> map) {
  this->sdf_map_ = map;
  resolution_inv_ = 1 / sdf_map_->getResolution();
}

void EDTEnvironment::setObjPrediction(ObjPrediction prediction) {
  this->obj_prediction_ = prediction;
}

void EDTEnvironment::setObjScale(ObjScale scale) {
  this->obj_scale_ = scale;
}

double EDTEnvironment::distToBox(int idx, const Eigen::Vector3d& pos, const double& time) {
  // Eigen::Vector3d pos_box = obj_prediction_->at(idx).evaluate(time);
  Eigen::Vector3d pos_box = obj_prediction_->at(idx).evaluateConstVel(time);

  Eigen::Vector3d box_max = pos_box + 0.5 * obj_scale_->at(idx);
  Eigen::Vector3d box_min = pos_box - 0.5 * obj_scale_->at(idx);

  Eigen::Vector3d dist;

  for (int i = 0; i < 3; i++) {
    dist(i) = pos(i) >= box_min(i) && pos(i) <= box_max(i) ? 0.0 : min(fabs(pos(i) - box_min(i)),
                                                                       fabs(pos(i) - box_max(i)));
  }

  return dist.norm();
}

double EDTEnvironment::minDistToAllBox(const Eigen::Vector3d& pos, const double& time) {
  double dist = 10000000.0;
  for (int i = 0; i < obj_prediction_->size(); i++) {
    double di = distToBox(i, pos, time);
    if (di < dist) dist = di;
  }

  return dist;
}

void EDTEnvironment::getSurroundDistance(Eigen::Vector3d pts[2][2][2], double dists[2][2][2]) {
  for (int x = 0; x < 2; x++) {
    for (int y = 0; y < 2; y++) {
      for (int z = 0; z < 2; z++) {
        dists[x][y][z] = sdf_map_->getDistance(pts[x][y][z]);
      }
    }
  }
}

void EDTEnvironment::interpolateTrilinear(double values[2][2][2],
                                                                   const Eigen::Vector3d& diff,
                                                                   double& value,
                                                                   Eigen::Vector3d& grad) {
  // trilinear interpolation
  double v00 = (1 - diff(0)) * values[0][0][0] + diff(0) * values[1][0][0];
  double v01 = (1 - diff(0)) * values[0][0][1] + diff(0) * values[1][0][1];
  double v10 = (1 - diff(0)) * values[0][1][0] + diff(0) * values[1][1][0];
  double v11 = (1 - diff(0)) * values[0][1][1] + diff(0) * values[1][1][1];
  double v0 = (1 - diff(1)) * v00 + diff(1) * v10;
  double v1 = (1 - diff(1)) * v01 + diff(1) * v11;

  value = (1 - diff(2)) * v0 + diff(2) * v1;

  grad[2] = (v1 - v0) * resolution_inv_;
  grad[1] = ((1 - diff[2]) * (v10 - v00) + diff[2] * (v11 - v01)) * resolution_inv_;
  grad[0] = (1 - diff[2]) * (1 - diff[1]) * (values[1][0][0] - values[0][0][0]);
  grad[0] += (1 - diff[2]) * diff[1] * (values[1][1][0] - values[0][1][0]);
  grad[0] += diff[2] * (1 - diff[1]) * (values[1][0][1] - values[0][0][1]);
  grad[0] += diff[2] * diff[1] * (values[1][1][1] - values[0][1][1]);
  grad[0] *= resolution_inv_;
}

void EDTEnvironment::evaluateEDTWithGrad(const Eigen::Vector3d& pos,
                                                                  double time, double& dist,
                                                                  Eigen::Vector3d& grad) {
  Eigen::Vector3d diff;
  Eigen::Vector3d sur_pts[2][2][2];
  sdf_map_->getSurroundPts(pos, sur_pts, diff);

  double dists[2][2][2];
  getSurroundDistance(sur_pts, dists);

  interpolateTrilinear(dists, diff, dist, grad);
}

double EDTEnvironment::evaluateCoarseEDT(Eigen::Vector3d& pos, double time) {
  double d1 = sdf_map_->getDistance(pos);
  if (time < 0.0) {
    return d1;
  } else {
    double d2 = minDistToAllBox(pos, time);
    return min(d1, d2);
  }
}

// EDTEnvironment::
}  // namespace fast_planner

3. cd to the workspace folder and catkin_make

catkin_make

[ba1Sta]

@iamrajee Thare are two more bugs in fast-planner codes, which are functions interpolateTrilinear and evaluateEDTWithGrad.

Their declarations both declaim a return value: L69-L72 but their definition miss a return value: L84-L105 L107-L118

their returns are unnecessary, so I changed them to return void.

wish to be useful to you.

Thanks! This really helped me out! Previous I encounter the same problem which is mentioned by @iamrajee . Now the simulation is good to go. Also, thank you teams from HKUST-Aerial-Robotics! I am also willing to keep contributing to this repo. If anyone still in the trouble working with Ubuntu 20.04 and ROS-Noetic. Please feel free to contact me anytime!

[yanzw1]

@iamrajee Thare are two more bugs in fast-planner codes, which are functions interpolateTrilinear and evaluateEDTWithGrad.
Their declarations both declaim a return value: L69-L72 but their definition miss a return value: L84-L105 L107-L118
their returns are unnecessary, so I changed them to return void.
wish to be useful to you.

Thanks! This really helped me out! Previous I encounter the same problem which is mentioned by @iamrajee . Now the simulation is good to go. Also, thank you teams from HKUST-Aerial-Robotics! I am also willing to keep contributing to this repo. If anyone still in the trouble working with Ubuntu 20.04 and ROS-Noetic. Please feel free to contact me anytime!

I met the same problem which is mentioned by @iamrajee .how could i fix it thanks！！@ba1Sta