How to launch the "FeasibleRRTstar" ?

[9woods123]

Hello！Thank you for opening such a great job！ I am trying to modify the planner_config （.yaml file）for using "FeasibleRRTStar" which is mentioned in the wiki. But when I modify the type from "RRTStar" to the "FeasibleRRTStar", the program cannot proceed. If you have a planner_config for "FeasibleRRTstar", could you please upload it? Looking for your kind reply.

Here is my config

#Config for 3D Reconstruction Planner
replan_pos_threshold: 0.15
replan_yaw_threshold: 0.15

max_new_segments: 0
min_new_segments: 0
max_new_tries: 0
min_new_tries: 0
min_new_value: 0

verbose_modules: true
build_modules_on_init: true
expand_batch: 1
visualize_gain: true
highlight_executed_trajectory: true

map:
  type: "VoxbloxMap"

system_constraints:
  v_max: 1.0
  a_max: 1.0
  yaw_rate_max: 1.6
  yaw_accel_max: 1.6
  collision_radius: 1.0

trajectory_generator:
  collision_optimistic: false
  bounding_volume_args: "/map_bounding_volume"
  clearing_radius: 0.0

  type: "FeasibleRRTStar"
  all_semgents_feasible: false
  crop_segments: true
  crop_margin: 0.3
  crop_min_length: 0.5
  min_path_length: 0.5
  max_extension_range: 1.5

  maximum_tries: 1000
  sampling_rate: 20.0
  sample_yaw: true

  sampling_mode: "semilocal"
  semilocal_sampling_count: 10
  semilocal_sampling_radius_max: 1.5
  semilocal_sampling_radius_min: 1.5

  rewire_root: true
  rewire_intermediate: true
  reinsert_root: true
  max_rewire_range: 1.6
  max_density_range: 1.0
  n_neighbors: 20

  generator_updater:
    type: "RecheckCollision"

trajectory_evaluator:
  type: "RRTStarEvaluatorAdapter"

  following_evaluator:
    type: "ContinuousYawPlanningEvaluator"
    n_directions: 12
    n_sections_fov: 3
    update_range: 3.0
    update_gain: 0.0
    update_sections_separate: false

    following_evaluator:
      type: "VoxelWeightEvaluator"
      visualize_sensor_view: true
      accurate_frontiers: true
      checking_distance: 2.0
      bounding_volume_args: "/target_bounding_volume"
      camera_params_ns: "/unreal/unreal_ros_client/camera_params"

      frontier_voxel_weight: 1.0
      new_voxel_weight: 0.0
      min_impact_factor: 0.01
      ray_angle_x: 0.002454
      ray_angle_y: 0.002681

      sensor_model:
        type: "IterativeRayCaster"
        sampling_time: 0.0
        ray_length: 5.0
        focal_length: 320.0
        resolution_x: 172 # 640-90/172-30/127-22.5 deg fov
        resolution_y: 480
        downsampling_factor: 5.0

      cost_computer:
        type: "SegmentTime"

      value_computer:
        type: "GlobalNormalizedGain"

      next_selector:
        type: "SubsequentBest"

back_tracker:
  type: "RotateReverse"
  turn_rate: 1.6
  update_rate: 0.5
  sampling_rate: 20.0
  n_rotations: 0
  stack_size: 10

and, the (error) output in the terminal:

[ INFO] [1647237917.189774603, 997.772000000]: Replanning!
(0.000243s elapsed, 1 new, 1 total, 1 killed by root change, 0 killed while updating)
terminate called after throwing an instance of 'std::bad_alloc'
  what():  std::bad_alloc
*** Aborted at 1647237917 (unix time) try "date -d @1647237917" if you are using GNU date ***
PC: @     0x7f00e59a7fb7 gsignal
*** SIGABRT (@0x3e800006a84) received by PID 27268 (TID 0x7f00e7a21c40) from PID 27268; stack trace: ***
    @     0x7f00e630d980 (unknown)
    @     0x7f00e59a7fb7 gsignal
    @     0x7f00e59a9921 abort
    @     0x7f00e5ffe957 (unknown)
    @     0x7f00e6004ae6 (unknown)
    @     0x7f00e6004b21 std::terminate()
    @     0x7f00e6004d54 __cxa_throw
    @     0x7f00e7427ce2 Eigen::internal::throw_std_bad_alloc()
    @     0x7f00e745f012 active_3d_planning::LinearMavTrajectoryGenerator::createTrajectory()
    @     0x7f00e74294ae active_3d_planning::trajectory_generator::FeasibleRRTStar::extractTrajectoryToPublish()
    @     0x7f00e6f7c262 active_3d_planning::OnlinePlanner::requestNextTrajectory()
    @     0x7f00e768a5b0 active_3d_planning::ros::RosPlanner::requestNextTrajectory()
    @     0x7f00e7687363 active_3d_planning::ros::RosPlanner::planningLoop()
    @     0x557d2b048ec3 (unknown)
    @     0x7f00e598abf7 __libc_start_main
    @     0x557d2b04906a (unknown)

[planner/planner_node-13] process has died [pid 27268, exit code -6, cmd /home/woods/exploration/planning/devel
/lib/active_3d_planning_app_reconstruction/reconstruction_planner_node planner_node/esdf_map_in:=voxblox_node
/esdf_map_out planner_node/tsdf_map_in:=voxblox_node/tsdf_map_out odometry:=/firefly/ground_truth/odometry 
command/trajectory:=/firefly/command/trajectory __name:=planner_node __log:=/home/woods/.ros/log/a8c8d80e-
a35c-11ec-b530-08beac02482b/planner-planner_node-13.log].log file: /home/woods/.ros/log/a8c8d80e-a35c-11ec-b530-08beac02482b/planner-planner_node-13*.log

[Schmluk]

Hi @9woods123

Thanks a lot for your interest! The main issue with FeasibleRRTStar is a problem with a downstream package (https://github.com/ethz-asl/mav_trajectory_generation) as far as I know, which we currently don't have the resources to fix.

If you use the regular RRTStar the outcome should be almost exactly the same in simulation and on a real robot with a decent controller (we tested this on one of our hexacopters too). So for the moment I'd recommend just sticking to RRTStar.

Hope this helps, let us know if you have more questions.

[9woods123]

Thanks for the reply, your RRTStar work is excellent. I am going to study the package [mav_trajectory_generation] that you have mentioned, maybe I will have more questions in the future.

[9woods123]

After my debug, I am sure the problem is in the

bool LinearMavTrajectoryGenerator::createTrajectory(
    const EigenTrajectoryPoint& start, const EigenTrajectoryPoint& goal,
    EigenTrajectoryPointVector* result) 

the code shut down when run the following sentence ：

EigenTrajectoryPointVector states = mavMsgToPlanningMsg(tmp_states); where：

inline const EigenTrajectoryPointVector& mavMsgToPlanningMsg(
   const :: mav_msgs::EigenTrajectoryPointVector& mavTraj) {
  return *reinterpret_cast<const EigenTrajectoryPointVector*>(&mavTraj);
}

Maybe you have solution to the *reinterpret_cast<const EigenTrajectoryPointVector*>(&mavTraj);？ I think the problem is right here.

[9woods123]

I try to turn the mav_msgs::EigenTrajectoryPoint to the EigenTrajectoryPoint like following：

EigenTrajectoryPointVector R;
  for( auto &ts : tmp_states)
  {
 EigenTrajectoryPoint r= *reinterpret_cast<const EigenTrajectoryPoint*> (&ts) ;
 R.push_back(r);

  }

  *result = R;

And it works well !

[Schmluk]

Great to hear! Please feel free to open a PR for this fix, this might also be useful for others!

[Schmluk]

Thank you for your investigations and contribution! This issue should be fixed as of #50 .