Path taken by TEB is not showing & costmap conversion is not working

[tiara567]

Global planner : navfn Local planner : TEB

the path taken by the TEB is not showing. Please see the following video. Screencast from 09-06-2022 10_32_39 AM.webm

It think that the TEB should shows its way (small red arrows).

Besides, i have tried to plugin the map converter (point obstacle to polygon). However it is still not working (same video) before error: No costmap conversion plugin specified. All occupied costmap cells are treaten as point obstacles. after : Costmap conversion plugin costmap_converter::CostmapToPolygonsDBSMCCH loaded.

[jcmonteiro]

Could you share your parameter files?

[tiara567]

Here is it.

costmap_common_params_burger.yaml

obstacle_range: 3.0 raytrace_range: 3.5

footprint: [[-0.105, -0.105], [-0.105, 0.105], [0.041, 0.105], [0.041, -0.105]]

robot_radius: 0.105

inflation_radius: 1.0 cost_scaling_factor: 3.0

map_type: costmap observation_sources: scan scan: {sensor_frame: base_scan, data_type: LaserScan, topic: scan, marking: true, clearing: true}

local_costmap_params.yaml

local_costmap: global_frame: odom robot_base_frame: base_footprint

update_frequency: 10.0 publish_frequency: 10.0 transform_tolerance: 0.5

static_map: false
rolling_window: true width: 3 height: 3 resolution: 0.05

global_costmap_params.yaml

global_costmap: global_frame: map robot_base_frame: base_footprint

update_frequency: 10.0 publish_frequency: 10.0 transform_tolerance: 0.5

static_map: true

move_base_params

shutdown_costmaps: false controller_frequency: 10.0 planner_patience: 5.0 controller_patience: 15.0 conservative_reset_dist: 3.0 planner_frequency: 5.0 oscillation_timeout: 10.0 oscillation_distance: 0.2

TebLocalPlannerROS:

odom_topic: odom map_frame: /odom

Costmap converter plugin
costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH" costmap_converter_spin_thread: True costmap_converter_rate: 5

Configure plugins (namespace move_base/TebLocalPlannerROS/PLUGINNAME) The parameters must be added for each plugin separately costmap_converter/CostmapToLinesDBSRANSAC: cluster_max_distance: 0.4 cluster_min_pts: 2 ransac_inlier_distance: 0.15 ransac_min_inliers: 10 ransac_no_iterations: 2000 ransac_remainig_outliers: 3 ransac_convert_outlier_pts: True ransac_filter_remaining_outlier_pts: False convex_hull_min_pt_separation: 0.1

Trajectory teb_autosize : true dt_ref : 0.3 dt_hysteresis : 0.1 min_samples : 3 max_samples : 500 global_plan_overwrite_orientation : true allow_init_with_backwards_motion : false global_plan_viapoint_sep : -1 via_points_ordered : false max_global_plan_lookahead_dist : 1 global_plan_prune_distance : 1 exact_arc_length : false force_reinit_new_goal_dist : 1 force_reinit_new_goal_angular : 0.5 * M_PI feasibility_check_no_poses : 5 publish_feedback : false min_resolution_collision_check_angular : M_PI control_look_ahead_poses : 1 prevent_look_ahead_poses_near_goal : 0

Robot

max_vel_x : 0.4 max_vel_x_backwards : 0.2 max_vel_y : 0.0 max_vel_trans : 0.0 max_vel_theta : 0.3 acc_lim_x : 0.2 acc_lim_y : 0.2 acc_lim_theta : 0.5 min_turning_radius : 0 wheelbase : 1.0 cmd_angle_instead_rotvel : false is_footprint_dynamic : false use_proportional_saturation : false

GoalTolerance

xy_goal_tolerance : 0.2 yaw_goal_tolerance : 0.2 free_goal_vel : false trans_stopped_vel : 0.1 theta_stopped_vel : 0.1 complete_global_plan : true

Obstacles

min_obstacle_dist : 0.1 inflation_dist : 0.15 dynamic_obstacle_inflation_dist : 0.15 include_dynamic_obstacles : true include_costmap_obstacles : true costmap_obstacles_behind_robot_dist : 1.5 obstacle_poses_affected : 25 legacy_obstacle_association : false obstacle_association_force_inclusion_factor : 1.5 obstacle_association_cutoff_factor : 5 costmap_converter_plugin : "" costmap_converter_spin_thread : true costmap_converter_rate : 5 obstacle_proximity_ratio_max_vel : 1 obstacle_proximity_lower_bound : 0 obstacle_proximity_upper_bound : 0.5

Optimization

no_inner_iterations : 5 no_outer_iterations : 4 optimization_activate : true optimization_verbose : false penalty_epsilon : 0.1 weight_max_vel_x : 2 weight_max_vel_y : 2 weight_max_vel_theta : 1 weight_acc_lim_x : 1.0 weight_acc_lim_y : 1.0 weight_acc_lim_theta : 1 weight_kinematics_nh : 1000 weight_kinematics_forward_drive : 1 weight_kinematics_turning_radius : 1 weight_optimaltime : 1 weight_shortest_path : 0 weight_obstacle : 100 weight_inflation : 0.1 weight_dynamic_obstacle : 50 weight_dynamic_obstacle_inflation : 0.1 weight_velocity_obstacle_ratio : 0 weight_viapoint : 1 weight_prefer_rotdir : 50

weight_adapt_factor : 2.0 obstacle_cost_exponent : 4.0

Homotopy Class Planner

enable_homotopy_class_planning : true enable_multithreading : true simple_exploration : false max_number_classes : 5 selection_cost_hysteresis : 1.0 selection_prefer_initial_plan : 0.95 selection_obst_cost_scale : 100.0 selection_viapoint_cost_scale : 1.0 selection_alternative_time_cost : false selection_dropping_probability : 0.0

obstacle_keypoint_offset : 0.1 obstacle_heading_threshold : 0.45 roadmap_graph_no_samples : 15 roadmap_graph_area_width : 6 // [m] roadmap_graph_area_length_scale : 1.0 h_signature_prescaler : 1 h_signature_threshold : 0.1 switching_blocking_period : 0.0

viapoints_all_candidates : true

visualize_hc_graph : false visualize_with_time_as_z_axis_scale : 0.0 delete_detours_backwards : true detours_orientation_tolerance : M_PI / 2.0 length_start_orientation_vector : 0.4 max_ratio_detours_duration_best_duration : 3.0

Recovery

shrink_horizon_backup : true shrink_horizon_min_duration : 10 oscillation_recovery : true oscillation_v_eps : 0.1 oscillation_omega_eps : 0.1 oscillation_recovery_min_duration : 10 oscillation_filter_duration : 10

costmap_converter_params.yaml

########################################################################################### NOTE: Costmap conversion is experimental. Its purpose is to combine many point ## obstales into clusters, computed in a separate thread in order to improve the overall ## efficiency of local planning. However, the implemented conversion algorithms are in a ## very early stage of development. Contributions are welcome! ## ###########################################################################################

TebLocalPlannerROS:

Costmap converter plugin
costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"

costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSRANSAC"

costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSMCCH"

costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSConcaveHull"

costmap_converter_spin_thread: True costmap_converter_rate: 5

Configure plugins (namespace move_base/costmap_to_lines or move_base/costmap_to_polygons) costmap_converter/CostmapToLinesDBSRANSAC, costmap_converter/CostmapToLinesDBSMCCH, costmap_converter/CostmapToPolygonsDBSMCCH costmap_converter/CostmapToLinesDBSRANSAC: cluster_max_distance: 0.4 cluster_min_pts: 2 ransac_inlier_distance: 0.15 ransac_min_inliers: 10 ransac_no_iterations: 2000 ransac_remainig_outliers: 3 ransac_convert_outlier_pts: True ransac_filter_remaining_outlier_pts: False convex_hull_min_pt_separation: 0.1

[tiara567]

here my launch file move_base.launch

[tiara567]

here is my launch file turtlebot3_navigation.launch

[jcmonteiro]

You have publish_feedback set to false. Could you change it to true and see if it helps?

[tiara567]

I have tried to change it to "true" but still nothing changes. Yesterday, i reconfigured my rviz parameter, it change to this. PLease se following video: Screencast from 09-07-2022 04:31:35 PM.webm

The path of TEB planner is shown ( yellow path, without the red-small arrow) and the costmap has changed but the polygon has not shown yet.

[jcmonteiro]

Check your RViz configuration then, as it seems like everything is fine from TEB's side. Make sure you are using the correct plugin to visualize the trajectory and that it is configured to show the arrows.

[tiara567]

Attached is the configuration of my rviz setting. Which one that need to be configured, Sir?

Panels:

• Class: rviz/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded:
  • /TF1/Frames1
  • /TF1/Tree1
  • /Global Map1
  • /Global Map1/Costmap1
  • /Global Map1/Planner1
  • /Local Map1
  • /Local Map1/Polygon1
  • /Local Map1/Costmap1
  • /Local Map1/Planner1 Splitter Ratio: 0.5 Tree Height: 804
• Class: rviz/Selection Name: Selection
• Class: rviz/Tool Properties Expanded:
  • /2D Pose Estimate1
  • /2D Nav Goal1 Name: Tool Properties Splitter Ratio: 0.5886790156364441
• Class: rviz/Views Expanded:
  • /Current View1 Name: Views Splitter Ratio: 0.5
• Class: rviz/Time Experimental: false Name: Time SyncMode: 0 SyncSource: LaserScan Preferences: PromptSaveOnExit: true Toolbars: toolButtonStyle: 2 Visualization Manager: Class: "" Displays:
  • Alpha: 0.5 Cell Size: 1 Class: rviz/Grid Color: 160; 160; 164 Enabled: true Line Style: Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 Offset: X: 0 Y: 0 Z: 0 Plane: XY Plane Cell Count: 20 Reference Frame: Value: true
  • Alpha: 1 Class: rviz/RobotModel Collision Enabled: false Enabled: true Links: All Links Enabled: true Expand Joint Details: false Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order base_footprint: Alpha: 1 Show Axes: false Show Trail: false base_link: Alpha: 1 Show Axes: false Show Trail: false Value: true base_scan: Alpha: 1 Show Axes: false Show Trail: false Value: true caster_back_link: Alpha: 1 Show Axes: false Show Trail: false Value: true imu_link: Alpha: 1 Show Axes: false Show Trail: false wheel_left_link: Alpha: 1 Show Axes: false Show Trail: false Value: true wheel_right_link: Alpha: 1 Show Axes: false Show Trail: false Value: true Name: RobotModel Robot Description: robot_description TF Prefix: "" Update Interval: 0 Value: true Visual Enabled: true
  • Class: rviz/TF Enabled: false Frame Timeout: 15 Frames: All Enabled: false Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: false Tree: {} Update Interval: 0 Value: false
  • Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/LaserScan Color: 0; 255; 0 Color Transformer: FlatColor Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 Name: LaserScan Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.030000001192092896 Style: Flat Squares Topic: /scan Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true
  • Class: rviz/Image Enabled: false Image Topic: /raspicam_node/image Max Value: 1 Median window: 5 Min Value: 0 Name: Image Normalize Range: true Queue Size: 2 Transport Hint: compressed Unreliable: false Value: false
  • Alpha: 0.699999988079071 Class: rviz/Map Color Scheme: map Draw Behind: false Enabled: true Name: Map Topic: /map Unreliable: false Use Timestamp: false Value: true
  • Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 0; 0; 0 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 Name: Planner Plan Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /move_base/NavfnROS/plan Unreliable: false Value: true
  • Class: rviz/Group Displays:
    • Alpha: 0.699999988079071 Class: rviz/Map Color Scheme: costmap Draw Behind: true Enabled: true Name: Costmap Topic: /move_base/global_costmap/costmap Unreliable: false Use Timestamp: false Value: true
    • Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 0; 0 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 Name: Planner Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /move_base/NavfnROS/plan Unreliable: false Value: true Enabled: true Name: Global Map
  • Class: rviz/Group Displays:
    • Alpha: 1 Class: rviz/Polygon Color: 0; 0; 0 Enabled: true Name: Polygon Topic: /move_base/local_costmap/footprint Unreliable: false Value: true
    • Alpha: 0.699999988079071 Class: rviz/Map Color Scheme: costmap Draw Behind: false Enabled: true Name: Costmap Topic: /move_base/local_costmap/costmap Unreliable: false Use Timestamp: false Value: true
    • Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 255; 0 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 Name: Planner Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /move_base/TebLocalPlannerROS/local_plan Unreliable: false Value: true Enabled: true Name: Local Map
  • Alpha: 1 Arrow Length: 0.05000000074505806 Axes Length: 0.30000001192092896 Axes Radius: 0.009999999776482582 Class: rviz/PoseArray Color: 0; 192; 0 Enabled: true Head Length: 0.07000000029802322 Head Radius: 0.029999999329447746 Name: Amcl Particles Shaft Length: 0.23000000417232513 Shaft Radius: 0.009999999776482582 Shape: Arrow (Flat) Topic: /particlecloud Unreliable: false Value: true
  • Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 Class: rviz/Pose Color: 255; 25; 0 Enabled: true Head Length: 0.30000001192092896 Head Radius: 0.10000000149011612 Name: Goal Shaft Length: 0.5 Shaft Radius: 0.05000000074505806 Shape: Arrow Topic: /move_base_simple/goal Unreliable: false Value: true Enabled: true Global Options: Background Color: 48; 48; 48 Default Light: true Fixed Frame: map Frame Rate: 30 Name: root Tools:
  • Class: rviz/MoveCamera
  • Class: rviz/Interact Hide Inactive Objects: true
  • Class: rviz/Select
  • Class: rviz/SetInitialPose Theta std deviation: 0.2617993950843811 Topic: /initialpose X std deviation: 0.5 Y std deviation: 0.5
  • Class: rviz/SetGoal Topic: /move_base_simple/goal
  • Class: rviz/Measure Value: true Views: Current: Angle: -1.5707963705062866 Class: rviz/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 Scale: 100 Target Frame: Value: TopDownOrtho (rviz) X: 0 Y: 0 Saved: ~ Window Geometry: Displays: collapsed: false Height: 1027 Hide Left Dock: false Hide Right Dock: true Image: collapsed: false QMainWindow State: 000000ff00000000fd00000004000000000000016a000003adfc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003ad000000c700fffffffb0000000a0049006d0061006700650000000317000000cc0000001600fffffffb0000000a0049006d0061006700650000000330000000ce0000000000000000000000010000010f000003a0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000043000003a0000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000042700fffffffb0000000800540069006d006501000000000000045000000000000000000000038f000003ad00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: true Width: 1279 X: 67 Y: 27

[jcmonteiro]

If you play around with the rviz/Path plugin you use to visualize TEB you should be able to figure it out.

[tiara567]

Yes, Sir. It's worked. I add TEB visualization by topic on rviz. Screencast from 09-09-2022 01:34:36 PM.webm

Thank you for the insight. I hope it can help others too