diff --git a/src/cmp9767_tutorial/launch/limo_navigation_bringup.launch.py b/src/cmp9767_tutorial/launch/limo_navigation_bringup.launch.py new file mode 100644 index 0000000..dc23398 --- /dev/null +++ b/src/cmp9767_tutorial/launch/limo_navigation_bringup.launch.py @@ -0,0 +1,99 @@ +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_ros.actions import Node + +def generate_launch_description(): + # get package directory + pkg_dir = get_package_share_directory('limo_navigation') + + # Create the launch configuration variables + namespace = LaunchConfiguration('namespace') + use_sim_time = LaunchConfiguration('use_sim_time') + autostart = LaunchConfiguration('autostart') + map_yaml_file = LaunchConfiguration('map') + params_file = LaunchConfiguration('params_file') + + lifecycle_nodes = [ + # 'map_server', + # 'amcl', + 'controller_server', + 'planner_server', + 'behavior_server', + 'bt_navigator', + 'waypoint_follower'] + + declare_namespace = DeclareLaunchArgument( + 'namespace', default_value='', + description='Top-level namespace') + + declare_use_sim_time = DeclareLaunchArgument( + 'use_sim_time', default_value='false', + description='Use simulation (Gazebo) clock if true') + + declare_autostart = DeclareLaunchArgument( + 'autostart', default_value='true', + description='Automatically startup the nav2 stack') + + # declare_map = DeclareLaunchArgument( + # 'map', + # default_value=os.path.join(pkg_dir, 'maps', 'simple_map.yaml'), + # description='Full path to map yaml file to load') + + declare_params_file = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join( + pkg_dir, 'params', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use') + + # launch_limo_localization = IncludeLaunchDescription( + # PythonLaunchDescriptionSource( + # os.path.join(pkg_dir, 'launch', 'limo_localization.launch.py')), + # launch_arguments={ + # 'namespace': namespace, + # 'use_sim_time': use_sim_time, + # 'use_lifecycle_mgr': 'false', + # 'use_rviz': 'false', + # 'map': map_yaml_file, + # 'params_file': params_file, + # }.items()) + + launch_limo_controller = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_dir, 'launch', 'limo_controller.launch.py')), + launch_arguments={ + 'namespace': namespace, + 'use_sim_time': use_sim_time, + 'use_lifecycle_mgr': 'false' + }.items()) + + start_lifecycle_mgr = Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_navigation', + output='screen', + parameters=[{'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}]) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_use_sim_time) + ld.add_action(declare_autostart) + ld.add_action(declare_namespace) + # ld.add_action(declare_map) + ld.add_action(declare_params_file) + + # ld.add_action(launch_limo_localization) + ld.add_action(launch_limo_controller) + + ld.add_action(start_lifecycle_mgr) + + return ld diff --git a/src/cmp9767_tutorial/launch/limo_simulation.launch.py b/src/cmp9767_tutorial/launch/limo_simulation.launch.py new file mode 100644 index 0000000..dea37a0 --- /dev/null +++ b/src/cmp9767_tutorial/launch/limo_simulation.launch.py @@ -0,0 +1,257 @@ + + +import os +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.conditions import IfCondition, UnlessCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, LaunchConfiguration, PythonExpression +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + + # Constants for paths to different files and folders + urdf_model_name = 'limo_four_diff.gazebo' + world_file_name = 'simple.world' + rviz_config_file_name = 'urdf.rviz' + + robot_name_in_model = 'limo_gazebosim' + + # Pose where we want to spawn the robot + spawn_x_val = '0.0' + spawn_y_val = '0.0' + spawn_z_val = '0.0' + spawn_yaw_val = '0.00' + + # Define the robots with their unique names and positions + robots = [ + {'name': 'limo1', 'x': '0.0', 'y': '0.0', 'yaw': '0.0'}, + # {'name': 'limo2', 'x': '1.0', 'y': '0.0', 'yaw': '0.0'} + ] + ############ You do not need to change anything below this line ############# + + # Set the path to different files and folders. + pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') + + default_urdf_model_path = os.path.join( + get_package_share_directory('limo_description'), + 'urdf', + urdf_model_name + ) + + world_path = os.path.join( + get_package_share_directory('limo_gazebosim'), + 'worlds', + world_file_name + ) + + gazebo_models_path = os.path.join( + get_package_share_directory('limo_gazebosim'), + 'models' + ) + # os.environ["GAZEBO_MODEL_PATH"] = gazebo_models_path + local_models_path = os.path.join( + get_package_share_directory('cmp9767_tutorial'), + 'models' + ) + os.environ["GAZEBO_MODEL_PATH"] = os.pathsep.join(filter(None, [gazebo_models_path, local_models_path])) + + + default_rviz_config_path = os.path.join( + get_package_share_directory('limo_gazebosim'), + 'rviz', + rviz_config_file_name + ) + + + # Launch configuration variables specific to simulation + use_sim_time = LaunchConfiguration('use_sim_time', default='true') + gui = LaunchConfiguration('gui') + headless = LaunchConfiguration('headless') + namespace = LaunchConfiguration('namespace') + rviz_config_file = LaunchConfiguration('rviz_config_file') + urdf_model = LaunchConfiguration('urdf_model') + use_namespace = LaunchConfiguration('use_namespace') + use_robot_state_pub = LaunchConfiguration('use_robot_state_pub') + use_rviz = LaunchConfiguration('use_rviz') + use_simulator = LaunchConfiguration('use_simulator') + world = LaunchConfiguration('world') + + remappings = [((namespace, '/tf'), '/tf'), + ((namespace, '/tf_static'), '/tf_static'), + ('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Declare the launch arguments + declare_use_sim_time_cmd = DeclareLaunchArgument( + name='use_sim_time', + default_value='True', + description='Use simulation (Gazebo) clock if true') + + declare_use_joint_state_publisher_cmd = DeclareLaunchArgument( + name='gui', + default_value='False', + description='Flag to enable joint_state_publisher_gui') + + declare_namespace_cmd = DeclareLaunchArgument( + name='namespace', + default_value='', + description='Top-level namespace') + + declare_use_namespace_cmd = DeclareLaunchArgument( + name='use_namespace', + default_value='False', + description='Whether to apply a namespace to the navigation stack') + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + name='rviz_config_file', + default_value=default_rviz_config_path, + description='Full path to the RVIZ config file to use') + + declare_simulator_cmd = DeclareLaunchArgument( + name='headless', + default_value='False', + description='Whether to execute gzclient') + + declare_urdf_model_path_cmd = DeclareLaunchArgument( + name='urdf_model', + default_value=default_urdf_model_path, + description='Absolute path to robot urdf file') + + declare_use_robot_state_pub_cmd = DeclareLaunchArgument( + name='use_robot_state_pub', + default_value='True', + description='Whether to start the robot state publisher') + + declare_use_rviz_cmd = DeclareLaunchArgument( + name='use_rviz', + default_value='False', + description='Whether to start RVIZ') + + declare_use_simulator_cmd = DeclareLaunchArgument( + name='use_simulator', + default_value='True', + description='Whether to start the simulator') + + declare_world_cmd = DeclareLaunchArgument( + name='world', + default_value=world_path, + description='Full path to the world model file to load') + + # start_dummy_sensors=Node( + # package='dummy_sensors', + # node_executable='dummy_joint_states', + # output='screen') + + # Launch RViz + start_rviz_cmd = Node( + condition=IfCondition(use_rviz), + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', rviz_config_file]) + + # Start Gazebo server + start_gazebo_server_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')), + condition=IfCondition(use_simulator), + launch_arguments={'world': world}.items()) + + # Start Gazebo client + start_gazebo_client_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')), + condition=IfCondition(PythonExpression([use_simulator, ' and not ', headless]))) + twist_watchdog = Node( + package='limo_gazebosim', + executable='twist_watchdog.py', + name='twist_watchdog' + ) + + ld = LaunchDescription() + # Declare the launch options + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_joint_state_publisher_cmd) + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_namespace_cmd) + ld.add_action(declare_rviz_config_file_cmd) + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_urdf_model_path_cmd) + ld.add_action(declare_use_robot_state_pub_cmd) + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + + ld.add_action(start_rviz_cmd) + ld.add_action(twist_watchdog) + + # Launch the robot +# remappings = [("/tf", "tf"), ("/tf_static", "tf_static")] + for robot in robots: + ns = robot['name'] + + remappings = [((ns, '/tf'), '/tf'), + ((ns, '/tf_static'), '/tf_static'), + ('/tf', 'tf'), + ('/tf_static', 'tf_static')] + spawn_entity_cmd = Node( + package='gazebo_ros', + executable='spawn_entity.py', + arguments=['-entity', robot['name'], + '-x', robot['x'], + '-y', robot['y'], + '-z', spawn_z_val, + '-Y', robot['yaw'], + '-topic', 'robot_description', + '-robot_namespace', ns], + output='screen') + # arguments=['-entity', robot_name_in_model, + # '-topic', 'robot_description', + # '-x', spawn_x_val, + # '-y', spawn_y_val, + # '-z', spawn_z_val, + # '-Y', spawn_yaw_val], + # output='screen') + + # Subscribe to the joint states of the robot, and publish the 3D pose of each link. + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + namespace=ns, + parameters=[{'robot_description': Command(['xacro ', urdf_model]),'use_sim_time': use_sim_time}], + remappings=remappings, + output='screen' + ) + # Publish the joint states of the robot + start_joint_state_publisher_cmd = Node( + package='joint_state_publisher', + executable='joint_state_publisher', + namespace=ns, + name='joint_state_publisher', + condition=UnlessCondition(gui), + parameters=[{'use_sim_time': use_sim_time}], + remappings=remappings, + output='screen' + ) + + + # Add actions to the launch description + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(start_joint_state_publisher_cmd) + ld.add_action(spawn_entity_cmd) + + + + # Create the launch description and populate + + + + + return ld \ No newline at end of file diff --git a/src/cmp9767_tutorial/launch/limo_simulation_multi.launch.py b/src/cmp9767_tutorial/launch/limo_simulation_multi.launch.py new file mode 100644 index 0000000..5b1fa5f --- /dev/null +++ b/src/cmp9767_tutorial/launch/limo_simulation_multi.launch.py @@ -0,0 +1,215 @@ +import os +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition, UnlessCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, LaunchConfiguration, PythonExpression +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + + # Constants for paths to different files and folders + urdf_model_name = 'limo_four_diff.gazebo' + world_file_name = 'simple.world' + rviz_config_file_name = 'urdf.rviz' + + robot_name_in_model = 'limo_gazebosim' + + # Pose where we want to spawn the robots + spawn_z_val = '0.0' + + ############ You do not need to change anything below this line ############# + + # Set the path to different files and folders. + pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') + + default_urdf_model_path = os.path.join( + get_package_share_directory('limo_description'), + 'urdf', + urdf_model_name + ) + + world_path = os.path.join( + get_package_share_directory('limo_gazebosim'), + 'worlds', + world_file_name + ) + + gazebo_models_path = os.path.join( + get_package_share_directory('limo_gazebosim'), + 'models' + ) + os.environ["GAZEBO_MODEL_PATH"] = gazebo_models_path + + default_rviz_config_path = os.path.join( + get_package_share_directory('limo_gazebosim'), + 'rviz', + rviz_config_file_name + ) + + # Launch configuration variables specific to simulation + use_sim_time = LaunchConfiguration('use_sim_time', default='true') + gui = LaunchConfiguration('gui') + headless = LaunchConfiguration('headless') + namespace = LaunchConfiguration('namespace') + rviz_config_file = LaunchConfiguration('rviz_config_file') + urdf_model = LaunchConfiguration('urdf_model') + use_namespace = LaunchConfiguration('use_namespace') + use_robot_state_pub = LaunchConfiguration('use_robot_state_pub') + use_rviz = LaunchConfiguration('use_rviz') + use_simulator = LaunchConfiguration('use_simulator') + world = LaunchConfiguration('world') + + # Declare the launch arguments + declare_use_sim_time_cmd = DeclareLaunchArgument( + name='use_sim_time', + default_value='True', + description='Use simulation (Gazebo) clock if true') + + declare_use_joint_state_publisher_cmd = DeclareLaunchArgument( + name='gui', + default_value='False', + description='Flag to enable joint_state_publisher_gui') + + declare_namespace_cmd = DeclareLaunchArgument( + name='namespace', + default_value='', + description='Top-level namespace') + + declare_use_namespace_cmd = DeclareLaunchArgument( + name='use_namespace', + default_value='True', + description='Whether to apply a namespace to the navigation stack') + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + name='rviz_config_file', + default_value=default_rviz_config_path, + description='Full path to the RVIZ config file to use') + + declare_simulator_cmd = DeclareLaunchArgument( + name='headless', + default_value='False', + description='Whether to execute gzclient') + + declare_urdf_model_path_cmd = DeclareLaunchArgument( + name='urdf_model', + default_value=default_urdf_model_path, + description='Absolute path to robot urdf file') + + declare_use_rviz_cmd = DeclareLaunchArgument( + name='use_rviz', + default_value='False', + description='Whether to start RVIZ') + + declare_use_simulator_cmd = DeclareLaunchArgument( + name='use_simulator', + default_value='True', + description='Whether to start the simulator') + + declare_world_cmd = DeclareLaunchArgument( + name='world', + default_value=world_path, + description='Full path to the world model file to load') + + # Start Gazebo server + start_gazebo_server_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')), + condition=IfCondition(use_simulator), + launch_arguments={'world': world}.items()) + + # Start Gazebo client + start_gazebo_client_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')), + condition=IfCondition( + PythonExpression([use_simulator, ' and not ', headless]))) + + twist_watchdog = Node( + package='limo_gazebosim', + executable='twist_watchdog.py', + name='twist_watchdog' + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_joint_state_publisher_cmd) + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_namespace_cmd) + ld.add_action(declare_rviz_config_file_cmd) + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_urdf_model_path_cmd) + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + ld.add_action(twist_watchdog) + + # Add actions to launch Gazebo + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + + # Define the robots with their unique names and positions + robots = [ + {'name': 'limo1', 'x': '0.0', 'y': '0.0', 'yaw': '0.0'}, + # {'name': 'limo2', 'x': '1.0', 'y': '0.0', 'yaw': '0.0'} + ] + + # Loop through each robot and launch the required nodes + for robot in robots: + ns = robot['name'] + + # Start robot state publisher + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + namespace=ns, + parameters=[{'robot_description': Command(['xacro ', urdf_model]), + 'use_sim_time': use_sim_time}], + ) + + # Start joint state publisher + start_joint_state_publisher_cmd = Node( + package='joint_state_publisher', + executable='joint_state_publisher', + namespace=ns, + name='joint_state_publisher', + condition=UnlessCondition(gui), + parameters=[{'use_sim_time': use_sim_time}], + ) + + # Spawn the robot in Gazebo + spawn_entity_cmd = Node( + package='gazebo_ros', + executable='spawn_entity.py', + arguments=['-entity', robot['name'], + '-x', robot['x'], + '-y', robot['y'], + '-z', spawn_z_val, + '-Y', robot['yaw'], + '-topic', 'robot_description', + '-robot_namespace', ns], + output='screen' + ) + + # Add actions to the launch description + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(start_joint_state_publisher_cmd) + ld.add_action(spawn_entity_cmd) + + # Launch RViz + start_rviz_cmd = Node( + condition=IfCondition(use_rviz), + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', rviz_config_file]) + + ld.add_action(start_rviz_cmd) + + return ld diff --git a/src/cmp9767_tutorial/models/fire_extinguisher/model.config b/src/cmp9767_tutorial/models/fire_extinguisher/model.config new file mode 100644 index 0000000..e840c8d --- /dev/null +++ b/src/cmp9767_tutorial/models/fire_extinguisher/model.config @@ -0,0 +1,8 @@ + + + fire_extinguisher + 1.0 + model.sdf + CMP9767 + Simple red cylinder stand-in + diff --git a/src/cmp9767_tutorial/models/fire_extinguisher/model.sdf b/src/cmp9767_tutorial/models/fire_extinguisher/model.sdf new file mode 100644 index 0000000..d164169 --- /dev/null +++ b/src/cmp9767_tutorial/models/fire_extinguisher/model.sdf @@ -0,0 +1,14 @@ + + + true + 0 0 0.35 0 0 0 + + 0.060.45 + + + 0.060.45 + 0.8 0 0 10.8 0 0 1 + + + + diff --git a/src/cmp9767_tutorial/models/first_aid_kit/model.config b/src/cmp9767_tutorial/models/first_aid_kit/model.config new file mode 100644 index 0000000..d7f43e5 --- /dev/null +++ b/src/cmp9767_tutorial/models/first_aid_kit/model.config @@ -0,0 +1,8 @@ + + + first_aid_kit + 1.0 + model.sdf + CMP9767 + Green box + diff --git a/src/cmp9767_tutorial/models/first_aid_kit/model.sdf b/src/cmp9767_tutorial/models/first_aid_kit/model.sdf new file mode 100644 index 0000000..be54e3e --- /dev/null +++ b/src/cmp9767_tutorial/models/first_aid_kit/model.sdf @@ -0,0 +1,12 @@ + + + true + 0 0 0.07 0 0 0 + 0.28 0.18 0.14 + + 0.28 0.18 0.14 + 0 0.5 0 10 0.5 0 1 + + + + diff --git a/src/cmp9767_tutorial/params/nav2_params.yaml b/src/cmp9767_tutorial/param/nav2_params.yaml similarity index 100% rename from src/cmp9767_tutorial/params/nav2_params.yaml rename to src/cmp9767_tutorial/param/nav2_params.yaml diff --git a/src/cmp9767_tutorial/setup.py b/src/cmp9767_tutorial/setup.py index 1051024..749ff9b 100644 --- a/src/cmp9767_tutorial/setup.py +++ b/src/cmp9767_tutorial/setup.py @@ -1,5 +1,5 @@ from setuptools import find_packages, setup -import os +from os import path from glob import glob package_name = 'cmp9767_tutorial' @@ -12,7 +12,14 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), - (os.path.join('share', package_name), glob('launch/*launch.py')), + (path.join('share', package_name, 'launch'), glob(path.join('launch', '*launch.[pxy][yma]*'))), + (path.join('share', package_name, 'config'), glob(path.join('config', '*.yaml'))), + (path.join('share', package_name, 'param'), glob(path.join('param', '*.yaml'))), + (path.join('share', package_name, 'urdf'), glob(path.join('urdf', 'tidybot.*'))), + (path.join('share', package_name, 'worlds'), glob(path.join('worlds', '*.world'))), + (path.join('share', package_name, 'models', 'fire_extinguisher'), glob(path.join('models', 'fire_extinguisher', 'model.*'))), + (path.join('share', package_name, 'models', 'first_aid_kit'), glob(path.join('models', 'first_aid_kit', 'model.*'))), + (path.join('share', package_name, 'meshes'), glob(path.join('meshes', '*'))), ], install_requires=['setuptools'], zip_safe=True,