Feature/support multiple mini pupper model (#93)

* tidy up bring up and gazebo launch files

* futher clean up code

* minor syntax tidy up

* tidy up for review

README.md

@@ -146,9 +146,13 @@ Note: This step is only for PC
 ```sh
 # Terminal 1
 . ~/ros2_ws/install/setup.bash # setup.zsh if you use zsh instead of bash
-ros2 launch mini_pupper_bringup bringup.launch.py joint_hardware_connected:=false rviz:=true robot_name:=mini_pupper_2
+ros2 launch mini_pupper_bringup bringup.launch.py hardware_connected:=False
 
 # Terminal 2
+. ~/ros2_ws/install/setup.bash
+ros2 launch mini_pupper_bringup rviz.launch.py
+
+# Terminal 3
 ros2 run teleop_twist_keyboard teleop_twist_keyboard
 # Then control robot dog with the keyboard
 ```
@@ -160,7 +164,7 @@ Note: This step is only for PC
 ```sh
 # Terminal 1
 . ~/ros2_ws/install/setup.bash # setup.zsh if you use zsh instead of bash
-ros2 launch mini_pupper_gazebo gazebo.launch.py rviz:=true robot_name:=mini_pupper_2
+ros2 launch mini_pupper_gazebo gazebo.launch.py
 
 # Terminal 2
 ros2 run teleop_twist_keyboard teleop_twist_keyboard
@@ -175,7 +179,7 @@ Note: This step is only for PC
 ```sh
 # Terminal 1
 . ~/ros2_ws/install/setup.bash
-ros2 launch mini_pupper_gazebo gazebo.launch.py robot_name:=mini_pupper_2
+ros2 launch mini_pupper_gazebo gazebo.launch.py
 ```
 
 - Mapping on PC
@@ -208,7 +212,7 @@ The map will be saved under home directory. Two files will be generated, namely
 ```sh
 # Terminal 1
 . ~/ros2_ws/install/setup.bash
-ros2 launch mini_pupper_gazebo gazebo.launch.py robot_name:=mini_pupper_2
+ros2 launch mini_pupper_gazebo gazebo.launch.py
 ```
 
 - Navigation   

mini_pupper_bringup/CMakeLists.txt

@@ -4,7 +4,7 @@ project(mini_pupper_bringup)
 find_package(ament_cmake REQUIRED)
 
 install(
-  DIRECTORY launch
+  DIRECTORY launch config
   DESTINATION share/${PROJECT_NAME}
 )
 

mini_pupper_bringup/config/mini_pupper.yaml

@@ -0,0 +1,3 @@
+sensors:
+  lidar: true
+  imu: false

mini_pupper_bringup/config/mini_pupper_2.yaml

@@ -0,0 +1,3 @@
+sensors:
+  lidar: true
+  imu: true

mini_pupper_bringup/launch/bringup.launch.py

@@ -16,31 +16,34 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-# This program is based on https://github.com/champ/champ.
-# which is released under the Apache-2.0 License.
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Copyright (c) 2021 Juan Miguel Jimeno
-#
-# https://github.com/chvmp/champ/blob/f76d066d8964c8286afbcd9d5d2c08d781e85f54/champ_config/launch/bringup.launch.py
-
+import os
+import yaml
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.substitutions import FindPackageShare
-from launch_ros.actions import Node
 from launch.conditions import IfCondition
+from ament_index_python.packages import get_package_share_directory
+
+ROBOT_MODEL = os.getenv('ROBOT_MODEL', default="mini_pupper_2")
 
 
-def launch_bring_up(context, *args, **kwargs):
-    robot_name = LaunchConfiguration("robot_name")
-    sim = LaunchConfiguration("sim")
-    rviz = LaunchConfiguration("rviz")
-    joint_hardware_connected = LaunchConfiguration("joint_hardware_connected")
+def get_sensors_config():
+    bringup_package = get_package_share_directory('mini_pupper_bringup')
+    config_file_name = ROBOT_MODEL + '.yaml'
+    config_file_path = os.path.join(bringup_package, 'config', config_file_name)
 
-    robot_name_str = context.perform_substitution(robot_name)
-    description_package = FindPackageShare(f'{robot_name_str}_description')
+    with open(config_file_path, 'r') as f:
+        configuration = yaml.safe_load(f)
+        return configuration['sensors']
+
+
+def generate_launch_description():
+    if ROBOT_MODEL == "mini_pupper_2":
+        description_package = FindPackageShare('mini_pupper_2_description')
+    else:
+        description_package = FindPackageShare('mini_pupper_description')
 
     description_path = PathJoinSubstitution(
         [description_package, 'urdf', 'mini_pupper_description.urdf.xacro']
@@ -56,106 +59,64 @@ def launch_bring_up(context, *args, **kwargs):
         [description_package, 'config', 'champ', 'gait.yaml']
     )
 
-    bringup_launch_path = PathJoinSubstitution(
+    champ_bringup_launch_path = PathJoinSubstitution(
         [FindPackageShare('champ_bringup'), 'launch', 'bringup.launch.py']
     )
+    hardware_interface_launch_path = PathJoinSubstitution(
+        [FindPackageShare('mini_pupper_bringup'), 'launch', 'hardware_interface.launch.py']
+    )
+
+    sensors_config = get_sensors_config()
+    # This is the confusing part to wrap so much around a bool value.
+    # In ROS2 launch file, we cannot pass bool value directly to launch_arguments.
+    has_lidar = PythonExpression([str(sensors_config['lidar'])])
+    has_imu = PythonExpression([str(sensors_config['imu'])])
+
+    use_sim_time = LaunchConfiguration('use_sim_time')
+    use_sim_time_launch_arg = DeclareLaunchArgument(
+        name='use_sim_time',
+        default_value='False',
+        description='Use simulation (Gazebo) clock if true'
+    )
 
-    rviz_config_path = PathJoinSubstitution(
-        [description_package, 'rviz', 'urdf_viewer.rviz']
+    hardware_connected = LaunchConfiguration("hardware_connected")
+    hardware_connected_launch_arg = DeclareLaunchArgument(
+        name='hardware_connected',
+        default_value='True',
+        description='Set to true if connected to a physical robot'
     )
 
-    bringup_launch = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(bringup_launch_path),
+    champ_bringup_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(champ_bringup_launch_path),
         launch_arguments={
-            "use_sim_time": sim,
-            "robot_name": robot_name,
-            "gazebo": sim,
-            "rviz": "false",  # set always false to launch RViz2 with costom .rviz file
-            "joint_hardware_connected": joint_hardware_connected,
-            "orientation_from_imu": "true",
-            "publish_foot_contacts": "true",
-            "close_loop_odom": "true",
+            "use_sim_time": use_sim_time,
+            "robot_name": ROBOT_MODEL,
+            "gazebo": use_sim_time,
+            "rviz": "False",  # set always false to launch RViz2 with costom .rviz file
+            "joint_hardware_connected": hardware_connected,
+            "orientation_from_imu": has_imu,
+            "publish_foot_contacts": "True",
+            "close_loop_odom": "True",
             "joint_controller_topic": "joint_group_effort_controller/joint_trajectory",
             "joints_map_path": joints_config_path,
             "links_map_path": links_config_path,
             "gait_config_path": gait_config_path,
             "description_path": description_path
-        }.items(),
+        }.items()
     )
 
-    rviz2_node = Node(
-        package="rviz2",
-        namespace="",
-        executable="rviz2",
-        name="rviz2",
-        arguments=["-d", rviz_config_path],
-        condition=IfCondition(rviz)
-    )
-
-    return [rviz2_node,
-            bringup_launch]
-
-
-def generate_launch_description():
-    servo_interface_launch_path = PathJoinSubstitution(
-        [FindPackageShare('mini_pupper_driver'), 'launch', 'servo_interface.launch.py']
-    )
-    imu_launch_path = PathJoinSubstitution(
-        [FindPackageShare('mini_pupper_driver'), 'launch', 'imu_interface.launch.py']
-    )
-
-    lidar_launch_path = PathJoinSubstitution(
-        [FindPackageShare('mini_pupper_bringup'), 'launch', 'lidar.launch.py']
-    )
-
-    joint_hardware_connected = LaunchConfiguration("joint_hardware_connected")
-
-    declare_robot_name = DeclareLaunchArgument(
-            name='robot_name',
-            default_value='mini_pupper',
-            description='Set robot name for multi robot'
-        )
-
-    declare_sim = DeclareLaunchArgument(
-            name='sim',
-            default_value='false',
-            description='Enable use_sime_time to true'
-        )
-
-    declare_rviz = DeclareLaunchArgument(
-            name='rviz',
-            default_value='false',
-            description='Run rviz'
-        )
-
-    declare_hardware_connected = DeclareLaunchArgument(
-            name='joint_hardware_connected',
-            default_value='true',
-            description='Set to true if connected to a physical robot'
-        )
-
-    servo_interface_launch = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(servo_interface_launch_path),
-        condition=IfCondition(joint_hardware_connected),
-    )
-
-    imu_launch = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(imu_launch_path),
-        condition=IfCondition(joint_hardware_connected),
-    )
-
-    lidar_launch = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(lidar_launch_path),
-        condition=IfCondition(joint_hardware_connected),
+    hardware_interface_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(hardware_interface_launch_path),
+        condition=IfCondition(hardware_connected),
+        launch_arguments={
+            "has_lidar": has_lidar,
+            "has_imu": has_imu
+        }.items()
     )
 
     return LaunchDescription([
-        declare_robot_name,
-        declare_sim,
-        declare_rviz,
-        declare_hardware_connected,
-        OpaqueFunction(function=launch_bring_up),
-        servo_interface_launch,
-        imu_launch,
-        lidar_launch,
+        use_sim_time_launch_arg,
+        hardware_connected_launch_arg,
+        champ_bringup_launch,
+        hardware_interface_launch
     ])

mini_pupper_bringup/launch/hardware_interface.launch.py

@@ -0,0 +1,67 @@
+#!/usr/bin/env python3
+
+# SPDX-License-Identifier: Apache-2.0
+#
+# Copyright (c) 2022-2023 MangDang
+#
+# 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.
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.substitutions import FindPackageShare
+from launch.conditions import IfCondition
+
+
+def generate_launch_description():
+    has_lidar = LaunchConfiguration("has_lidar")
+    has_lidar_launch_arg = DeclareLaunchArgument(
+        name='has_lidar',
+        description='if the robot has lidar sensor'
+    )
+
+    has_imu = LaunchConfiguration("has_imu")
+    has_imu_launch_arg = DeclareLaunchArgument(
+        name='has_imu',
+        description='if the robot has imu sensor'
+    )
+
+    driver_package = FindPackageShare('mini_pupper_driver')
+
+    servos_launch_path = PathJoinSubstitution(
+        [driver_package, 'launch', 'servo_interface.launch.py']
+    )
+    lidar_launch_path = PathJoinSubstitution(
+        [driver_package, 'launch', 'lidar_ld06.launch.py']
+    )
+    imu_launch_path = PathJoinSubstitution(
+        [driver_package, 'launch', 'imu_interface.launch.py']
+    )
+
+    return LaunchDescription([
+        has_lidar_launch_arg,
+        has_imu_launch_arg,
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(servos_launch_path)
+        ),
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(lidar_launch_path),
+            condition=IfCondition(has_lidar)
+        ),
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(imu_launch_path),
+            condition=IfCondition(has_imu)
+        )
+    ])

mini_pupper_bringup/launch/rviz.launch.py

@@ -0,0 +1,40 @@
+#!/usr/bin/env python3
+
+# SPDX-License-Identifier: Apache-2.0
+#
+# Copyright (c) 2022-2023 MangDang
+#
+# 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.
+
+from launch import LaunchDescription
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    description_package = FindPackageShare('mini_pupper_description')
+
+    rviz_config_path = PathJoinSubstitution(
+        [description_package, 'rviz', 'urdf_viewer.rviz']
+    )
+
+    return LaunchDescription([
+       Node(
+            package="rviz2",
+            namespace="",
+            executable="rviz2",
+            name="rviz2",
+            arguments=["-d", rviz_config_path]
+        )
+    ])