Fixes local_costmap and global_costmap scan topics. (#79)

Signed-off-by: Franco Cipollone <[email protected]>
Ekumen-OS · Aug 23, 2024 · c7e1a88 · c7e1a88
1 parent 8b2143a
commit c7e1a88
Show file tree

Hide file tree

Showing 2 changed files with 40 additions and 21 deletions.
diff --git a/andino_gz/config/nav2_params.yaml b/andino_gz/config/nav2_params.yaml
@@ -1,7 +1,22 @@
-# Nav2 parameters inspired in nav2_bringup/params/nav2_params.yaml
-# Important changes:
-#  - Topics here are set relative. So when multiple robots are used, the topic will go under the robot namespace.
-
+# This file has been copied and adapted from: https://github.com/ros-planning/navigation2/blob/3bb9a95719539fcdfedf520c63339bc3157f80b4/nav2_bringup/params/nav2_params.yaml
+#
+# Copyright could not be identified. Copyright holders identification should refer to the aforementioned code source.
+#
+# License note follows:
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#    http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+# Changes in some parameters has been introduced to adapt to the andino robot
 amcl:
   ros__parameters:
     use_sim_time: True
@@ -22,7 +37,7 @@ amcl:
     laser_min_range: -1.0
     laser_model_type: "likelihood_field"
     max_beams: 60
-    max_particles: 2000
+    max_particles: 3000
     min_particles: 500
     odom_frame_id: "odom"
     pf_err: 0.05
@@ -51,7 +66,6 @@ bt_navigator:
     odom_topic: odom
     bt_loop_duration: 10
     default_server_timeout: 20
-    wait_for_service_timeout: 1000
     # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
     # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
     # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
@@ -116,7 +130,7 @@ bt_navigator_navigate_to_pose_rclcpp_node:
 controller_server:
   ros__parameters:
     use_sim_time: True
-    controller_frequency: 20.0
+    controller_frequency: 10.0
     min_x_velocity_threshold: 0.001
     min_y_velocity_threshold: 0.5
     min_theta_velocity_threshold: 0.001
@@ -153,8 +167,6 @@ controller_server:
       min_speed_xy: 0.0
       max_speed_xy: 0.26
       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
@@ -196,12 +208,12 @@ local_costmap:
       width: 3
       height: 3
       resolution: 0.05
-      robot_radius: 0.22
+      robot_radius: 0.12
       plugins: ["voxel_layer", "inflation_layer"]
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
-        cost_scaling_factor: 3.0
-        inflation_radius: 0.3
+        cost_scaling_factor: 2.0
+        inflation_radius: 0.2
       voxel_layer:
         plugin: "nav2_costmap_2d::VoxelLayer"
         enabled: True
@@ -258,8 +270,8 @@ global_costmap:
         map_subscribe_transient_local: True
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
-        cost_scaling_factor: 3.0
-        inflation_radius: 0.30
+        cost_scaling_factor: 2.0
+        inflation_radius: 0.2
       always_send_full_costmap: True
 
 map_server:
@@ -279,13 +291,13 @@ map_saver:
 
 planner_server:
   ros__parameters:
-    expected_planner_frequency: 20.0
+    expected_planner_frequency: 10.0
     use_sim_time: True
     planner_plugins: ["GridBased"]
     GridBased:
       plugin: "nav2_navfn_planner/NavfnPlanner"
       tolerance: 0.5
-      use_astar: false
+      use_astar: true
       allow_unknown: true
 
 smoother_server:
@@ -330,7 +342,7 @@ robot_state_publisher:
 waypoint_follower:
   ros__parameters:
     use_sim_time: True
-    loop_rate: 20
+    loop_rate: 10
     stop_on_failure: false
     waypoint_task_executor_plugin: "wait_at_waypoint"
     wait_at_waypoint:
@@ -341,11 +353,11 @@ waypoint_follower:
 velocity_smoother:
   ros__parameters:
     use_sim_time: True
-    smoothing_frequency: 20.0
+    smoothing_frequency: 10.0
     scale_velocities: False
     feedback: "OPEN_LOOP"
-    max_velocity: [0.26, 0.0, 1.0]
-    min_velocity: [-0.26, 0.0, -1.0]
+    max_velocity: [0.25, 0.0, 1.0]
+    min_velocity: [-0.25, 0.0, -1.0]
     max_accel: [2.5, 0.0, 3.2]
     max_decel: [-2.5, 0.0, -3.2]
     odom_topic: "odom"

diff --git a/andino_gz/launch/andino_gz.launch.py b/andino_gz/launch/andino_gz.launch.py
@@ -7,7 +7,7 @@
 from launch.conditions import IfCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression, TextSubstitution
-from launch_ros.actions import Node, PushRosNamespace
+from launch_ros.actions import Node, PushRosNamespace, SetRemap
 
 from nav2_common.launch import ParseMultiRobotPose
 
@@ -182,6 +182,11 @@ def generate_launch_description():
               'nav2': nav2_flag,
           },
           actions=[
+              # Remapping scan topics for Nav2 local and global costmap.
+              # As we use relative values in the param file for supporting multiple robots,
+              # the scan topic needs to be remapped otherwise goes under global-costmap/scan topic.
+              SetRemap(src='/' + robot_name + '/global_costmap/scan', dst='/' + robot_name + '/scan', condition=IfCondition(PythonExpression([more_than_one_robot, ' and ', LaunchConfiguration('nav2')]))),
+              SetRemap(src='/' + robot_name + '/local_costmap/scan', dst='/' + robot_name + '/scan', condition=IfCondition(PythonExpression([more_than_one_robot, ' and ', LaunchConfiguration('nav2')]))),
               # Nav2 Bringup for multiple robots
               IncludeLaunchDescription(
                   PythonLaunchDescriptionSource(
@@ -197,6 +202,8 @@ def generate_launch_description():
                   }.items(),
                   condition=IfCondition(PythonExpression([more_than_one_robot, ' and ', LaunchConfiguration('nav2')])),
               ),
+              SetRemap(src='/global_costmap/scan', dst='/scan', condition=IfCondition(PythonExpression([one_robot, ' and ', LaunchConfiguration('nav2')]))),
+              SetRemap(src='/local_costmap/scan', dst='/scan', condition=IfCondition(PythonExpression([one_robot, ' and ', LaunchConfiguration('nav2')]))),
               # Nav2 Bringup for single robot
               IncludeLaunchDescription(
                   PythonLaunchDescriptionSource(