EdgeAcceleration::computeError() in teb local planner simulated with gazebo

asked 2020-02-13 02:07:11 -0600

madmax gravatar image

I have an issue with the planner crashing after printing these errors:

[teb_local_planner] [ERROR]     EdgeAcceleration::computeError() rotational: _error[1]=-nan
[controller_server-1] 
[controller_server-1] 1581580042.028181405: [osg_1.controller_server_rclcpp_node] [ERROR]   Action server failed while executing action callback: "EdgeAcceleration::computeError() rotational: _error[1]=-nan
[controller_server-1] "

Until now it happened only in gazebo, on the real robot it didn't happen yet.
Can't figure out the cause of this error, as robot params are the same in real world and simulation.
Has this seen someone before?

Sytem:
ROS2 Eloquent
teb_local_planner: eloquent sources
g2o lib: latest master sources
nav2: latest master sources

Controller params:

use_sim_time: True
controller_frequency: 20.
controller_plugin_ids: ["FollowPath"]
controller_plugin_types: ["teb_local_planner::TebLocalPlannerROS"]
min_theta_velocity_threshold: 0.0001
min_x_velocity_threshold: 0.0001
min_y_velocity_threshold: 0.0001
FollowPath.acc_lim_theta: 3.
FollowPath.acc_lim_x: 2.
FollowPath.acc_lim_y: 0.
FollowPath.allow_init_with_backwards_motion: False
FollowPath.cmd_angle_instead_rotvel: False
FollowPath.complete_global_plan: True
FollowPath.costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"
FollowPath.costmap_converter_spin_thread: True
FollowPath.costmap_obstacles_behind_robot_dist: 1.5
FollowPath.delete_detours_backwards: True
FollowPath.detours_orientation_tolerance: 1.5708
FollowPath.dt_hysteresis: 0.1
FollowPath.dt_ref: 0.3
FollowPath.dynamic_obstacle_inflation_dist: 0.6
FollowPath.enable_homotopy_class_planning: False
FollowPath.enable_multithreading: True
FollowPath.exact_arc_length: False
FollowPath.feasibility_check_no_poses: 5
#FollowPath.footprint_model.front_offset: NOT_SET
#FollowPath.footprint_model.front_radius: NOT_SET
#FollowPath.footprint_model.line_end: NOT_SET
#FollowPath.footprint_model.line_start: NOT_SET
FollowPath.footprint_model.radius: 0.8
#FollowPath.footprint_model.rear_offset: NOT_SET
#FollowPath.footprint_model.rear_radius: NOT_SET
FollowPath.footprint_model.type: "circular"
#FollowPath.footprint_model.vertices: NOT_SET
FollowPath.force_reinit_new_goal_dist: 1.
FollowPath.free_goal_vel: False
FollowPath.global_plan_overwrite_orientation: True
FollowPath.global_plan_prune_distance: 1.
FollowPath.global_plan_viapoint_sep: -1. # defines the minimum separation between two consecutive via-points along the global plan (in meters)
FollowPath.h_signature_prescaler: 1.
FollowPath.h_signature_threshold: 0.1
FollowPath.include_costmap_obstacles: True
FollowPath.include_dynamic_obstacles: True
FollowPath.inflation_dist: 0.1
FollowPath.is_footprint_dynamic: False
FollowPath.legacy_obstacle_association: False
FollowPath.length_start_orientation_vector: 0.4
FollowPath.map_frame: "map"
FollowPath.max_global_plan_lookahead_dist: 2.
FollowPath.max_number_classes: 5
FollowPath.max_ratio_detours_duration_best_duration: 3.
FollowPath.max_samples: 20
FollowPath.max_vel_theta: 1.
FollowPath.max_vel_x: 1.3
FollowPath.max_vel_x_backwards: 0.3
FollowPath.max_vel_y: 0.0
FollowPath.min_obstacle_dist: 0.1
FollowPath.min_resolution_collision_check_angular: 3.14159
FollowPath.min_samples: 3
FollowPath.min_turning_radius: 0.0
FollowPath.no_inner_iterations: 5
FollowPath.no_outer_iterations: 4
FollowPath.obstacle_association_cutoff_factor: 5.
FollowPath.obstacle_association_force_inclusion_factor: 1.5
FollowPath.obstacle_cost_exponent: 1.
FollowPath.obstacle_heading_threshold: 0.45
FollowPath.obstacle_keypoint_offset: 0.1
FollowPath.obstacle_poses_affected: 25
FollowPath.odom_topic: "odom"
FollowPath.optimization_activate: True
FollowPath.optimization_verbose: False
FollowPath.oscillation_filter_duration: 10.
FollowPath.oscillation_omega_eps: 0.1
FollowPath.oscillation_recovery: True
FollowPath.oscillation_recovery_min_duration: 10.
FollowPath.oscillation_v_eps: 0.1
FollowPath.penalty_epsilon: 0.1
FollowPath.publish_feedback: False
FollowPath.roadmap_graph_area_length_scale: 1.
FollowPath.roadmap_graph_area_width: 6.
FollowPath.roadmap_graph_samples: 15
FollowPath.selection_alternative_time_cost: False
FollowPath.selection_cost_hysteresis: 1.
FollowPath.selection_obst_cost_scale: 100.
FollowPath.selection_prefer_initial_plan: 0.95
FollowPath.selection_viapoint_cost_scale: 1.
FollowPath.shrink_horizon_backup: True
FollowPath.shrink_horizon_min_duration: 10.
FollowPath.simple_exploration: False
FollowPath.switching_blocking_period: 0.
FollowPath.teb_autosize: 1.
FollowPath.via_points_ordered: False
FollowPath.viapoints_all_candidates: True
FollowPath.visualize_hc_graph: True
FollowPath.visualize_with_time_as_z_axis_scale: 0.
FollowPath.weight_acc_lim_theta: 1.
FollowPath.weight_acc_lim_x: 1.
FollowPath.weight_acc_lim_y: 1.
FollowPath.weight_adapt_factor: 2.
FollowPath.weight_dynamic_obstacle: 50.
FollowPath.weight_dynamic_obstacle_inflation: 0.1
FollowPath.weight_inflation: 0.1
FollowPath.weight_kinematics_forward_drive: 1.
FollowPath.weight_kinematics_nh: 1000.
FollowPath.weight_kinematics_turning_radius: 1.
FollowPath.weight_max_vel_theta: 1.
FollowPath.weight_max_vel_x: 2.
FollowPath.weight_max_vel_y: 2.
FollowPath.weight_obstacle: 50.
FollowPath.weight_optimaltime: 1.
FollowPath.weight_prefer_rotdir: 50.
FollowPath.weight_shortest_path: 0.0
FollowPath.weight_viapoint: 1.
FollowPath.wheelbase: 1. # only required if cmd_angle_instead_rotvelis set to true
FollowPath.xy_goal_tolerance: 0.15
FollowPath.yaw_goal_tolerance: 0.15
edit retag flag offensive close merge delete