Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

Robot drifts away from goal and laserscan skews on random interval

I have to apologize for the vague and/or convoluted title. We don't know where the issue is ourselves.

The robot I'm working on makes use of ROS Kinetic, the standard navigation stack with amcl, teb_local_planner, laser_scan_matcher and move_base. It is a front-wheeled differential drive with two caster wheels on the back with a rectangular body/footprint, built on the chassis of an electric wheelchair.

The issue we're facing is happening on a random interval: Whenever we set a goal in rviz, the robot does move towards his goal. After a random amount of time or after a certain distance has been traveled, two errors could occur:

The robot loses track of his position by "teleporting" on the map within rviz, commonly setting himself approx. 1 meter to the right. The laser scan output could also become skewed, throwing off its odometry. Each of these erratic behaviours is visible within rviz and the console output can mention the errors Map Update/Control loop missed its desired rate <...>

As a result, the robot would keep on its last set velocity (cmd_vel output) and rviz would no longer be able to set and display a new goal, rendering the robot unresponsive. We're able to clear cmd_vel by using manual control (teleop_twist_joy) and stopping the robot that way. Upon hitting the reset button on the bottom-right in rviz, we progressively lose the local and global costmap per press, accompanied by the warning "No map provided". Rebooting the ROS environment will restore everything until the "drift" occurs again.

We can consistently recreate this behaviour by having the robot drive various distances, believing it appears more often when traversing small distances of approximately 2-3 meters, on longer distances i.e 5-20 meters the errors occure less often, but the laserscan becoming askew is still prevalent. We're hoping to get video footage of the erroneous behaviour in order to demonstrate what's happening both physically and in rviz.

Below are the common configuration files requested:

  • navigation.launch

<launch> <arg name="sim" default="false"/> <arg name="path"/>

<node if="$(eval sim == 'true')" pkg="rosbag" type="play" name="play" args="$(arg path)/bags/t5_lidar.bag --delay=5 --clock"/>

    <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /laser 40" />

    <node pkg="tf" type="static_transform_publisher" name="base_link_to_sonar_front" args="0.0 0 0 0.0 0.0 0.0 /base_link /front 40"/>
    <node pkg="tf" type="static_transform_publisher" name="base_link_to_sonar_right" args="0.0 0 0 -1.57 0.0 0.0 /base_link /backRight 40"/>
    <node pkg="tf" type="static_transform_publisher" name="base_link_to_sonar_left" args="0.0 0 0 1.57 0.0 0.0 /base_link /backLeft 40"/>

    <node pkg="tf" type="static_transform_publisher" name="world_to_map" args="0.0 0 0 0.0 0.0 0.0 /world /map 40"/>
    <node pkg="tf" type="static_transform_publisher" name="world_to_map_nav" args="0.0 0 0 0.0 0.0 0.0 /world /map_nav 40"/>

<!-- <node pkg="laser_filters" type="scan_to_scan_filter_chain" name="laser_filter">
        <rosparam command="load" file="$(arg path)/config/laser_filter_config.yaml" />
</node> -->


    <node pkg="rviz" type="rviz" name="rviz" args="-d $(arg path)/navigation.rviz"/>

    <node pkg="laser_scan_matcher" type="laser_scan_matcher_node" name="laser_scan_matcher_node" output="screen">
        <param name="base_frame" value = "/base_link"/>
        <param name="fixed_frame" value = "/odom"/>
        <param name="vel" value="/cmd_vel" />
        <param name="use_cloud_input" value="false"/>
        <param name="publish_tf" value="true"/>
        <param name="publish_odom" value="true"/>
        <param name="use_odom" value="false"/>
        <param name="use_imu" value="false"/>
        <param name="use_vel" value="true"/>
        <param name="use_alpha_beta" value="true"/>
        <param name="max_iterations" value="10"/>
        <remap from="scan" to="scan_filtered" />
    </node>

    <node pkg="map_server" type="map_server" name="map_server" args="$(arg path)/maps/T5_full_small.yaml" output="screen"/>
    <node pkg="map_server" type="map_server" name="map_server_nav" args="$(arg path)/maps/T5_full_small_move_base.yaml" output="screen">
    <remap from="map" to="map_nav"/>
</node>

<node pkg="amcl" type="amcl" name="amcl" output="screen">
    <param name="odom_model_type" value="diff"/>
    <param name="odom_alpha5" value="0.1"/>
    <param name="transform_tolerance" value="0.2" />
    <param name="gui_publish_rate" value="10.0"/>
    <param name="laser_max_beams" value="30"/>
    <param name="min_particles" value="500"/>
    <param name="max_particles" value="15000"/>
    <param name="kld_err" value="0.05"/>
    <param name="kld_z" value="0.99"/>
    <param name="odom_alpha1" value="0.2"/>
    <param name="odom_alpha2" value="0.2"/>

    <param name="odom_alpha3" value="0.8"/>
    <param name="odom_alpha4" value="0.2"/>
    <param name="laser_z_hit" value="0.5"/>
    <param name="laser_z_short" value="0.05"/>
    <param name="laser_z_max" value="0.05"/>
    <param name="laser_z_rand" value="0.5"/>
    <param name="laser_sigma_hit" value="0.2"/>
    <param name="laser_lambda_short" value="0.1"/>
    <param name="laser_lambda_short" value="0.1"/>
    <param name="laser_model_type" value="likelihood_field"/>

    <param name="laser_likelihood_max_dist" value="2.0"/>
    <param name="update_min_d" value="0.2"/>
    <param name="update_min_a" value="0.5"/>
    <param name="odom_frame_id" value="odom"/>
    <param name="resample_interval" value="1"/>
    <param name="transform_tolerance" value="0.1"/>
    <param name="recovery_alpha_slow" value="0.0"/>
    <param name="recovery_alpha_fast" value="0.0"/>
        <remap from="scan" to="scan_filtered" />
</node>

<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
        <rosparam file="$(arg path)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(arg path)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(arg path)/config/local_costmap_params.yaml" command="load" />
        <rosparam file="$(arg path)/config/global_costmap_params.yaml" command="load" />
        <rosparam file="$(arg path)/config/base_local_planner_params.yaml" command="load" />
        <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
        <param name="controller_frequency" value="10.0" />
</node>

</launch>

  • base_local_planner_params.yaml

controller_frequency: 2.0 recovery_behavior_enabled: false clearing_rotation_allowed: false recovery_behaviors: [{name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery}, {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery}]

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: 2.0
 feasibility_check_no_poses: 4

 # Robot

 max_vel_x: 0.7
 min_vel_x: 0.3
 max_vel_x_backwards: 0.5
 max_vel_theta: 0.7
 acc_lim_x: 0.7
 acc_lim_theta: 0.7
 min_turning_radius: 0.4
 footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
   type: "line"
   line_start: [0.0, 0.0]
   line_end: [-0.6, 0.0]
   radius: 0.2

 # GoalTolerance

 xy_goal_tolerance: 0.2
 yaw_goal_tolerance: 0.1
 free_goal_vel: False

 # Obstacles

 min_obstacle_dist: 0.3
 include_costmap_obstacles: True
 costmap_obstacles_behind_robot_dist: 1.0
 obstacle_poses_affected: 30
 costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"
 costmap_converter_spin_thread: True
 costmap_converter_rate: 5

 # Optimization

 no_inner_iterations: 4
 no_outer_iterations: 3
 optimization_activate: True
 optimization_verbose: False
 penalty_epsilon: 0.1
 weight_max_vel_x: 2
 weight_max_vel_theta: 1
 weight_acc_lim_x: 0.5
 weight_acc_lim_theta: 6.0
 weight_kinematics_nh: 1000
 weight_kinematics_forward_drive: 1
 weight_kinematics_turning_radius: 1
 weight_optimaltime: 1
 weight_obstacle: 50

 # Homotopy Class Planner

 enable_homotopy_class_planning: True
 enable_multithreading: True
 simple_exploration: True
 max_number_classes: 2
 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
  • costmap_common_params.yaml

      plugins:
        - {name: static_layer, type: 'costmap_2d::StaticLayer'}
        - {name: obstacle_layer, type: 'costmap_2d::ObstacleLayer'}
        - {name: inflation_layer, type: 'costmap_2d::InflationLayer'}
        - {name: sonar,   type: "range_sensor_layer::RangeSensorLayer"}
    
      footprint: [[0.25,-0.31],[0.25,0.31],[-0.69,0.31],[-0.69,-0.31]]
      footprint_padding: 0.03
    
      inflation_layer:
          inflation_radius: 0.4
          cost_scaling_factor: 2.58
    
      sonar:
          topics: ['/sonar_data']
    
      obstacle_layer:
          combination_method: 1
          enabled: true
          footprint_clearing_enabled: true
          max_obstacle_height: 0.6
          observation_sources: scan
          obstacle_range: 2.0
          raytrace_range: 2.0
          scan: {clearing: true, data_type: LaserScan, expected_update_rate: 0, marking: true,
            topic: /scan_filtered}
    
  • global_costmap_params.yaml

    global_costmap:
      global_frame: map
      robot_base_frame: /base_link
      update_frequency: 5.0
      publish_frequency: 1.0
      static_map: true
      rolling_window: false
      resolution: 0.05
      transform_tolerance: 1.0
      map_type: costmap
    
  • local_costmap_params.yaml

    local_costmap:
     global_frame: odom
     robot_base_frame: base_link
     update_frequency: 5.0
     publish_frequency: 1.0
     static_map: true
     rolling_window: true
     width: 5.0
     height: 5.0
     resolution: 0.05
     transform_tolerance: 1.0
     map_type: costmap
    
     inflation_layer:
        inflation_radius: 0.2
        cost_scaling_factor: 10
    

Robot drifts away from goal and laserscan skews on random interval

I have to apologize for the vague and/or convoluted title. We don't know where the issue is ourselves.

The robot I'm working on makes use of ROS Kinetic, the standard navigation stack with amcl, teb_local_planner, laser_scan_matcher and move_base. It is a front-wheeled differential drive with two caster wheels on the back with a rectangular body/footprint, built on the chassis of an electric wheelchair.

The issue we're facing is happening on a random interval: Whenever we set a goal in rviz, the robot does move towards his goal. After a random amount of time or after a certain distance has been traveled, two errors could occur:

The robot loses track of his position by "teleporting" on the map within rviz, commonly setting himself approx. 1 meter to the right. The laser scan output could also as the local costmap would become skewed, throwing off its odometry. we see this by lines drawn from the LIDAR sensor, indicating cabinets and walls, becoming skewed and/or rotating from their location. This behaviour causes it to continue on "a path" until it does encounter an obstacle. Each of these erratic behaviours is visible within rviz and the console output can mention the errors Map Update/Control loop missed its desired rate <...>

As a result, which we think might not be related to CPU load, as it is averaging at about 50% including when generating new paths. never has it gone above 90%.

Additionally, the robot would keep on its last set velocity (cmd_vel output) and rviz would no longer be able to set and display a new goal, rendering the robot unresponsive. We're able to clear cmd_vel by using manual control (teleop_twist_joy) and stopping the robot that way. Upon hitting the reset button on the bottom-right in rviz, we progressively lose the local and global costmap per press, accompanied by the warning "No map provided". Rebooting the ROS environment will restore everything until the "drift" occurs again.

We can consistently recreate this behaviour by having the robot drive various distances, believing it appears more often when traversing small distances of approximately 2-3 meters, on longer distances i.e 5-20 meters the errors occure less often, but the laserscan becoming askew is still prevalent. We're hoping The LIDAR/local costmap becoming askew is easily reproducible by rotating the robot while stationary, when turning both left and right the laser scan and local costmap would rotate in short intervals and progressively move to get video footage of the erroneous behaviour in order to demonstrate what's happening both physically and in rviz.the right a certain distance within rviz until no longer, where it is still skewed massively.

Below are the common configuration files requested:

  • navigation.launch

<launch> <arg name="sim" default="false"/> <arg name="path"/>

<node if="$(eval sim == 'true')" pkg="rosbag" type="play" name="play" args="$(arg path)/bags/t5_lidar.bag --delay=5 --clock"/>

    <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /laser 40" />

    <node pkg="tf" type="static_transform_publisher" name="base_link_to_sonar_front" args="0.0 0 0 0.0 0.0 0.0 /base_link /front 40"/>
    <node pkg="tf" type="static_transform_publisher" name="base_link_to_sonar_right" args="0.0 0 0 -1.57 0.0 0.0 /base_link /backRight 40"/>
    <node pkg="tf" type="static_transform_publisher" name="base_link_to_sonar_left" args="0.0 0 0 1.57 0.0 0.0 /base_link /backLeft 40"/>

    <node pkg="tf" type="static_transform_publisher" name="world_to_map" args="0.0 0 0 0.0 0.0 0.0 /world /map 40"/>
    <node pkg="tf" type="static_transform_publisher" name="world_to_map_nav" args="0.0 0 0 0.0 0.0 0.0 /world /map_nav 40"/>

<!-- <node pkg="laser_filters" type="scan_to_scan_filter_chain" name="laser_filter">
        <rosparam command="load" file="$(arg path)/config/laser_filter_config.yaml" />
</node> -->


 <node pkg="rviz" type="rviz" name="rviz" args="-d $(arg path)/navigation.rviz"/>

 <node pkg="laser_scan_matcher" type="laser_scan_matcher_node" name="laser_scan_matcher_node" output="screen">
     <param name="base_frame" value = "/base_link"/>
     <param name="fixed_frame" value = "/odom"/>
     <param name="vel" value="/cmd_vel" />
     <param name="use_cloud_input" value="false"/>
     <param name="publish_tf" value="true"/>
     <param name="publish_odom" value="true"/>
     <param name="use_odom" value="false"/>
     <param name="use_imu" value="false"/>
     <param name="use_vel" value="true"/>
     <param name="use_alpha_beta" value="true"/>
     <param name="max_iterations" value="10"/>
     <remap from="scan" to="scan_filtered" />
 </node>

 <node pkg="map_server" type="map_server" name="map_server" args="$(arg path)/maps/T5_full_small.yaml" output="screen"/>
 <node pkg="map_server" type="map_server" name="map_server_nav" args="$(arg path)/maps/T5_full_small_move_base.yaml" output="screen">
 <remap from="map" to="map_nav"/>
</node>

</node>

<node pkg="amcl" type="amcl" name="amcl" output="screen"> <param name="odom_model_type" value="diff"/> <param name="odom_alpha5" value="0.1"/> <param name="transform_tolerance" value="0.2" /> <param name="gui_publish_rate" value="10.0"/> <param name="laser_max_beams" value="30"/> <param name="min_particles" value="500"/> <param name="max_particles" value="15000"/> <param name="kld_err" value="0.05"/> <param name="kld_z" value="0.99"/> <param name="odom_alpha1" value="0.2"/> <param name="odom_alpha2" value="0.2"/>

<param name="odom_alpha3" value="0.8"/>
 <param name="odom_alpha4" value="0.2"/>
 <param name="laser_z_hit" value="0.5"/>
 <param name="laser_z_short" value="0.05"/>
 <param name="laser_z_max" value="0.05"/>
 <param name="laser_z_rand" value="0.5"/>
 <param name="laser_sigma_hit" value="0.2"/>
 <param name="laser_lambda_short" value="0.1"/>
 <param name="laser_lambda_short" value="0.1"/>
 <param name="laser_model_type" value="likelihood_field"/>

 <param name="laser_likelihood_max_dist" value="2.0"/>
 <param name="update_min_d" value="0.2"/>
 <param name="update_min_a" value="0.5"/>
 <param name="odom_frame_id" value="odom"/>
 <param name="resample_interval" value="1"/>
 <param name="transform_tolerance" value="0.1"/>
 <param name="recovery_alpha_slow" value="0.0"/>
 <param name="recovery_alpha_fast" value="0.0"/>
     <remap from="scan" to="scan_filtered" />
</node>

</node>

<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen"> <rosparam file="$(arg path)/config/costmap_common_params.yaml" command="load" ns="global_costmap" /> ns="global_costmap"/> <rosparam file="$(arg path)/config/costmap_common_params.yaml" command="load" ns="local_costmap" /> ns="local_costmap"/> <rosparam file="$(arg path)/config/local_costmap_params.yaml" command="load" /> command="load"/> <rosparam file="$(arg path)/config/global_costmap_params.yaml" command="load" /> command="load"/> <rosparam file="$(arg path)/config/base_local_planner_params.yaml" command="load" /> <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" /> <param name="controller_frequency" value="10.0" /> </node> command="load"/> </node>

</launch>

  • base_local_planner_params.yaml

controller_frequency: 2.0 recovery_behavior_enabled: false clearing_rotation_allowed: false recovery_behaviors: [{name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery}, {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery}]

TebLocalPlannerROS:

 

TebLocalPlannerROS:

odom_topic: odom map_frame: /odom /odom

# Trajectory Trajectory

teb_autosize: True dt_ref: 0.3 dt_hysteresis: 0.1 global_plan_overwrite_orientation: True max_global_plan_lookahead_dist: 2.0 feasibility_check_no_poses: 4 4

# Robot Robot

max_vel_x: 0.7 min_vel_x: 0.3 max_vel_x_backwards: 0.5 max_vel_theta: 0.7 acc_lim_x: 0.7 acc_lim_theta: 0.7 min_turning_radius: 0.4 footprint_model: # types: "point", "circular", "two_circles", "line", "polygon" type: "line" line_start: [0.0, 0.0] line_end: [-0.6, 0.0] radius: 0.2 0.2

# GoalTolerance GoalTolerance

xy_goal_tolerance: 0.2 yaw_goal_tolerance: 0.1 free_goal_vel: False False

# Obstacles Obstacles

min_obstacle_dist: 0.3 include_costmap_obstacles: True costmap_obstacles_behind_robot_dist: 1.0 obstacle_poses_affected: 30 costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH" costmap_converter_spin_thread: True costmap_converter_rate: 5 5

# Optimization Optimization

no_inner_iterations: 4 no_outer_iterations: 3 optimization_activate: True optimization_verbose: False penalty_epsilon: 0.1 weight_max_vel_x: 2 weight_max_vel_theta: 1 weight_acc_lim_x: 0.5 weight_acc_lim_theta: 6.0 weight_kinematics_nh: 1000 weight_kinematics_forward_drive: 1 weight_kinematics_turning_radius: 1 weight_optimaltime: 1 weight_obstacle: 50 50

# Homotopy Class Planner Planner

enable_homotopy_class_planning: True enable_multithreading: True simple_exploration: True max_number_classes: 2 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

    False

  • costmap_common_params.yaml

      plugins:
        - {name: static_layer, type: 'costmap_2d::StaticLayer'}
        - {name: obstacle_layer, type: 'costmap_2d::ObstacleLayer'}
        - {name: inflation_layer, type: 'costmap_2d::InflationLayer'}
        - {name: sonar,   type: "range_sensor_layer::RangeSensorLayer"}
    
      footprint: [[0.25,-0.31],[0.25,0.31],[-0.69,0.31],[-0.69,-0.31]]
      footprint_padding: 0.03
    
      inflation_layer:
          inflation_radius: 0.4
          cost_scaling_factor: 2.58
    
      sonar:
          topics: ['/sonar_data']
    
      obstacle_layer:
          combination_method: 1
          enabled: true
          footprint_clearing_enabled: true
          max_obstacle_height: 0.6
          observation_sources: scan
          obstacle_range: 2.0
          raytrace_range: 2.0
          scan: {clearing: true, data_type: LaserScan, expected_update_rate: 0, marking: true,
            topic: /scan_filtered}
    
  • global_costmap_params.yaml

    global_costmap:
      global_frame: map
      robot_base_frame: /base_link
      update_frequency: 5.0
      publish_frequency: 1.0
      static_map: true
      rolling_window: false
      resolution: 0.05
      transform_tolerance: 1.0
      map_type: costmap
    
  • local_costmap_params.yaml

    local_costmap:
     global_frame: odom
     robot_base_frame: base_link
     update_frequency: 5.0
     publish_frequency: 1.0
     static_map: true
     rolling_window: true
     width: 5.0
     height: 5.0
     resolution: 0.05
     transform_tolerance: 1.0
     map_type: costmap
    
     inflation_layer:
        inflation_radius: 0.2
        cost_scaling_factor: 10
    

Robot drifts away from goal and laserscan skews on random interval

I have to apologize for the vague and/or convoluted title. We don't know where the issue is ourselves.

The robot I'm working on makes use of ROS Kinetic, the standard navigation stack with amcl, teb_local_planner, laser_scan_matcher and move_base. It is a front-wheeled differential drive with two caster wheels on the back with a rectangular body/footprint, built on the chassis of an electric wheelchair.

The issue we're facing is happening on a random interval: Whenever we set a goal in rviz, the robot does move towards his goal. After a random amount of time or after a certain distance has been traveled, two errors could occur:

The robot loses track of his position as the local costmap would become skewed, we see this by lines drawn from the LIDAR sensor, indicating cabinets and walls, becoming skewed and/or rotating from their location. This behaviour causes it to continue on "a path" until it does encounter an obstacle. Each of these erratic behaviours is visible within rviz and the console output can mention the errors Map Update/Control loop missed its desired rate <...> which we think might not be related to CPU load, as it is averaging at about 50% including when generating new paths. never has it gone above 90%.

Additionally, the robot would keep on its last set velocity (cmd_vel output) and rviz would no longer be able to set and display a new goal, rendering the robot unresponsive. We're able to clear cmd_vel by using manual control (teleop_twist_joy) and stopping the robot that way. Upon hitting the reset button on the bottom-right in rviz, we progressively lose the local and global costmap per press, accompanied by the warning "No map provided". Rebooting the ROS environment will restore everything until the "drift" occurs again.

We can consistently recreate this behaviour by having the robot drive various distances, believing it appears more often when traversing small distances of approximately 2-3 meters, on longer distances i.e 5-20 meters the errors occure less often, but the laserscan becoming askew is still prevalent. The LIDAR/local costmap becoming askew is easily reproducible by rotating the robot while stationary, when turning both left and right the laser scan and local costmap would rotate in short intervals and progressively move to the right a certain distance within rviz until no longer, where it is still skewed massively.

We've managed to make a quick recording of what happens when the robot is spinning around its axis once, alas, the video is rather shaky. https://www.dropbox.com/s/iywk2r9c6db0wda/VID_20190326_151328.mp4?dl=0 (The link will be removed if it's not allowed to post here.)

Below are the common configuration files requested:

  • navigation.launch

    <launch> <arg name="sim" default="false"/> <arg name="path"/>

    <node if="$(eval sim == 'true')" pkg="rosbag" type="play" name="play" args="$(arg path)/bags/t5_lidar.bag --delay=5 --clock"/>
    
        <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /laser 40" />
    
        <node pkg="tf" type="static_transform_publisher" name="base_link_to_sonar_front" args="0.0 0 0 0.0 0.0 0.0 /base_link /front 40"/>
        <node pkg="tf" type="static_transform_publisher" name="base_link_to_sonar_right" args="0.0 0 0 -1.57 0.0 0.0 /base_link /backRight 40"/>
        <node pkg="tf" type="static_transform_publisher" name="base_link_to_sonar_left" args="0.0 0 0 1.57 0.0 0.0 /base_link /backLeft 40"/>
    
        <node pkg="tf" type="static_transform_publisher" name="world_to_map" args="0.0 0 0 0.0 0.0 0.0 /world /map 40"/>
        <node pkg="tf" type="static_transform_publisher" name="world_to_map_nav" args="0.0 0 0 0.0 0.0 0.0 /world /map_nav 40"/>
    
    <!-- <node pkg="laser_filters" type="scan_to_scan_filter_chain" name="laser_filter">
            <rosparam command="load" file="$(arg path)/config/laser_filter_config.yaml" />
    </node> -->
    
    
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(arg path)/navigation.rviz"/>
    
    <node pkg="laser_scan_matcher" type="laser_scan_matcher_node" name="laser_scan_matcher_node" output="screen">
        <param name="base_frame" value = "/base_link"/>
        <param name="fixed_frame" value = "/odom"/>
        <param name="vel" value="/cmd_vel" />
        <param name="use_cloud_input" value="false"/>
        <param name="publish_tf" value="true"/>
        <param name="publish_odom" value="true"/>
        <param name="use_odom" value="false"/>
        <param name="use_imu" value="false"/>
        <param name="use_vel" value="true"/>
        <param name="use_alpha_beta" value="true"/>
        <param name="max_iterations" value="10"/>
        <remap from="scan" to="scan_filtered" />
    </node>
    
    <node pkg="map_server" type="map_server" name="map_server" args="$(arg path)/maps/T5_full_small.yaml" output="screen"/>
    <node pkg="map_server" type="map_server" name="map_server_nav" args="$(arg path)/maps/T5_full_small_move_base.yaml" output="screen">
    <remap from="map" to="map_nav"/>
    

    </node>

    <node pkg="amcl" type="amcl" name="amcl" output="screen">

    <param name="odom_alpha3" value="0.8"/>
    <param name="odom_alpha4" value="0.2"/>
    <param name="laser_z_hit" value="0.5"/>
    <param name="laser_z_short" value="0.05"/>
    <param name="laser_z_max" value="0.05"/>
    <param name="laser_z_rand" value="0.5"/>
    <param name="laser_sigma_hit" value="0.2"/>
    <param name="laser_lambda_short" value="0.1"/>
    <param name="laser_lambda_short" value="0.1"/>
    <param name="laser_model_type" value="likelihood_field"/>
    
    <param name="laser_likelihood_max_dist" value="2.0"/>
    <param name="update_min_d" value="0.2"/>
    <param name="update_min_a" value="0.5"/>
    <param name="odom_frame_id" value="odom"/>
    <param name="resample_interval" value="1"/>
    <param name="transform_tolerance" value="0.1"/>
    <param name="recovery_alpha_slow" value="0.0"/>
    <param name="recovery_alpha_fast" value="0.0"/>
        <remap from="scan" to="scan_filtered" />
    

    </node>

    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen"> <rosparam file="$(arg path)/config/costmap_common_params.yaml" command="load" ns="global_costmap"/> <rosparam file="$(arg path)/config/costmap_common_params.yaml" command="load" ns="local_costmap"/> <rosparam file="$(arg path)/config/local_costmap_params.yaml" command="load"/> <rosparam file="$(arg path)/config/global_costmap_params.yaml" command="load"/> <rosparam file="$(arg path)/config/base_local_planner_params.yaml" command="load"/> </node>

</launch>

  • base_local_planner_params.yaml

    controller_frequency: 2.0 recovery_behavior_enabled: false clearing_rotation_allowed: false recovery_behaviors: [{name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery}, {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery}]

    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: 2.0 feasibility_check_no_poses: 4

    # Robot

    max_vel_x: 0.7 min_vel_x: 0.3 max_vel_x_backwards: 0.5 max_vel_theta: 0.7 acc_lim_x: 0.7 acc_lim_theta: 0.7 min_turning_radius: 0.4 footprint_model: # types: "point", "circular", "two_circles", "line", "polygon" type: "line" line_start: [0.0, 0.0] line_end: [-0.6, 0.0] radius: 0.2

    # GoalTolerance

    xy_goal_tolerance: 0.2 yaw_goal_tolerance: 0.1 free_goal_vel: False

    # Obstacles

    min_obstacle_dist: 0.3 include_costmap_obstacles: True costmap_obstacles_behind_robot_dist: 1.0 obstacle_poses_affected: 30 costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH" costmap_converter_spin_thread: True costmap_converter_rate: 5

    # Optimization

    no_inner_iterations: 4 no_outer_iterations: 3 optimization_activate: True optimization_verbose: False penalty_epsilon: 0.1 weight_max_vel_x: 2 weight_max_vel_theta: 1 weight_acc_lim_x: 0.5 weight_acc_lim_theta: 6.0 weight_kinematics_nh: 1000 weight_kinematics_forward_drive: 1 weight_kinematics_turning_radius: 1 weight_optimaltime: 1 weight_obstacle: 50

    # Homotopy Class Planner

    enable_homotopy_class_planning: True enable_multithreading: True simple_exploration: True max_number_classes: 2 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

  • costmap_common_params.yaml

      plugins:
        - {name: static_layer, type: 'costmap_2d::StaticLayer'}
        - {name: obstacle_layer, type: 'costmap_2d::ObstacleLayer'}
        - {name: inflation_layer, type: 'costmap_2d::InflationLayer'}
        - {name: sonar,   type: "range_sensor_layer::RangeSensorLayer"}
    
      footprint: [[0.25,-0.31],[0.25,0.31],[-0.69,0.31],[-0.69,-0.31]]
      footprint_padding: 0.03
    
      inflation_layer:
          inflation_radius: 0.4
          cost_scaling_factor: 2.58
    
      sonar:
          topics: ['/sonar_data']
    
      obstacle_layer:
          combination_method: 1
          enabled: true
          footprint_clearing_enabled: true
          max_obstacle_height: 0.6
          observation_sources: scan
          obstacle_range: 2.0
          raytrace_range: 2.0
          scan: {clearing: true, data_type: LaserScan, expected_update_rate: 0, marking: true,
            topic: /scan_filtered}
    
  • global_costmap_params.yaml

    global_costmap:
      global_frame: map
      robot_base_frame: /base_link
      update_frequency: 5.0
      publish_frequency: 1.0
      static_map: true
      rolling_window: false
      resolution: 0.05
      transform_tolerance: 1.0
      map_type: costmap
    
  • local_costmap_params.yaml

    local_costmap:
     global_frame: odom
     robot_base_frame: base_link
     update_frequency: 5.0
     publish_frequency: 1.0
     static_map: true
     rolling_window: true
     width: 5.0
     height: 5.0
     resolution: 0.05
     transform_tolerance: 1.0
     map_type: costmap
    
     inflation_layer:
        inflation_radius: 0.2
        cost_scaling_factor: 10