0
$\begingroup$

Rosanswers logo

I have been trying to test out the teb_local_planner in place of the DWA_local planner that the ROS navigation stack uses. I'm basicalIy trying to see if teb_local_planner is better for creating paths when there are obstacles involved in certain scenarios. I have been using the Tutorial found in the ROS wiki to figure out how to get it to work (http://wiki.ros.org/teb_local_planner/Tutorials/Configure%20and%20run%20Robot%20Navigation). I can set goal on rviz and a path is created but the robot is unable to move to them, for instance the robot moves but is unable to get to the goal and behaves like obstacles are in its way so it just moves back and forth. Inother cases the robot reaches the goal but then moves passed it or away from it for some reason. here is a picture of an instance where the robot is unable to complete it's journey: Robot_cant_move_to_goal

I have tried to attach pictures of relevant .launch files and .yaml files which might contain parameters I don't know how to change to fix my issues. Any help would be much appreciated.

Here is my turtlebot3_navigation.launch file below

<launch>
  <!-- Arguments -->
  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
  <arg name="map_file" default="$(find turtlebot3_navigation)/maps/map.yaml"/>
  <arg name="open_rviz" default="true"/>
  <arg name="move_forward_only" default="false"/>

  <!-- Turtlebot3 -->
  <include file="$(find turtlebot3_bringup)/launch/turtlebot3_remote.launch">
    <arg name="model" value="$(arg model)" />
  </include>

  <!-- Map server -->
  <node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)"/>

  <!-- AMCL -->
  <include file="$(find turtlebot3_navigation)/launch/amcl.launch"/>

  <!-- move_base -->
  <include file="$(find turtlebot3_navigation)/launch/move_base.launch">
    <arg name="model" value="$(arg model)" />
    <arg name="move_forward_only" value="$(arg move_forward_only)"/>
  </include>

  <!-- rviz -->
  <group if="$(arg open_rviz)"> 
    <node pkg="rviz" type="rviz" name="rviz" required="true"
          args="-d $(find turtlebot3_navigation)/rviz/turtlebot3_navigation.rviz"/>
  </group>
</launch>

which opens up the rviz when you roslaunch. I get these Warning and error messages in my terminal("TebOptimalPlanner::getVelocityCommand() - timediff<=0 is invalid") here:

[ WARN] [1642015893.008329450, 157.973000000]: Control loop missed its desired rate of 10.0000Hz... the loop actually took 0.1560 seconds
[ WARN] [1642015893.008403174, 157.973000000]: Map update loop missed its desired rate of 10.0000Hz... the loop actually took 0.1560 seconds
[ WARN] [1642015893.153565037, 158.116000000]: Control loop missed its desired rate of 10.0000Hz... the loop actually took 0.1990 seconds
[ WARN] [1642015893.297226947, 158.259000000]: Control loop missed its desired rate of 10.0000Hz... the loop actually took 0.2420 seconds
[ WARN] [1642015893.297362902, 158.259000000]: Map update loop missed its desired rate of 10.0000Hz... the loop actually took 0.3420 seconds
[ WARN] [1642015893.426782063, 158.388000000]: Control loop missed its desired rate of 10.0000Hz... the loop actually took 0.1290 seconds
[ WARN] [1642015893.564398820, 158.524000000]: Control loop missed its desired rate of 10.0000Hz... the loop actually took 0.1650 seconds
[ WARN] [1642015893.564452689, 158.524000000]: Map update loop missed its desired rate of 10.0000Hz... the loop actually took 0.2650 seconds
[ WARN] [1642015893.696767613, 158.656000000]: Control loop missed its desired rate of 10.0000Hz... the loop actually took 0.1960 seconds
[ WARN] [1642015893.696972577, 158.656000000]: Map update loop missed its desired rate of 10.0000Hz... the loop actually took 0.1320 seconds
[ WARN] [1642015893.832297292, 158.790000000]: Control loop missed its desired rate of 10.0000Hz... the loop actually took 0.2310 seconds
[ WARN] [1642015893.938082536, 158.891000000]: Control loop missed its desired rate of 10.0000Hz... the loop actually took 0.1010 seconds

This is my former move_base.launch file which uses the DWA planner-- Former_move_base.launch:

<launch>
  <!-- Arguments -->
  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
  <arg name="cmd_vel_topic" default="/cmd_vel" />
  <arg name="odom_topic" default="odom" />
  <arg name="move_forward_only" default="false"/>

  <!-- move_base -->
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
    <rosparam file="$(find turtlebot3_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find turtlebot3_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find turtlebot3_navigation)/param/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find turtlebot3_navigation)/param/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find turtlebot3_navigation)/param/move_base_params.yaml" command="load" />
    <rosparam file="$(find turtlebot3_navigation)/param/dwa_local_planner_params_$(arg model).yaml" command="load" />
    <remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
    <remap from="odom" to="$(arg odom_topic)"/>
    <param name="DWAPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" />
    <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
  </node>
</launch>

This is my 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
#0.106
inflation_radius: 0.05
#cost_scaling_factor: 1.5

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

This is my current move_base.launch file which uses the teb_local_planner -- Current_move_base.launch:

<launch>
  <!-- Arguments -->
  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
  <arg name="cmd_vel_topic" default="/cmd_vel" />
  <arg name="odom_topic" default="odom" />
  <arg name="move_forward_only" default="false"/>

  <!-- move_base -->
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
    <rosparam file="$(find turtlebot3_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find turtlebot3_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find turtlebot3_navigation)/param/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find turtlebot3_navigation)/param/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find turtlebot3_navigation)/param/move_base_params.yaml" command="load" />
    <rosparam file="$(find turtlebot3_navigation)/param/dwa_local_planner_params_$(arg model).yaml" command="load" />
    <remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
    <remap from="odom" to="$(arg odom_topic)"/>
    <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
    <param name="controller_frequency" value="10.0" />
  </node>
</launch>

This is my 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
#0.106
inflation_radius: 0.05
#cost_scaling_factor: 1.5

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

This is my base_local_planner_params.yaml file:

TebLocalPlannerROS:
  odom_topic: odom
  map_frame: /odom
    
# Trajectory
  
  teb_autosize: True
  dt_ref: 0.3
  dt_hysteresis: 0.1
  global_plan_overwrite_orientation: True
  max_global_plan_lookahead_dist: 3.0
  feasibility_check_no_poses: 5
    
 
# Robot Configuration Parameters
  max_vel_x: 0.18
  max_vel_x_backwards: 0.1
  min_vel_x: 0.08

  max_vel_theta:  1.0
  min_vel_theta: -1.0
  min_in_place_vel_theta: 1.0

  acc_lim_x: 1.0
  acc_lim_y: 0.0
  acc_lim_theta: 0.6
  min_turning_radius: 0.0
  footprint_model: # types: "point", "circular", "line", "two_circles", "polygon"
   type: "radius"
   radius: 0.105 # for type "circular" 0.2
   line_start: [-0.3, 0.0] # for type "line"
   line_end: [0.3, 0.0] # for type "line"
   front_offset: 0.2 # for type "two_circles"
   front_radius: 0.2 # for type "two_circles"
   rear_offset: 0.2 # for type "two_circles"
   rear_radius: 0.2 # for type "two_circles"
   vertices: [ [0.25, -0.05], [0.18, -0.05], [0.18, -0.18], [-0.19, -0.18], [-0.25, 0], [-0.19, 0.18], [0.18, 0.18], [0.18, 0.05], [0.25, 0.05] ] # for type "polygon"
  

# Goal Tolerance Parameters
  xy_goal_tolerance: 0.10
  yaw_goal_tolerance: 0.05
  free_goal_vel: False

# Obstacles
  min_obstacle_dist: 0.4
  include_costmap_obstacles: True
  costmap_obstacles_behind_robot_dist: 1.0
  obstacle_poses_affected: 30
  costmap_converter_plugin: ""
  costmap_converter_spin_thread: True
  costmap_converter_rate: 5


# Optimization
  no_inner_iterations: 1 #5
  no_outer_iterations: 1 #4
  optimization_activate: True
  optimization_verbose: False
  penalty_epsilon: 0.1
  weight_max_vel_x: 2
  weight_max_vel_theta: 1
  weight_acc_lim_x: 1
  weight_acc_lim_theta: 1
  weight_kinematics_nh: 1000
  weight_kinematics_forward_drive: 1
  weight_kinematics_turning_radius: 1 #1
  weight_optimaltime: 1
  weight_obstacle: 12 #50
  weight_dynamic_obstacle: 10 # not in use yet
  alternative_time_cost: False # not in use yet

# Homotopy Class Planner
  enable_homotopy_class_planning: True
  enable_multithreading: True
  simple_exploration: False
  max_number_classes: 4
  roadmap_graph_no_samples: 15
  roadmap_graph_area_width: 5
  h_signature_prescaler: 0.5
  h_signature_threshold: 0.1
  obstacle_keypoint_offset: 0.1
  obstacle_heading_threshold: 0.45
  visualize_hc_graph: False
  
  
# Differential-drive robot configuration
  holonomic_robot: false

# Forward Simulation Parameters
  sim_time: 0.8
  vx_samples: 18
  vtheta_samples: 20
  sim_granularity: 0.05

On further research I have learned that the teb_local_planner requires the costmap_converter plugin to be able to make convergent paths. I also need help trying to figure out how to get the converter working.


Originally posted by distro on ROS Answers with karma: 167 on 2022-01-12

Post score: 1


Original comments

Comment by osilva on 2022-01-12:
Hi @distro, any reason why you commented cost_scaling_factor: 1.5?

Comment by distro on 2022-01-12:
@osilva, the # infront of the cost factor comments it out so I'm not using that at all

Comment by osilva on 2022-01-12:
Take a look at this guide for navigation for turtlebot3: https://emanual.robotis.com/docs/en/platform/turtlebot3/navigation/

It also includes this guide that explains step by step how to adjust each parameter and it’s purpose: https://kaiyuzheng.me/documents/navguide.pdf

Comment by osilva on 2022-01-12:
But cost scaling factor is important so I was surprised to see it commented

Comment by distro on 2022-01-12:
@osilva when I was using the DWA_local_planner I also had the cost scaling factor commented out, it didnt affect anything,everything ran smoothly. I dont think thats what the issue is

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

I figured it out, for turtlebot3 this is how you edit your move_base.launch file to use teb_local_planner :

    <launch>
      <!-- Arguments -->
      <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
      <arg name="cmd_vel_topic" default="/cmd_vel" />
      <arg name="odom_topic" default="odom" />
      <arg name="move_forward_only" default="false"/>
    
      <!-- move_base -->
      <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
        <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS"/>
        <param name="base_global_planer" value="global_planner/GlobalPlanner"/>
        <param name="controller_frequency" value="10.0" />
        <rosparam file="$(find turtlebot3_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find turtlebot3_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find turtlebot3_navigation)/param/local_costmap_params.yaml" command="load" />
         <rosparam file="$(find turtlebot3_navigation)/param/base_local_planner_params.yaml" command="load" />
        <rosparam file="$(find turtlebot3_navigation)/param/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find turtlebot3_navigation)/param/move_base_params.yaml" command="load" />
        <remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
        <remap from="odom" to="$(arg odom_topic)"/>
      </node>
      <node pkg="teb_local_planner" type="cmd_vel_to_ackermann_drive.py" name="cmd_vel_to_ackermann"> 
      </node>
</launch>

Originally posted by distro with karma: 167 on 2022-01-27

This answer was ACCEPTED on the original site

Post score: 3

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.