Omnidirectional square robot in Nav2
Hi!
I am trying to navigate an omnidirectional square robot using Nav2 and RTABMap. RTABMap is used for creating the map and afterwards localize the robot in it, therefore I don't need to use the AMCL nor the Map Server. Moreover I am using Ros2 Foxy.
I am pretty new with Nav2 and I am not able to tune the parameters correctly as there are some of them that I don't really understand how they work, although I have read the Nav2 documentation. I need to get the robot pass along a very narrow cave through where the robot enters roughly-
First of all, I tried to define my robot's footprint in the local and global costmap, however it seems like the footprint doesn't change, that is why I used the robot_radius although my robot has a square footprint... (I don't know if I have set something wrong)
Secondly, my robot gets stuck with the cave walls as the route created by the planner doesn't avoid part of them (when it seems it is possible to go through other path. As there are not many Planner parameters I don't know if maybe it has to something with the critics of the controller, but I don't understand them properly. It is also worth to mention that I am using the NavFn Planner although in the Nav2 documentation (https://navigation.ros.org/setup_guides/algorithm/select_algorithm.html) they recommend the Smac Lattice Planner for my type of robot, however, this planner is not available for Ros2 Foxy (distribution I have to work with).
Finally, when the robot gets stuck, it doesn't enter in the recovery mode (I am using the predefined behavior tree where, after the robot isn't able to compute or follow the path, it spins 90º and waits), although the controller_server shows: Failed to make progess, Aborting handle.
Here it is my configuration file, any idea or clue will be very well appreciated:
bt_navigator:
ros__parameters:
use_sim_time: True
global_frame: map
robot_base_frame: base_link
odom_topic: world
enable_groot_monitoring: True
groot_zmq_publisher_port: 1666
groot_zmq_server_port: 1667
default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
plugin_lib_names:
- nav2_compute_path_to_pose_action_bt_node
- nav2_follow_path_action_bt_node
- nav2_back_up_action_bt_node
- nav2_spin_action_bt_node
- nav2_wait_action_bt_node
- nav2_clear_costmap_service_bt_node
- nav2_is_stuck_condition_bt_node
- nav2_goal_reached_condition_bt_node
- nav2_goal_updated_condition_bt_node
- nav2_initial_pose_received_condition_bt_node
- nav2_reinitialize_global_localization_service_bt_node
- nav2_rate_controller_bt_node
- nav2_distance_controller_bt_node
- nav2_speed_controller_bt_node
- nav2_truncate_path_action_bt_node
- nav2_goal_updater_node_bt_node
- nav2_recovery_node_bt_node
- nav2_pipeline_sequence_bt_node
- nav2_round_robin_node_bt_node
- nav2_transform_available_condition_bt_node
- nav2_time_expired_condition_bt_node
- nav2_distance_traveled_condition_bt_node
bt_navigator_rclcpp_node:
ros__parameters:
use_sim_time: True
controller_server:
ros__parameters:
use_sim_time: True
odom_topic: "odom/unfiltered"
controller_frequency: 5.0
min_x_velocity_threshold: 0.001
min_y_velocity_threshold: 0.001
min_theta_velocity_threshold: 0.001
progress_checker_plugin: "progress_checker"
goal_checker_plugin: "goal_checker"
controller_plugins: ["FollowPath"]
# Progress checker parameters
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.1
movement_time_allowance: 20.0
# Goal checker parameters
goal_checker:
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.25
yaw_goal_tolerance: 0.25
stateful: True
# DWB parameters
FollowPath:
plugin: "dwb_core::DWBLocalPlanner"
debug_trajectory_details: True
min_vel_x: 0.0
min_vel_y: 0.0
max_vel_x: 2.0
max_vel_y: 1.0
max_vel_theta: 4.5
min_speed_xy: 0.2
max_speed_xy: 1.0
min_speed_theta: 0.0
# Add high threshold velocity for turtlebot 3 issue.
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
acc_lim_x: 2.5
acc_lim_y: 0.0
acc_lim_theta: 3.2
decel_lim_x: -2.5
decel_lim_y: 0.0
decel_lim_theta: -3.2
vx_samples: 20
vy_samples: 10
vtheta_samples: 20
sim_time: 1.7
linear_granularity: 0.5
angular_granularity: 0.025
transform_tolerance: 0.2
xy_goal_tolerance: 0.25
trans_stopped_velocity: 0.25
short_circuit_trajectory_evaluation: True
stateful: True
critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
BaseObstacle.scale: 0.02
PathAlign.scale: 32.0
PathAlign.forward_point_distance: 0.1
GoalAlign.scale: 24.0
GoalAlign.forward_point_distance: 0.1
PathDist.scale: 32.0
GoalDist.scale: 24.0
RotateToGoal.scale: 32.0
RotateToGoal.slowing_factor: 5.0
RotateToGoal.lookahead_time: -1.0
controller_server_rclcpp_node:
ros__parameters:
use_sim_time: True
local_costmap:
local_costmap:
ros__parameters:
update_frequency: 5.0
publish_frequency: 2.0
global_frame: world
robot_base_frame: base_link
use_sim_time: True
rolling_window: true
width: 3
height: 3
resolution: 0.05
always_send_full_costmap: True
footprint: “[[0.4, 0.4], [0.4, -0.4], [-0.4, -0.4], [-0.4, 0.4]]”
plugins: ["voxel_layer", "inflation_layer"]
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 10.0
inflation_radius: 0.1
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: True
observation_sources: pointcloud
pointcloud:
topic: /camera/points
max_obstacle_height: 0.5
min_obstacle_height: 0.0
obstacle_max_range: 2.5
obstacle_min_range: 0.0
raytrace_max_range: 3.0
raytrace_min_range: 0.0
clearing: True
marking: True
data_type: "PointCloud2"
voxel_layer:
plugin: "nav2_costmap_2d::VoxelLayer"
enabled: True
publish_voxel_map: True
origin_z: 0.0
z_resolution: 0.05
z_voxels: 16
max_obstacle_height: 2.0
mark_threshold: 0
observation_sources: pointcloud
pointcloud:
topic: /camera/points
max_obstacle_height: 0.5
min_obstacle_height: 0.0
obstacle_max_range: 2.5
obstacle_min_range: 0.0
raytrace_max_range: 3.0
raytrace_min_range: 0.0
clearing: True
marking: True
data_type: "PointCloud2"
local_costmap_client:
ros__parameters:
use_sim_time: True
local_costmap_rclcpp_node:
ros__parameters:
use_sim_time: True
global_costmap:
global_costmap:
ros__parameters:
update_frequency: 1.0
publish_frequency: 1.0
global_frame: map
robot_base_frame: base_link
use_sim_time: True
footprint: “[[0.4, 0.4], [0.4, -0.4], [-0.4, -0.4], [-0.4, 0.4]]”
resolution: 0.05
width: 3
height: 3
track_unknown_space: true
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: True
observation_sources: pointcloud
pointcloud:
topic: /camera/points
max_obstacle_height: 0.5
min_obstacle_height: 0.0
obstacle_max_range: 2.5
obstacle_min_range: 0.0
raytrace_max_range: 3.0
raytrace_min_range: 0.0
clearing: True
marking: True
data_type: "PointCloud2"
static_layer:
plugin: "nav2_costmap_2d::StaticLayer"
map_subscribe_transient_local: True
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 10.0
inflation_radius: 0.1
always_send_full_costmap: True
global_costmap_client:
ros__parameters:
use_sim_time: True
global_costmap_rclcpp_node:
ros__parameters:
use_sim_time: True
planner_server:
ros__parameters:
expected_planner_frequency: 1.0
use_sim_time: True
planner_plugins: ["GridBased"]
GridBased:
plugin: "nav2_navfn_planner/NavfnPlanner"
tolerance: 0.5
use_astar: True
allow_unknown: True
planner_server_rclcpp_node:
ros__parameters:
use_sim_time: True
recoveries_server:
ros__parameters:
costmap_topic: local_costmap/costmap_raw
footprint_topic: local_costmap/published_footprint
cycle_frequency: 10.0
recovery_plugins: ["spin", "back_up", "wait"]
spin:
plugin: "nav2_recoveries/Spin"
back_up:
plugin: "nav2_recoveries/BackUp"
wait:
plugin: "nav2_recoveries/Wait"
global_frame: world
robot_base_frame: base_link
transform_timeout: 0.1
use_sim_time: true
simulate_ahead_time: 2.0
max_rotational_vel: 1.0
min_rotational_vel: 0.4
rotational_acc_lim: 3.2
robot_state_publisher:
ros__parameters:
use_sim_time: True
And here are some images of the behavior:
Asked by marpeja on 2022-07-14 14:02:52 UTC
Answers
If you specify both a robot radius and a footprint, it will use the radius I believe, so you should make sure to specify your footprint and not a radius to model a square robot. For the planner this is important so that you have properly modeled your footprint to plan through a space and make sure it doesn't try to enter something too small. But with NavFn or the holonomic planners its going to take the largest cross section of the robot and essentially treat the robot as a circle with that maximum radius, as it assumes it can pivot in place as a holonomic planner. So if you used a radius for the global planner and had that set to the diagonal of your square robot, it would be virtually the same.
You should use something like Hybrid- A* or state lattice if you want feasible behavior with footprint collision checking, but you're right, its not available in your distribution. You'd need to upgrade.
I can't speak to the tuning of DWB or other local trajectory planners, that specific to each person's platform and I'm not going to wade into that. Though, you should make sure to model your footprint correctly for the local costmap since that is what is going to forward project trajectories to score them based on their feasibility if you use the footprint scoring critic.
Asked by stevemacenski on 2022-07-14 14:27:41 UTC
Comments
Hi Steve!
It was my error, I copied a wrong version of my configuration file (already fixed it in the question). The problem is that having defined the footprint, it appears me a small circle as the robot footprint in Rviz which is much more smaller than it should. In fact it doesn't increase when I increase the footprint dimensions... Therefore I think I would go the other way defining a robot_radius that covers my whole robot (both in the global and local costmap)
I also added the ObstacleFootprintCritic to my controller and gave it the highest scale factor, but it still doesn't avoid the walls correctly.
Moreover, the robot doesn't enter into the recovery mode when it gets stuck... Do I have something bad configured?
Thank you in advance for all your help!!
Asked by marpeja on 2022-07-15 04:25:30 UTC
Here are my up to date critics:
critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist", "ObstacleFootprint"]
BaseObstacle.scale: 38.0
ObstacleFootprint.scale: 80.0
PathAlign.scale: 32.0
PathAlign.forward_point_distance: 0.4
GoalAlign.scale: 10.0
GoalAlign.forward_point_distance: 0.1
PathDist.scale: 32.0
GoalDist.scale: 10.0
RotateToGoal.scale: 32.0
RotateToGoal.slowing_factor: 5.0
RotateToGoal.lookahead_time: -1.0
Asked by marpeja on 2022-07-15 04:25:59 UTC
I'm unfortunately not going to be able to help you tune your controllers.
Asked by stevemacenski on 2022-07-15 15:07:59 UTC
Comments