Foxy devel cloi

nav2_bringup/bringup/launch/localization_launch.py

@@ -23,6 +23,24 @@
 from nav2_common.launch import RewrittenYaml
 
 
+def get_initial_pose_from_env():
+    set_initial_pose = False
+    initial_pose_x = '0.0'
+    initial_pose_y = '0.0'
+    initial_pose_yaw = '0.0'
+    if 'INITIAL_POSE_X' in os.environ.keys():
+        set_initial_pose = True
+        initial_pose_x = os.environ['INITIAL_POSE_X']
+    if 'INITIAL_POSE_Y' in os.environ.keys():
+        set_initial_pose = True
+        initial_pose_y = os.environ['INITIAL_POSE_Y']
+    if 'INITIAL_POSE_YAW' in os.environ.keys():
+        set_initial_pose = True
+        initial_pose_yaw = os.environ['INITIAL_POSE_YAW']
+    result = [set_initial_pose, initial_pose_x, initial_pose_y, initial_pose_yaw]
+    return result
+
+
 def generate_launch_description():
     # Get the launch directory
     bringup_dir = get_package_share_directory('nav2_bringup')
@@ -48,6 +66,18 @@ def generate_launch_description():
         'use_sim_time': use_sim_time,
         'yaml_filename': map_yaml_file}
 
+    initial_pose_env = get_initial_pose_from_env()
+    if initial_pose_env[0]:
+        print('get_initial_pose_from_env initial_pose_env True')
+        set_initial_pose = LaunchConfiguration('set_initial_pose', default='true')
+        initial_pose_x = LaunchConfiguration('x', default=initial_pose_env[1])
+        initial_pose_y = LaunchConfiguration('y', default=initial_pose_env[2])
+        initial_pose_yaw = LaunchConfiguration('yaw', default=initial_pose_env[3])
+        param_substitutions['set_initial_pose'] = set_initial_pose
+        param_substitutions['x'] = initial_pose_x
+        param_substitutions['y'] = initial_pose_y
+        param_substitutions['yaw'] = initial_pose_yaw
+
     configured_params = RewrittenYaml(
         source_file=params_file,
         root_key=namespace,

nav2_bringup/bringup/launch/navigation_launch.py

@@ -13,6 +13,8 @@
 # limitations under the License.
 
 import os
+import sys
+from collections import OrderedDict
 
 from ament_index_python.packages import get_package_share_directory
 
@@ -23,7 +25,23 @@
 from nav2_common.launch import RewrittenYaml
 
 
+def parse_launch_arguments(launch_arguments):
+    """Parse the given launch arguments from the command line, into list of tuples for launch."""
+    parsed_launch_arguments = OrderedDict()  # type: ignore
+    for argument in launch_arguments:
+        count = argument.count(':=')
+        if count == 0 or argument.startswith(':=') or (count == 1 and argument.endswith(':=')):
+            pass
+        else:
+            name, value = argument.split(':=', maxsplit=1)
+            parsed_launch_arguments[name] = value  # last one wins is intentional
+    return parsed_launch_arguments
+
 def generate_launch_description():
+    parsed_args_dict = parse_launch_arguments(sys.argv)
+    remap_ns = parsed_args_dict['namespace']
+    print('remap_ns : '+remap_ns)
+
     # Get the launch directory
     bringup_dir = get_package_share_directory('nav2_bringup')
 
@@ -47,7 +65,8 @@ def generate_launch_description():
     # TODO(orduno) Substitute with `PushNodeRemapping`
     #              https://github.com/ros2/launch_ros/issues/56
     remappings = [('/tf', 'tf'),
-                  ('/tf_static', 'tf_static')]
+                  ('/tf_static', 'tf_static'),
+                  ('/scan', '/'+remap_ns+'/scan')]
 
     # Create our own temporary YAML files that include substitutions
     param_substitutions = {

nav2_bringup/bringup/launch/rviz_launch.py

@@ -61,17 +61,13 @@ def generate_launch_description():
         arguments=['-d', rviz_config_file],
         output='screen')
 
-    namespaced_rviz_config_file = ReplaceString(
-            source_file=rviz_config_file,
-            replacements={'<robot_namespace>': ('/', namespace)})
-
     start_namespaced_rviz_cmd = Node(
         condition=IfCondition(use_namespace),
         package='rviz2',
         executable='rviz2',
         name='rviz2',
         namespace=namespace,
-        arguments=['-d', namespaced_rviz_config_file],
+        arguments=['-d', rviz_config_file],
         output='screen',
         remappings=[('/tf', 'tf'),
                     ('/tf_static', 'tf_static'),

nav2_bringup/bringup/params/nav2_params.yaml

@@ -1,6 +1,12 @@
 amcl:
   ros__parameters:
     use_sim_time: True
+    set_initial_pose: True
+    initial_pose:
+      x: 62.0
+      y: 28.0
+      z: 0.0
+      yaw: 0.0
     alpha1: 0.2
     alpha2: 0.2
     alpha3: 0.2
@@ -51,8 +57,8 @@ bt_navigator:
   ros__parameters:
     use_sim_time: True
     global_frame: map
-    robot_base_frame: base_link
-    odom_topic: /odom
+    robot_base_frame: base_footprint
+    odom_topic: odom
     enable_groot_monitoring: True
     groot_zmq_publisher_port: 1666
     groot_zmq_server_port: 1667
@@ -113,20 +119,20 @@ controller_server:
       debug_trajectory_details: True
       min_vel_x: 0.0
       min_vel_y: 0.0
-      max_vel_x: 0.26
+      max_vel_x: 1.5
       max_vel_y: 0.0
       max_vel_theta: 1.0
-      min_speed_xy: 0.0
-      max_speed_xy: 0.26
-      min_speed_theta: 0.0
+      min_speed_xy: 0.01
+      max_speed_xy: 1.5
+      min_speed_theta: 0.1
       # 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_x: -5.0
       decel_lim_y: 0.0
-      decel_lim_theta: -3.2
+      decel_lim_theta: -5.0
       vx_samples: 20
       vy_samples: 5
       vtheta_samples: 20
@@ -144,7 +150,7 @@ controller_server:
       PathAlign.forward_point_distance: 0.1
       GoalAlign.scale: 24.0
       GoalAlign.forward_point_distance: 0.1
-      PathDist.scale: 32.0
+      PathDist.scale: 96.0
       GoalDist.scale: 24.0
       RotateToGoal.scale: 32.0
       RotateToGoal.slowing_factor: 5.0
@@ -160,7 +166,7 @@ local_costmap:
       update_frequency: 5.0
       publish_frequency: 2.0
       global_frame: odom
-      robot_base_frame: base_link
+      robot_base_frame: base_footprint
       use_sim_time: True
       rolling_window: true
       width: 3
@@ -201,21 +207,21 @@ local_costmap:
 global_costmap:
   global_costmap:
     ros__parameters:
-      update_frequency: 1.0
+      update_frequency: 10.0
       publish_frequency: 1.0
       global_frame: map
-      robot_base_frame: base_link
+      robot_base_frame: base_footprint
       use_sim_time: True
       robot_radius: 0.22
       resolution: 0.05
       track_unknown_space: true
-      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+      plugins: ["static_layer", "obstacle_layer", "inflation_layer", "others_footprint_layer"]
       obstacle_layer:
         plugin: "nav2_costmap_2d::ObstacleLayer"
         enabled: True
         observation_sources: scan
         scan:
-          topic: /scan
+          topic: "/scan"
           max_obstacle_height: 2.0
           clearing: True
           marking: True
@@ -227,6 +233,9 @@ global_costmap:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 3.0
         inflation_radius: 0.55
+      others_footprint_layer:
+        plugin: "nav2_others_footprint_costmap_plugin::OthersFootprintLayer"
+        topic: "/cloi2/global_costmap/published_footprint"
       always_send_full_costmap: True
   global_costmap_client:
     ros__parameters:
@@ -276,7 +285,7 @@ recoveries_server:
     wait:
       plugin: "nav2_recoveries/Wait"
     global_frame: odom
-    robot_base_frame: base_link
+    robot_base_frame: base_footprint
     transform_timeout: 0.1
     use_sim_time: true
     simulate_ahead_time: 2.0