diff --git a/README.md b/README.md index 6f02f2f..d8f0a84 100755 --- a/README.md +++ b/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 diff --git a/mini_pupper_bringup/CMakeLists.txt b/mini_pupper_bringup/CMakeLists.txt index 2fd4049..44363cf 100644 --- a/mini_pupper_bringup/CMakeLists.txt +++ b/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} ) diff --git a/mini_pupper_bringup/config/mini_pupper.yaml b/mini_pupper_bringup/config/mini_pupper.yaml new file mode 100644 index 0000000..4bafe2d --- /dev/null +++ b/mini_pupper_bringup/config/mini_pupper.yaml @@ -0,0 +1,3 @@ +sensors: + lidar: true + imu: false \ No newline at end of file diff --git a/mini_pupper_bringup/config/mini_pupper_2.yaml b/mini_pupper_bringup/config/mini_pupper_2.yaml new file mode 100644 index 0000000..1aa6ce0 --- /dev/null +++ b/mini_pupper_bringup/config/mini_pupper_2.yaml @@ -0,0 +1,3 @@ +sensors: + lidar: true + imu: true \ No newline at end of file diff --git a/mini_pupper_bringup/launch/bringup.launch.py b/mini_pupper_bringup/launch/bringup.launch.py index 9f93dc5..4486250 100644 --- a/mini_pupper_bringup/launch/bringup.launch.py +++ b/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 ]) diff --git a/mini_pupper_bringup/launch/hardware_interface.launch.py b/mini_pupper_bringup/launch/hardware_interface.launch.py new file mode 100644 index 0000000..3d473ad --- /dev/null +++ b/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) + ) + ]) diff --git a/mini_pupper_bringup/launch/rviz.launch.py b/mini_pupper_bringup/launch/rviz.launch.py new file mode 100644 index 0000000..5294edf --- /dev/null +++ b/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] + ) + ]) diff --git a/mini_pupper_bringup/launch/lidar.launch.py b/mini_pupper_driver/launch/lidar_ld06.launch.py similarity index 100% rename from mini_pupper_bringup/launch/lidar.launch.py rename to mini_pupper_driver/launch/lidar_ld06.launch.py diff --git a/mini_pupper_gazebo/launch/gazebo.launch.py b/mini_pupper_gazebo/launch/gazebo.launch.py index 7f1f0a4..e8d904c 100644 --- a/mini_pupper_gazebo/launch/gazebo.launch.py +++ b/mini_pupper_gazebo/launch/gazebo.launch.py @@ -16,139 +16,96 @@ # 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 BSD-3-Clause License. -# https://spdx.org/licenses/BSD-3-Clause.html -# -# Copyright (c) 2019-2020 Juan Miguel Jimeno -# -# https://github.com/chvmp/champ/blob/f76d066d8964c8286afbcd9d5d2c08d781e85f54/champ_gazebo/launch/gazebo.launch.py import os -from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + +ROBOT_MODEL = os.getenv('ROBOT_MODEL', default="mini_pupper_2") def generate_launch_description(): - gazebo_package = get_package_share_directory('mini_pupper_gazebo') - bringup_package = get_package_share_directory('mini_pupper_bringup') - - default_world_path = os.path.join( - gazebo_package, 'worlds', 'mini_pupper_home.world') - - bringup_launch_path = os.path.join( - bringup_package, 'launch', 'bringup.launch.py') - - robot_name = LaunchConfiguration("robot_name") - sim = LaunchConfiguration("sim") - joint_hardware_connected = LaunchConfiguration("joint_hardware_connected") - rviz = LaunchConfiguration("rviz") - lite = LaunchConfiguration("lite") - world = LaunchConfiguration("world"), - world_init_x = LaunchConfiguration("world_init_x"), - world_init_y = LaunchConfiguration("world_init_y"), - world_init_z = LaunchConfiguration("world_init_z"), - - world_init_heading = LaunchConfiguration("world_init_heading"), - gui = LaunchConfiguration("gui"), - - declare_rviz = DeclareLaunchArgument( - name="rviz", - default_value="false", - description="Launch rviz" + if ROBOT_MODEL == "mini_pupper_2": + description_package = FindPackageShare('mini_pupper_2_description') + else: + description_package = FindPackageShare('mini_pupper_description') + + links_map_path = PathJoinSubstitution( + [description_package, 'config', 'champ', 'links.yaml'] ) - declare_robot_name = DeclareLaunchArgument( - name="robot_name", - default_value="mini_pupper", - description="Robot name" + + this_package = FindPackageShare('mini_pupper_gazebo') + default_world_path = PathJoinSubstitution([this_package, 'worlds', 'mini_pupper_home.world']) + + bringup_launch_path = PathJoinSubstitution( + [FindPackageShare('mini_pupper_bringup'), 'launch', 'bringup.launch.py'] ) - declare_lite = DeclareLaunchArgument( - name="lite", - default_value="false", - description="Lite" + + champ_gazebo_launch_path = PathJoinSubstitution( + [FindPackageShare('champ_gazebo'), 'launch', 'gazebo.launch.py'] ) - declare_world = DeclareLaunchArgument( + world = LaunchConfiguration("world") + world_launch_arg = DeclareLaunchArgument( name="world", default_value=default_world_path, description="Gazebo world path" ) - declare_gui = DeclareLaunchArgument( - name="gui", - default_value="true", - description="Use gui" - ) - declare_world_init_x = DeclareLaunchArgument( + + world_init_x = LaunchConfiguration("world_init_x") + world_init_x_launch_arg = DeclareLaunchArgument( name="world_init_x", default_value="0.0" ) - declare_world_init_y = DeclareLaunchArgument( + + world_init_y = LaunchConfiguration("world_init_y") + world_init_y_launch_arg = DeclareLaunchArgument( name="world_init_y", default_value="0.0" ) - declare_world_init_z = DeclareLaunchArgument( + + world_init_z = LaunchConfiguration("world_init_z") + world_init_z_launch_arg = DeclareLaunchArgument( name="world_init_z", default_value="0.066" ) - declare_world_init_heading = DeclareLaunchArgument( + + world_init_heading = LaunchConfiguration("world_init_heading") + world_init_heading_launch_arg = DeclareLaunchArgument( name="world_init_heading", default_value="0.0" ) - declare_sim = DeclareLaunchArgument( - name='sim', - default_value='true', - description='Enable use_sime_time to true' - ) - declare_joint_hardware_connected = DeclareLaunchArgument( - name='joint_hardware_connected', - default_value='false', - description='Set to true if connected to a physical robot' - ) mini_pupper_bringup_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource(bringup_launch_path), launch_arguments={ - "use_sim_time": sim, - "robot_name": robot_name, - "gazebo": sim, - "rviz": rviz, - "joint_hardware_connected": joint_hardware_connected, - "publish_foot_contacts": "true", - "close_loop_odom": "true" - }.items(), + "use_sim_time": "True", + "hardware_connected": "False" + }.items() ) + champ_gazebo_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join( - get_package_share_directory("champ_gazebo"), "launch", "gazebo.launch.py")), + PythonLaunchDescriptionSource(champ_gazebo_launch_path), launch_arguments={ - "use_sim_time": sim, - "robot_name": robot_name, + "robot_name": ROBOT_MODEL, "world": world, - "lite": lite, + "links_map_path": links_map_path, "world_init_x": world_init_x, "world_init_y": world_init_y, "world_init_z": world_init_z, - "world_init_heading": world_init_heading, - "gui": gui, - "close_loop_odom": "true", - }.items(), + "world_init_heading": world_init_heading + }.items() ) return LaunchDescription([ - declare_rviz, - declare_robot_name, - declare_lite, - declare_world, - declare_gui, - declare_world_init_x, - declare_world_init_y, - declare_world_init_z, - declare_world_init_heading, - declare_sim, - declare_joint_hardware_connected, + world_launch_arg, + world_init_x_launch_arg, + world_init_y_launch_arg, + world_init_z_launch_arg, + world_init_heading_launch_arg, mini_pupper_bringup_launch, champ_gazebo_launch ]) diff --git a/mini_pupper_navigation/launch/navigation.launch.py b/mini_pupper_navigation/launch/navigation.launch.py index a3c08c5..9064122 100644 --- a/mini_pupper_navigation/launch/navigation.launch.py +++ b/mini_pupper_navigation/launch/navigation.launch.py @@ -37,14 +37,14 @@ def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time') use_sim_time_launch_arg = DeclareLaunchArgument( - 'use_sim_time', + name='use_sim_time', default_value='False', description='Use simulation (Gazebo) clock if true' ) map = LaunchConfiguration('map') map_launch_arg = DeclareLaunchArgument( - 'map', + name='map', default_value=default_map_path, description='Full path to map file to load' ) diff --git a/mini_pupper_slam/launch/slam.launch.py b/mini_pupper_slam/launch/slam.launch.py index f4c4af3..b32d50e 100644 --- a/mini_pupper_slam/launch/slam.launch.py +++ b/mini_pupper_slam/launch/slam.launch.py @@ -32,7 +32,7 @@ def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time') use_sim_time_launch_arg = DeclareLaunchArgument( - 'use_sim_time', + name='use_sim_time', default_value='False', description='Use simulation (Gazebo) clock if true' )