diff --git a/.github/workflows/python_lint.yml b/.github/workflows/python_lint.yml new file mode 100644 index 000000000..b6cb96732 --- /dev/null +++ b/.github/workflows/python_lint.yml @@ -0,0 +1,32 @@ +# Ensure Python code is linted and formatted, matching the style guide +name: Python Linting + +on: [push, pull_request] + +jobs: + black-lint: + runs-on: ubuntu-latest + name: Python black Lint + steps: + - uses: actions/checkout@v4 + - uses: psf/black@stable + flake8-lint: + runs-on: ubuntu-latest + name: Python flake8 Lint + steps: + - name: Check out source repository + uses: actions/checkout@v3 + + - name: Set up Python environment + uses: actions/setup-python@v4 + with: + python-version: "3.11" + + - name: flake8 Lint + uses: py-actions/flake8@v2 + with: + ignore: "E701,W503,E203" + exclude: "docs,.idea" + max-line-length: "88" + path: "." + plugins: "flake8-docstrings flake8-black" \ No newline at end of file diff --git a/CustomRobots/Turtlebot2/Turtlebot2_physical/kobuki_launch/launch/kobuki_base.launch.py b/CustomRobots/Turtlebot2/Turtlebot2_physical/kobuki_launch/launch/kobuki_base.launch.py index 63fba3b4d..1660f5836 100644 --- a/CustomRobots/Turtlebot2/Turtlebot2_physical/kobuki_launch/launch/kobuki_base.launch.py +++ b/CustomRobots/Turtlebot2/Turtlebot2_physical/kobuki_launch/launch/kobuki_base.launch.py @@ -4,21 +4,23 @@ from launch_ros.actions import Node import yaml + def generate_launch_description(): - shared_dir = get_package_share_directory('kobuki_launch') + shared_dir = get_package_share_directory("kobuki_launch") - params_file = os.path.join(shared_dir, 'config', 'kobuki_node_params.yaml') - with open(params_file, 'r') as f: - kobuki_params = yaml.safe_load(f)['kobuki_ros_node']['ros__parameters'] + params_file = os.path.join(shared_dir, "config", "kobuki_node_params.yaml") + with open(params_file, "r") as f: + kobuki_params = yaml.safe_load(f)["kobuki_ros_node"]["ros__parameters"] - kobuki_cmd = Node(package='kobuki_node', - executable='kobuki_ros_node', - output='screen', + kobuki_cmd = Node( + package="kobuki_node", + executable="kobuki_ros_node", + output="screen", parameters=[kobuki_params], - ) + ) ld = LaunchDescription() ld.add_action(kobuki_cmd) - + return ld diff --git a/CustomRobots/Turtlebot2/turtlebot2_simulated/kobuki_description/launch/robot_description.launch.py b/CustomRobots/Turtlebot2/turtlebot2_simulated/kobuki_description/launch/robot_description.launch.py index bff6ede53..c467f9a31 100755 --- a/CustomRobots/Turtlebot2/turtlebot2_simulated/kobuki_description/launch/robot_description.launch.py +++ b/CustomRobots/Turtlebot2/turtlebot2_simulated/kobuki_description/launch/robot_description.launch.py @@ -42,62 +42,54 @@ def generate_launch_description(): urdf_path = PathJoinSubstitution( - [FindPackageShare('custom_robots'), 'urdf', 'kobuki_standalone.urdf.xacro'] + [FindPackageShare("custom_robots"), "urdf", "kobuki_standalone.urdf.xacro"] ) rviz_config_path = PathJoinSubstitution( - [FindPackageShare('custom_robots'), 'rviz', 'model.rviz'] + [FindPackageShare("custom_robots"), "rviz", "model.rviz"] ) - return LaunchDescription([ - - DeclareLaunchArgument( - name='urdf', - default_value=urdf_path, - description='URDF path' - ), - - DeclareLaunchArgument( - name='use_sim_time', - default_value='false', - description='Use simulation time' - ), - - DeclareLaunchArgument( - name='rviz', - default_value='false', - description='Run rviz' - ), - - Node( - package='robot_state_publisher', - executable='robot_state_publisher', - name='robot_state_publisher', - output='screen', - parameters=[{ - 'use_sim_time': LaunchConfiguration('use_sim_time'), - 'robot_description': Command(['xacro ', LaunchConfiguration('urdf')]) - }] - ), - - Node( - package='joint_state_publisher', - executable='joint_state_publisher', - name='joint_state_publisher', - parameters=[{ - 'use_sim_time': LaunchConfiguration('use_sim_time') - }] - ), - - Node( - package='rviz2', - executable='rviz2', - name='rviz2', - output='screen', - arguments=['-d', rviz_config_path], - condition=IfCondition(LaunchConfiguration('rviz')), - parameters=[{ - 'use_sim_time': LaunchConfiguration('use_sim_time') - }] - ), - ]) + return LaunchDescription( + [ + DeclareLaunchArgument( + name="urdf", default_value=urdf_path, description="URDF path" + ), + DeclareLaunchArgument( + name="use_sim_time", + default_value="false", + description="Use simulation time", + ), + DeclareLaunchArgument( + name="rviz", default_value="false", description="Run rviz" + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + parameters=[ + { + "use_sim_time": LaunchConfiguration("use_sim_time"), + "robot_description": Command( + ["xacro ", LaunchConfiguration("urdf")] + ), + } + ], + ), + Node( + package="joint_state_publisher", + executable="joint_state_publisher", + name="joint_state_publisher", + parameters=[{"use_sim_time": LaunchConfiguration("use_sim_time")}], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="screen", + arguments=["-d", rviz_config_path], + condition=IfCondition(LaunchConfiguration("rviz")), + parameters=[{"use_sim_time": LaunchConfiguration("use_sim_time")}], + ), + ] + ) diff --git a/CustomRobots/Turtlebot2/turtlebot2_simulated/turtlebot2/launch/empty_world.launch.py b/CustomRobots/Turtlebot2/turtlebot2_simulated/turtlebot2/launch/empty_world.launch.py index d17e61fa8..8ca103805 100755 --- a/CustomRobots/Turtlebot2/turtlebot2_simulated/turtlebot2/launch/empty_world.launch.py +++ b/CustomRobots/Turtlebot2/turtlebot2_simulated/turtlebot2/launch/empty_world.launch.py @@ -2,7 +2,12 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import ExecuteProcess, DeclareLaunchArgument, IncludeLaunchDescription, SetEnvironmentVariable +from launch.actions import ( + ExecuteProcess, + DeclareLaunchArgument, + IncludeLaunchDescription, + SetEnvironmentVariable, +) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, Command from launch_ros.actions import Node @@ -10,25 +15,31 @@ def generate_launch_description(): - ld = LaunchDescription() + ld = LaunchDescription() - use_sim_time = LaunchConfiguration('use_sim_time', default='True') - - pkg_gazebo_ros = get_package_share_directory('gazebo_ros') + use_sim_time = LaunchConfiguration("use_sim_time", default="True") - gazebo_server = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')) - ) + pkg_gazebo_ros = get_package_share_directory("gazebo_ros") - gazebo_client = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')) - ) + gazebo_server = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_gazebo_ros, "launch", "gzserver.launch.py") + ) + ) - execute_process = ExecuteProcess( - cmd=['ros2', 'param', 'set', '/gazebo', 'use_sim_time', use_sim_time], output='screen') - - ld.add_action(gazebo_server) - ld.add_action(gazebo_client) - ld.add_action(execute_process) + gazebo_client = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_gazebo_ros, "launch", "gzclient.launch.py") + ) + ) - return ld + execute_process = ExecuteProcess( + cmd=["ros2", "param", "set", "/gazebo", "use_sim_time", use_sim_time], + output="screen", + ) + + ld.add_action(gazebo_server) + ld.add_action(gazebo_client) + ld.add_action(execute_process) + + return ld diff --git a/CustomRobots/Turtlebot2/turtlebot2_simulated/turtlebot2/launch/spawn_model.launch.py b/CustomRobots/Turtlebot2/turtlebot2_simulated/turtlebot2/launch/spawn_model.launch.py index c02834f25..124c5f518 100755 --- a/CustomRobots/Turtlebot2/turtlebot2_simulated/turtlebot2/launch/spawn_model.launch.py +++ b/CustomRobots/Turtlebot2/turtlebot2_simulated/turtlebot2/launch/spawn_model.launch.py @@ -7,53 +7,67 @@ from launch_ros.actions import Node import time + def generate_launch_description(): - # In order to ensure gazebo is properly intialized - time.sleep(4.0) - - # Set (x, y, z) default position of turtlebot2 - x_pos = LaunchConfiguration('-x', default='0') - y_pos = LaunchConfiguration('-y', default='0') - z_pos = LaunchConfiguration('-z', default='0') - - # Find Turtlebot2 package - pkg_share = launch_ros.substitutions.FindPackageShare(package='custom_robots').find('custom_robots') - - # Find Urdf File - urdf_file = os.path.join(pkg_share, 'urdf/turtlebot2.urdf') - - # Open Urdf file - with open(urdf_file, 'r') as info: - robot_desc = info.read() - - # Robot description Turtlebot2 - turtlebot2_model = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - parameters=[{'robot_description': robot_desc}], - arguments=[urdf_file] - ) - - # TF Tree - joint_state_publisher_node = Node( - package='joint_state_publisher', - executable='joint_state_publisher', - name='joint_state_publisher' - ) - - # Spawn Turtlebot2 - spawn_entity_node = Node( - package='gazebo_ros', - executable='spawn_entity.py', - name='entity_spawner', - output='screen', - arguments=["-topic", "/robot_description", "-entity", "turtlebot2", "-x", x_pos, "-y", y_pos, "-z", z_pos] - ) - - ld = LaunchDescription() - ld.add_action(turtlebot2_model) - ld.add_action(joint_state_publisher_node) - ld.add_action(spawn_entity_node) - - return ld \ No newline at end of file + # In order to ensure gazebo is properly intialized + time.sleep(4.0) + + # Set (x, y, z) default position of turtlebot2 + x_pos = LaunchConfiguration("-x", default="0") + y_pos = LaunchConfiguration("-y", default="0") + z_pos = LaunchConfiguration("-z", default="0") + + # Find Turtlebot2 package + pkg_share = launch_ros.substitutions.FindPackageShare(package="custom_robots").find( + "custom_robots" + ) + + # Find Urdf File + urdf_file = os.path.join(pkg_share, "urdf/turtlebot2.urdf") + + # Open Urdf file + with open(urdf_file, "r") as info: + robot_desc = info.read() + + # Robot description Turtlebot2 + turtlebot2_model = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + parameters=[{"robot_description": robot_desc}], + arguments=[urdf_file], + ) + + # TF Tree + joint_state_publisher_node = Node( + package="joint_state_publisher", + executable="joint_state_publisher", + name="joint_state_publisher", + ) + + # Spawn Turtlebot2 + spawn_entity_node = Node( + package="gazebo_ros", + executable="spawn_entity.py", + name="entity_spawner", + output="screen", + arguments=[ + "-topic", + "/robot_description", + "-entity", + "turtlebot2", + "-x", + x_pos, + "-y", + y_pos, + "-z", + z_pos, + ], + ) + + ld = LaunchDescription() + ld.add_action(turtlebot2_model) + ld.add_action(joint_state_publisher_node) + ld.add_action(spawn_entity_node) + + return ld diff --git a/CustomRobots/ackermann_cars/launch/autocar_autoparking.launch.py b/CustomRobots/ackermann_cars/launch/autocar_autoparking.launch.py index 566af2365..da1adaf45 100755 --- a/CustomRobots/ackermann_cars/launch/autocar_autoparking.launch.py +++ b/CustomRobots/ackermann_cars/launch/autocar_autoparking.launch.py @@ -6,74 +6,87 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'autoparking_autocar.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "autoparking_autocar.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/ackermann_cars/launch/f1_autoparking.launch.py b/CustomRobots/ackermann_cars/launch/f1_autoparking.launch.py index 62c207ea9..1ab256dfa 100755 --- a/CustomRobots/ackermann_cars/launch/f1_autoparking.launch.py +++ b/CustomRobots/ackermann_cars/launch/f1_autoparking.launch.py @@ -6,74 +6,87 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'autoparking_f1.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "autoparking_f1.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/ackermann_cars/launch/lincoln.launch.py b/CustomRobots/ackermann_cars/launch/lincoln.launch.py index af0c1e3b1..730262f62 100644 --- a/CustomRobots/ackermann_cars/launch/lincoln.launch.py +++ b/CustomRobots/ackermann_cars/launch/lincoln.launch.py @@ -7,74 +7,86 @@ from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare + def generate_launch_description(): - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to the car ROS package - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'lincoln.world' - world_path = os.path.join(pkg_share, "worlds/", world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - return ld + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to the car ROS package + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "lincoln.world" + world_path = os.path.join(pkg_share, "worlds/", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/ackermann_cars/launch/montmelo_circuit_ackermann.launch.py b/CustomRobots/ackermann_cars/launch/montmelo_circuit_ackermann.launch.py index 7f511dc73..6c92afd89 100755 --- a/CustomRobots/ackermann_cars/launch/montmelo_circuit_ackermann.launch.py +++ b/CustomRobots/ackermann_cars/launch/montmelo_circuit_ackermann.launch.py @@ -6,74 +6,87 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'montmelo_line_ackermann.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "montmelo_line_ackermann.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/ackermann_cars/launch/montmelo_circuit_ackermann_followingcam.launch.py b/CustomRobots/ackermann_cars/launch/montmelo_circuit_ackermann_followingcam.launch.py index b62eede1b..2c77e965e 100755 --- a/CustomRobots/ackermann_cars/launch/montmelo_circuit_ackermann_followingcam.launch.py +++ b/CustomRobots/ackermann_cars/launch/montmelo_circuit_ackermann_followingcam.launch.py @@ -6,74 +6,87 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'montmelo_line_ackermann_followingcam.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "montmelo_line_ackermann_followingcam.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/ackermann_cars/launch/montreal_circuit_ackermann.launch.py b/CustomRobots/ackermann_cars/launch/montreal_circuit_ackermann.launch.py index f52d18f06..241a4a106 100755 --- a/CustomRobots/ackermann_cars/launch/montreal_circuit_ackermann.launch.py +++ b/CustomRobots/ackermann_cars/launch/montreal_circuit_ackermann.launch.py @@ -6,74 +6,87 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'montreal_line_ackermann.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "montreal_line_ackermann.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/ackermann_cars/launch/montreal_circuit_ackermann_followingcam.launch.py b/CustomRobots/ackermann_cars/launch/montreal_circuit_ackermann_followingcam.launch.py index 0af7842cf..2ac501ca3 100755 --- a/CustomRobots/ackermann_cars/launch/montreal_circuit_ackermann_followingcam.launch.py +++ b/CustomRobots/ackermann_cars/launch/montreal_circuit_ackermann_followingcam.launch.py @@ -6,74 +6,87 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'montreal_line_ackermann_followingcam.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "montreal_line_ackermann_followingcam.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/ackermann_cars/launch/nurburgring_circuit_ackermann.launch.py b/CustomRobots/ackermann_cars/launch/nurburgring_circuit_ackermann.launch.py index f815850e5..3374b6879 100755 --- a/CustomRobots/ackermann_cars/launch/nurburgring_circuit_ackermann.launch.py +++ b/CustomRobots/ackermann_cars/launch/nurburgring_circuit_ackermann.launch.py @@ -6,74 +6,87 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'nurburgring_line_ackermann.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "nurburgring_line_ackermann.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/ackermann_cars/launch/nurburgring_circuit_ackermann_followingcam.launch.py b/CustomRobots/ackermann_cars/launch/nurburgring_circuit_ackermann_followingcam.launch.py index aecfb39c9..db557b258 100755 --- a/CustomRobots/ackermann_cars/launch/nurburgring_circuit_ackermann_followingcam.launch.py +++ b/CustomRobots/ackermann_cars/launch/nurburgring_circuit_ackermann_followingcam.launch.py @@ -6,74 +6,87 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'nurburgring_line_ackermann_followingcam.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "nurburgring_line_ackermann_followingcam.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/ackermann_cars/launch/simple_circuit_ackermann.launch.py b/CustomRobots/ackermann_cars/launch/simple_circuit_ackermann.launch.py index 485254209..08f09efed 100755 --- a/CustomRobots/ackermann_cars/launch/simple_circuit_ackermann.launch.py +++ b/CustomRobots/ackermann_cars/launch/simple_circuit_ackermann.launch.py @@ -6,74 +6,87 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'simple_circuit_ackermann.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "simple_circuit_ackermann.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/ackermann_cars/launch/simple_circuit_ackermann_followingcam.launch.py b/CustomRobots/ackermann_cars/launch/simple_circuit_ackermann_followingcam.launch.py index 5bd4f78cd..a474aa159 100755 --- a/CustomRobots/ackermann_cars/launch/simple_circuit_ackermann_followingcam.launch.py +++ b/CustomRobots/ackermann_cars/launch/simple_circuit_ackermann_followingcam.launch.py @@ -6,74 +6,87 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'simple_circuit_ackermann_followingcam.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "simple_circuit_ackermann_followingcam.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/ackermann_cars/launch/taxi_holo_1laser_autoparking.launch.py b/CustomRobots/ackermann_cars/launch/taxi_holo_1laser_autoparking.launch.py index e1d35e937..c22eec478 100755 --- a/CustomRobots/ackermann_cars/launch/taxi_holo_1laser_autoparking.launch.py +++ b/CustomRobots/ackermann_cars/launch/taxi_holo_1laser_autoparking.launch.py @@ -6,74 +6,87 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'autoparking_taxi.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "autoparking_taxi.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/ackermann_cars/launch/taxi_holo_3laser_autoparking.launch.py b/CustomRobots/ackermann_cars/launch/taxi_holo_3laser_autoparking.launch.py index 363066734..c5d84e39f 100755 --- a/CustomRobots/ackermann_cars/launch/taxi_holo_3laser_autoparking.launch.py +++ b/CustomRobots/ackermann_cars/launch/taxi_holo_3laser_autoparking.launch.py @@ -6,74 +6,87 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'autoparking_taxi_3laser.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "autoparking_taxi_3laser.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/ackermann_logistic_robot/launch/pallet_warehouse_with_ackermann_logistic_robot.launch.py b/CustomRobots/ackermann_logistic_robot/launch/pallet_warehouse_with_ackermann_logistic_robot.launch.py index dac7c8ea0..2498edb63 100644 --- a/CustomRobots/ackermann_logistic_robot/launch/pallet_warehouse_with_ackermann_logistic_robot.launch.py +++ b/CustomRobots/ackermann_logistic_robot/launch/pallet_warehouse_with_ackermann_logistic_robot.launch.py @@ -6,69 +6,79 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'pallet_warehouse_with_ackermann_logistic_robot.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "pallet_warehouse_with_ackermann_logistic_robot.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/CustomRobots/ackermann_logistic_robot/launch/small_warehouse_with_ackermann_logistic_robot.launch.py b/CustomRobots/ackermann_logistic_robot/launch/small_warehouse_with_ackermann_logistic_robot.launch.py index dab53f46b..9e02a12ed 100644 --- a/CustomRobots/ackermann_logistic_robot/launch/small_warehouse_with_ackermann_logistic_robot.launch.py +++ b/CustomRobots/ackermann_logistic_robot/launch/small_warehouse_with_ackermann_logistic_robot.launch.py @@ -6,69 +6,81 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'no_roof_small_warehouse_with_ackermann_logistic_robot_followingcam.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = ( + "no_roof_small_warehouse_with_ackermann_logistic_robot_followingcam.world" + ) + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/CustomRobots/amazon_hospital/hospital_world/launch/hospital_world.launch.py b/CustomRobots/amazon_hospital/hospital_world/launch/hospital_world.launch.py index 89f3eeb42..b56c97d9e 100755 --- a/CustomRobots/amazon_hospital/hospital_world/launch/hospital_world.launch.py +++ b/CustomRobots/amazon_hospital/hospital_world/launch/hospital_world.launch.py @@ -11,43 +11,46 @@ def generate_launch_description(): # Set the path to the Turtlebot2 ROS package - pkg_share_dir = FindPackageShare(package='custom_robots').find('custom_robots') + pkg_share_dir = FindPackageShare(package="custom_robots").find("custom_robots") world_file_name = "hospital.world" - world = os.path.join(pkg_share_dir, 'worlds', world_file_name) + world = os.path.join(pkg_share_dir, "worlds", world_file_name) - gazebo_models_path = os.path.join(pkg_share_dir, 'models') - gazebo_fuel_models_path = os.path.join(pkg_share_dir, 'fuel_models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_fuel_models_path)}" + gazebo_models_path = os.path.join(pkg_share_dir, "models") + gazebo_fuel_models_path = os.path.join(pkg_share_dir, "fuel_models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_fuel_models_path)}" + ) - gazebo_ros = get_package_share_directory('gazebo_ros') + gazebo_ros = get_package_share_directory("gazebo_ros") gazebo_client = launch.actions.IncludeLaunchDescription( launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(gazebo_ros, 'launch', 'gzclient.launch.py')) + os.path.join(gazebo_ros, "launch", "gzclient.launch.py") + ) ) - + gazebo_server = launch.actions.IncludeLaunchDescription( launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(gazebo_ros, 'launch', 'gzserver.launch.py')) + os.path.join(gazebo_ros, "launch", "gzserver.launch.py") + ) ) - - ld = launch.LaunchDescription([ - launch.actions.DeclareLaunchArgument( - 'world', - default_value=[world, ''], - description='SDF world file'), - launch.actions.DeclareLaunchArgument( - name='gui', - default_value='false' - ), - gazebo_server, - gazebo_client - ]) + ld = launch.LaunchDescription( + [ + launch.actions.DeclareLaunchArgument( + "world", default_value=[world, ""], description="SDF world file" + ), + launch.actions.DeclareLaunchArgument(name="gui", default_value="false"), + gazebo_server, + gazebo_client, + ] + ) return ld -if __name__ == '__main__': - generate_launch_description() \ No newline at end of file +if __name__ == "__main__": + generate_launch_description() diff --git a/CustomRobots/amazon_hospital/launch/follow_person.launch.py b/CustomRobots/amazon_hospital/launch/follow_person.launch.py index 4faeedccb..a41b2fb3b 100644 --- a/CustomRobots/amazon_hospital/launch/follow_person.launch.py +++ b/CustomRobots/amazon_hospital/launch/follow_person.launch.py @@ -7,93 +7,113 @@ from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare + def generate_launch_description(): - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to the Turtlebot2 ROS package - pkg_share_dir = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set Turtlebot2 Arguments - x_turtlebot2_position = '0' - y_turtlebot2_position = '10' - z_turtlebot2_position = '1' - - declare_x_position_cmd = DeclareLaunchArgument( - '-x', default_value=x_turtlebot2_position, - description="Position on the axis x of Turtlebot2" - ) - declare_y_position_cmd = DeclareLaunchArgument( - '-y', default_value=y_turtlebot2_position, - description="Position on the axis y of Turtlebot2" - ) - declare_z_position_cmd = DeclareLaunchArgument( - '-z', default_value=z_turtlebot2_position, - description="Position on the axis z of Turtlebot2" - ) - - world_name = "hospital_follow_person_followingcam.world" - current_dir = os.path.join(pkg_share_dir, 'launch') - world_path = os.path.join(current_dir, world_name) - - # Set the path to the SDF model files - gazebo_models_path = os.path.join(pkg_share_dir, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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_turtlebot2_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(pkg_share_dir, 'launch', 'spawn_model.launch.py')), - launch_arguments = {'-x': x_turtlebot2_position, '-y': y_turtlebot2_position, '-z': z_turtlebot2_position}.items()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - ld.add_action(declare_x_position_cmd) - ld.add_action(declare_y_position_cmd) - ld.add_action(declare_z_position_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - ld.add_action(start_turtlebot2_cmd) - - return ld + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to the Turtlebot2 ROS package + pkg_share_dir = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set Turtlebot2 Arguments + x_turtlebot2_position = "0" + y_turtlebot2_position = "10" + z_turtlebot2_position = "1" + + declare_x_position_cmd = DeclareLaunchArgument( + "-x", + default_value=x_turtlebot2_position, + description="Position on the axis x of Turtlebot2", + ) + declare_y_position_cmd = DeclareLaunchArgument( + "-y", + default_value=y_turtlebot2_position, + description="Position on the axis y of Turtlebot2", + ) + declare_z_position_cmd = DeclareLaunchArgument( + "-z", + default_value=z_turtlebot2_position, + description="Position on the axis z of Turtlebot2", + ) + + world_name = "hospital_follow_person_followingcam.world" + current_dir = os.path.join(pkg_share_dir, "launch") + world_path = os.path.join(current_dir, world_name) + + # Set the path to the SDF model files + gazebo_models_path = os.path.join(pkg_share_dir, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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_turtlebot2_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_share_dir, "launch", "spawn_model.launch.py") + ), + launch_arguments={ + "-x": x_turtlebot2_position, + "-y": y_turtlebot2_position, + "-z": z_turtlebot2_position, + }.items(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + ld.add_action(declare_x_position_cmd) + ld.add_action(declare_y_position_cmd) + ld.add_action(declare_z_position_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_turtlebot2_cmd) + + return ld diff --git a/CustomRobots/amazon_robot/launch/amazon_robot.launch.py b/CustomRobots/amazon_robot/launch/amazon_robot.launch.py index f5be42eed..68bd6fea6 100755 --- a/CustomRobots/amazon_robot/launch/amazon_robot.launch.py +++ b/CustomRobots/amazon_robot/launch/amazon_robot.launch.py @@ -6,102 +6,123 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'amazon_robot.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - # Set robot Arguments - x_robot_position = '0' - y_robot_position = '0' - z_robot_position = '1' - - declare_x_position_cmd = DeclareLaunchArgument( - '-x', default_value=x_robot_position, - description="Position on the axis x of robot" - ) - declare_y_position_cmd = DeclareLaunchArgument( - '-y', default_value=y_robot_position, - description="Position on the axis y of robot" - ) - declare_z_position_cmd = DeclareLaunchArgument( - '-z', default_value=z_robot_position, - description="Position on the axis z of robot" - ) - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - start_robot_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(pkg_share, 'launch', 'spawn_amazon_model.launch.py')), - launch_arguments = {'-x': x_robot_position, '-y': y_robot_position, '-z': z_robot_position}.items() - ) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - #ld.add_action(declare_x_position_cmd) - #ld.add_action(declare_y_position_cmd) - #ld.add_action(declare_z_position_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - #ld.add_action(start_robot_cmd) - ld.add_action(start_gazebo_client_cmd) - - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "amazon_robot.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + # Set robot Arguments + x_robot_position = "0" + y_robot_position = "0" + z_robot_position = "1" + + declare_x_position_cmd = DeclareLaunchArgument( + "-x", + default_value=x_robot_position, + description="Position on the axis x of robot", + ) + declare_y_position_cmd = DeclareLaunchArgument( + "-y", + default_value=y_robot_position, + description="Position on the axis y of robot", + ) + declare_z_position_cmd = DeclareLaunchArgument( + "-z", + default_value=z_robot_position, + description="Position on the axis z of robot", + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + start_robot_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_share, "launch", "spawn_amazon_model.launch.py") + ), + launch_arguments={ + "-x": x_robot_position, + "-y": y_robot_position, + "-z": z_robot_position, + }.items(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + # ld.add_action(declare_x_position_cmd) + # ld.add_action(declare_y_position_cmd) + # ld.add_action(declare_z_position_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + # ld.add_action(start_robot_cmd) + ld.add_action(start_gazebo_client_cmd) + + return ld diff --git a/CustomRobots/amazon_robot/launch/amazon_warehouse.launch.py b/CustomRobots/amazon_robot/launch/amazon_warehouse.launch.py index 9e7956d68..cf905acf9 100644 --- a/CustomRobots/amazon_robot/launch/amazon_warehouse.launch.py +++ b/CustomRobots/amazon_robot/launch/amazon_warehouse.launch.py @@ -6,75 +6,87 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'no_roof_small_warehouse_with_robot.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "no_roof_small_warehouse_with_robot.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/amazon_robot/launch/pallet_warehouse.launch.py b/CustomRobots/amazon_robot/launch/pallet_warehouse.launch.py index 6dc31ebc2..b90b00072 100644 --- a/CustomRobots/amazon_robot/launch/pallet_warehouse.launch.py +++ b/CustomRobots/amazon_robot/launch/pallet_warehouse.launch.py @@ -6,75 +6,87 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'pallet_warehouse_with_robot.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "pallet_warehouse_with_robot.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/amazon_robot/launch/robot_amazon_description.launch.py b/CustomRobots/amazon_robot/launch/robot_amazon_description.launch.py index 2a2f4df9e..39a2838f0 100644 --- a/CustomRobots/amazon_robot/launch/robot_amazon_description.launch.py +++ b/CustomRobots/amazon_robot/launch/robot_amazon_description.launch.py @@ -42,40 +42,38 @@ def generate_launch_description(): urdf_path = PathJoinSubstitution( - [FindPackageShare('custom_robots'), 'urdf', 'amazon_warehouse_robot.urdf.xacro'] + [FindPackageShare("custom_robots"), "urdf", "amazon_warehouse_robot.urdf.xacro"] ) - return LaunchDescription([ - - DeclareLaunchArgument( - name='urdf', - default_value=urdf_path, - description='URDF path' - ), - - DeclareLaunchArgument( - name='use_sim_time', - default_value='false', - description='Use simulation time' - ), - - Node( - package='robot_state_publisher', - executable='robot_state_publisher', - name='robot_state_publisher', - output='screen', - parameters=[{ - 'use_sim_time': LaunchConfiguration('use_sim_time'), - 'robot_description': Command(['xacro ', LaunchConfiguration('urdf')]) - }] - ), - - Node( - package='joint_state_publisher', - executable='joint_state_publisher', - name='joint_state_publisher', - parameters=[{ - 'use_sim_time': LaunchConfiguration('use_sim_time') - }] - ), - ]) + return LaunchDescription( + [ + DeclareLaunchArgument( + name="urdf", default_value=urdf_path, description="URDF path" + ), + DeclareLaunchArgument( + name="use_sim_time", + default_value="false", + description="Use simulation time", + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + parameters=[ + { + "use_sim_time": LaunchConfiguration("use_sim_time"), + "robot_description": Command( + ["xacro ", LaunchConfiguration("urdf")] + ), + } + ], + ), + Node( + package="joint_state_publisher", + executable="joint_state_publisher", + name="joint_state_publisher", + parameters=[{"use_sim_time": LaunchConfiguration("use_sim_time")}], + ), + ] + ) diff --git a/CustomRobots/amazon_robot/launch/spawn_amazon_model.launch.py b/CustomRobots/amazon_robot/launch/spawn_amazon_model.launch.py index a6bd77f0f..6531cbaf6 100755 --- a/CustomRobots/amazon_robot/launch/spawn_amazon_model.launch.py +++ b/CustomRobots/amazon_robot/launch/spawn_amazon_model.launch.py @@ -6,50 +6,64 @@ from launch.substitutions import Command, LaunchConfiguration from launch_ros.actions import Node + def generate_launch_description(): - - # Set (x, y, z) default position of turtlebot2 - x_pos = LaunchConfiguration('-x', default='0') - y_pos = LaunchConfiguration('-y', default='0') - z_pos = LaunchConfiguration('-z', default='0') - - # Find Turtlebot2 package - pkg_share = launch_ros.substitutions.FindPackageShare(package='custom_robots').find('custom_robots') - - # Find Urdf File - urdf_file = os.path.join(pkg_share, 'urdf/amazon_robot.urdf') - - # Open Urdf file - with open(urdf_file, 'r') as info: - robot_desc = info.read() - - # Robot description Turtlebot2 - turtlebot2_model = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - parameters=[{'robot_description': robot_desc}], - arguments=[urdf_file] - ) - - # TF Tree - joint_state_publisher_node = Node( - package='joint_state_publisher', - executable='joint_state_publisher', - name='joint_state_publisher' - ) - - # Spawn Turtlebot2 - spawn_entity_node = Node( - package='gazebo_ros', - executable='spawn_entity.py', - name='entity_spawner', - output='screen', - arguments=["-topic", "/robot_description", "-entity", "amazon_robot", "-x", x_pos, "-y", y_pos, "-z", z_pos] - ) - - ld = LaunchDescription() - ld.add_action(turtlebot2_model) - ld.add_action(joint_state_publisher_node) - ld.add_action(spawn_entity_node) - - return ld \ No newline at end of file + + # Set (x, y, z) default position of turtlebot2 + x_pos = LaunchConfiguration("-x", default="0") + y_pos = LaunchConfiguration("-y", default="0") + z_pos = LaunchConfiguration("-z", default="0") + + # Find Turtlebot2 package + pkg_share = launch_ros.substitutions.FindPackageShare(package="custom_robots").find( + "custom_robots" + ) + + # Find Urdf File + urdf_file = os.path.join(pkg_share, "urdf/amazon_robot.urdf") + + # Open Urdf file + with open(urdf_file, "r") as info: + robot_desc = info.read() + + # Robot description Turtlebot2 + turtlebot2_model = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + parameters=[{"robot_description": robot_desc}], + arguments=[urdf_file], + ) + + # TF Tree + joint_state_publisher_node = Node( + package="joint_state_publisher", + executable="joint_state_publisher", + name="joint_state_publisher", + ) + + # Spawn Turtlebot2 + spawn_entity_node = Node( + package="gazebo_ros", + executable="spawn_entity.py", + name="entity_spawner", + output="screen", + arguments=[ + "-topic", + "/robot_description", + "-entity", + "amazon_robot", + "-x", + x_pos, + "-y", + y_pos, + "-z", + z_pos, + ], + ) + + ld = LaunchDescription() + ld.add_action(turtlebot2_model) + ld.add_action(joint_state_publisher_node) + ld.add_action(spawn_entity_node) + + return ld diff --git a/CustomRobots/amazon_robot/platform_controller/platform_controller/move_platform.py b/CustomRobots/amazon_robot/platform_controller/platform_controller/move_platform.py index 0890f0058..e79028aa0 100644 --- a/CustomRobots/amazon_robot/platform_controller/platform_controller/move_platform.py +++ b/CustomRobots/amazon_robot/platform_controller/platform_controller/move_platform.py @@ -9,43 +9,45 @@ DEFAULT = 2 + class PlatformController(Node): def __init__(self, joint): - ''' + """ PlatformController Constructor. @param joint: Joint name to move @type joint: String - ''' + """ super().__init__("platform_controller") self.lock = threading.Lock() # Joint Effort Service - self.client = self.create_client(ApplyJointEffort, '/apply_joint_effort') + self.client = self.create_client(ApplyJointEffort, "/apply_joint_effort") while not self.client.wait_for_service(timeout_sec=1.0): - self.get_logger().info('service not available, waiting again...') + self.get_logger().info("service not available, waiting again...") self.req = ApplyJointEffort.Request() self.req.joint_name = joint - + self.future = None self.future_lock = threading.Lock() - - def send_effort(self,input_effort): + + def send_effort(self, input_effort): self.req.effort = input_effort self.req.start_time.sec = 0 self.req.start_time.nanosec = 0 self.req.duration.sec = 2000 self.req.duration.nanosec = 0 - + future = self.client.call_async(self.req) rclpy.spin_until_future_complete(self, future) self.future_lock.acquire() self.future = future - #print("self.future: ",self.future.result().success, self.future.result().status_message) + # print("self.future: ",self.future.result().success, self.future.result().status_message) self.future_lock.release() + class PlatformCommandListener(Node): def __init__(self): @@ -55,52 +57,52 @@ def __init__(self): self.controller = PlatformController("lift_joint") self.start() - def start (self): - ''' + def start(self): + """ Starts (Subscribes) to the command topic. - ''' - self.sub = self.create_subscription(String,'/send_effort',self.__callback,10) + """ + self.sub = self.create_subscription(String, "/send_effort", self.__callback, 10) def stop(self): - ''' + """ Stops (Unregisters) Node. - ''' + """ self.sub.unregister() - - def __callback (self, event): + + def __callback(self, event): """ Load/Unload logic so that effort is not accumulated in joint """ command = event.data - if (command == "load"): - if (self.applied_effort == 0): + if command == "load": + if self.applied_effort == 0: effort = DEFAULT - elif (self.applied_effort < 0): - effort = self.applied_effort*-2 + elif self.applied_effort < 0: + effort = self.applied_effort * -2 else: return - elif (command == "unload"): - if (self.applied_effort == DEFAULT): + elif command == "unload": + if self.applied_effort == DEFAULT: effort = -DEFAULT - elif (self.applied_effort > 0): - effort = self.applied_effort*-2 + elif self.applied_effort > 0: + effort = self.applied_effort * -2 else: return else: return - + self.controller.send_effort(float(effort)) self.applied_effort = self.applied_effort + effort - - + + def main(args=None): rclpy.init(args=args) - + command_listener = PlatformCommandListener() rclpy.spin(command_listener) command_listener.destroy_node() rclpy.shutdown() -if __name__ == '__main__': - main() \ No newline at end of file +if __name__ == "__main__": + main() diff --git a/CustomRobots/amazon_robot/platform_controller/setup.py b/CustomRobots/amazon_robot/platform_controller/setup.py index 7c516d6ba..83e6b2e57 100644 --- a/CustomRobots/amazon_robot/platform_controller/setup.py +++ b/CustomRobots/amazon_robot/platform_controller/setup.py @@ -1,26 +1,25 @@ from setuptools import setup -package_name = 'platform_controller' +package_name = "platform_controller" setup( name=package_name, - version='0.0.0', + version="0.0.0", packages=[package_name], data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), ], - install_requires=['setuptools'], + install_requires=["setuptools"], zip_safe=True, - maintainer='blanca', - maintainer_email='blancasoriaru@gmail.com', - description='TODO: Package description', - license='TODO: License declaration', - tests_require=['pytest'], + maintainer="blanca", + maintainer_email="blancasoriaru@gmail.com", + description="TODO: Package description", + license="TODO: License declaration", + tests_require=["pytest"], entry_points={ - 'console_scripts': [ - 'move_platform = platform_controller.move_platform:main', + "console_scripts": [ + "move_platform = platform_controller.move_platform:main", ], }, ) diff --git a/CustomRobots/amazon_robot/platform_controller/test/test_copyright.py b/CustomRobots/amazon_robot/platform_controller/test/test_copyright.py index 97a39196e..ceffe896d 100644 --- a/CustomRobots/amazon_robot/platform_controller/test/test_copyright.py +++ b/CustomRobots/amazon_robot/platform_controller/test/test_copyright.py @@ -17,9 +17,11 @@ # Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.skip( + reason="No copyright header has been placed in the generated source file." +) @pytest.mark.copyright @pytest.mark.linter def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/CustomRobots/amazon_robot/platform_controller/test/test_flake8.py b/CustomRobots/amazon_robot/platform_controller/test/test_flake8.py index 27ee1078f..ee79f31ac 100644 --- a/CustomRobots/amazon_robot/platform_controller/test/test_flake8.py +++ b/CustomRobots/amazon_robot/platform_controller/test/test_flake8.py @@ -20,6 +20,6 @@ @pytest.mark.linter def test_flake8(): rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) + assert rc == 0, "Found %d code style errors / warnings:\n" % len( + errors + ) + "\n".join(errors) diff --git a/CustomRobots/amazon_robot/platform_controller/test/test_pep257.py b/CustomRobots/amazon_robot/platform_controller/test/test_pep257.py index b234a3840..a2c3deb8e 100644 --- a/CustomRobots/amazon_robot/platform_controller/test/test_pep257.py +++ b/CustomRobots/amazon_robot/platform_controller/test/test_pep257.py @@ -19,5 +19,5 @@ @pytest.mark.linter @pytest.mark.pep257 def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings" diff --git a/CustomRobots/autoparking/launch/gas_station_battery.launch.py b/CustomRobots/autoparking/launch/gas_station_battery.launch.py index 6d0747833..d46222c9b 100755 --- a/CustomRobots/autoparking/launch/gas_station_battery.launch.py +++ b/CustomRobots/autoparking/launch/gas_station_battery.launch.py @@ -6,74 +6,87 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'gas_station_battery.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "gas_station_battery.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/autoparking/launch/gas_station_battery_ackermann.launch.py b/CustomRobots/autoparking/launch/gas_station_battery_ackermann.launch.py index 4425087ab..121863210 100755 --- a/CustomRobots/autoparking/launch/gas_station_battery_ackermann.launch.py +++ b/CustomRobots/autoparking/launch/gas_station_battery_ackermann.launch.py @@ -6,74 +6,87 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'gas_station_battery_ackermann.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "gas_station_battery_ackermann.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/autoparking/launch/gas_station_line.launch.py b/CustomRobots/autoparking/launch/gas_station_line.launch.py index 1ae5177fc..d8b7a92b4 100755 --- a/CustomRobots/autoparking/launch/gas_station_line.launch.py +++ b/CustomRobots/autoparking/launch/gas_station_line.launch.py @@ -6,74 +6,87 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'gas_station_line.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "gas_station_line.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/autoparking/launch/gas_station_line_ackermann.launch.py b/CustomRobots/autoparking/launch/gas_station_line_ackermann.launch.py index 065d066bd..a2ae40bb0 100755 --- a/CustomRobots/autoparking/launch/gas_station_line_ackermann.launch.py +++ b/CustomRobots/autoparking/launch/gas_station_line_ackermann.launch.py @@ -6,74 +6,87 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'gas_station_line_ackermann.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "gas_station_line_ackermann.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/autoparking/launch/gas_station_parking.launch.py b/CustomRobots/autoparking/launch/gas_station_parking.launch.py index 9dd2f6d09..e50d15089 100755 --- a/CustomRobots/autoparking/launch/gas_station_parking.launch.py +++ b/CustomRobots/autoparking/launch/gas_station_parking.launch.py @@ -6,74 +6,87 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'gas_station_parking.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "gas_station_parking.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/autoparking/launch/gas_station_parking_ackermann.launch.py b/CustomRobots/autoparking/launch/gas_station_parking_ackermann.launch.py index 12c9390db..2092b3383 100755 --- a/CustomRobots/autoparking/launch/gas_station_parking_ackermann.launch.py +++ b/CustomRobots/autoparking/launch/gas_station_parking_ackermann.launch.py @@ -6,74 +6,87 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'gas_station_parking_ackermann.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "gas_station_parking_ackermann.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/autoparking/launch/lincoln_autoparking.launch.py b/CustomRobots/autoparking/launch/lincoln_autoparking.launch.py index 519b3616a..4be40d7cf 100755 --- a/CustomRobots/autoparking/launch/lincoln_autoparking.launch.py +++ b/CustomRobots/autoparking/launch/lincoln_autoparking.launch.py @@ -6,74 +6,87 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'autoparking_lincoln_1laser.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "autoparking_lincoln_1laser.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/autoparking/launch/lincoln_bateria.launch.py b/CustomRobots/autoparking/launch/lincoln_bateria.launch.py index 65ed525c8..7cddcfcc7 100755 --- a/CustomRobots/autoparking/launch/lincoln_bateria.launch.py +++ b/CustomRobots/autoparking/launch/lincoln_bateria.launch.py @@ -6,74 +6,87 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'bateria_lincoln_1laser.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "bateria_lincoln_1laser.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/autoparking/launch/prius_autoparking.launch.py b/CustomRobots/autoparking/launch/prius_autoparking.launch.py index 218ef5d3f..2c20bc232 100755 --- a/CustomRobots/autoparking/launch/prius_autoparking.launch.py +++ b/CustomRobots/autoparking/launch/prius_autoparking.launch.py @@ -6,74 +6,87 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'autoparking_prius_3laser.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "autoparking_prius_3laser.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/autoparking/launch/prius_bateria.launch.py b/CustomRobots/autoparking/launch/prius_bateria.launch.py index 46ba17aef..38f953fbe 100755 --- a/CustomRobots/autoparking/launch/prius_bateria.launch.py +++ b/CustomRobots/autoparking/launch/prius_bateria.launch.py @@ -6,74 +6,87 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'bateria_prius_3laser.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "bateria_prius_3laser.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/f1/launch/montmelo_circuit.launch.py b/CustomRobots/f1/launch/montmelo_circuit.launch.py index 2683b80f2..fa0cb11c6 100755 --- a/CustomRobots/f1/launch/montmelo_circuit.launch.py +++ b/CustomRobots/f1/launch/montmelo_circuit.launch.py @@ -6,74 +6,87 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'montmelo_line.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "montmelo_line.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/f1/launch/montmelo_circuit_followingcam.launch.py b/CustomRobots/f1/launch/montmelo_circuit_followingcam.launch.py index 6088da7b5..ca749b91b 100755 --- a/CustomRobots/f1/launch/montmelo_circuit_followingcam.launch.py +++ b/CustomRobots/f1/launch/montmelo_circuit_followingcam.launch.py @@ -6,74 +6,87 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'montmelo_line_followingcam.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "montmelo_line_followingcam.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/f1/launch/montreal_circuit.launch.py b/CustomRobots/f1/launch/montreal_circuit.launch.py index c49cc2abc..43c347aa7 100755 --- a/CustomRobots/f1/launch/montreal_circuit.launch.py +++ b/CustomRobots/f1/launch/montreal_circuit.launch.py @@ -6,74 +6,87 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'montreal_line.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "montreal_line.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/f1/launch/montreal_circuit_followingcam.launch.py b/CustomRobots/f1/launch/montreal_circuit_followingcam.launch.py index 5b0883521..4613a06df 100755 --- a/CustomRobots/f1/launch/montreal_circuit_followingcam.launch.py +++ b/CustomRobots/f1/launch/montreal_circuit_followingcam.launch.py @@ -6,74 +6,87 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'montreal_line_followingcam.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "montreal_line_followingcam.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/f1/launch/nurburgring_circuit.launch.py b/CustomRobots/f1/launch/nurburgring_circuit.launch.py index 98b983a7c..5c299b3ea 100755 --- a/CustomRobots/f1/launch/nurburgring_circuit.launch.py +++ b/CustomRobots/f1/launch/nurburgring_circuit.launch.py @@ -6,74 +6,87 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'nurburgring_line.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "nurburgring_line.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/f1/launch/nurburgring_circuit_followingcam.launch.py b/CustomRobots/f1/launch/nurburgring_circuit_followingcam.launch.py index 5fbe51752..34d38d304 100755 --- a/CustomRobots/f1/launch/nurburgring_circuit_followingcam.launch.py +++ b/CustomRobots/f1/launch/nurburgring_circuit_followingcam.launch.py @@ -6,74 +6,87 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'nurburgring_line_followingcam.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "nurburgring_line_followingcam.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/f1/launch/simple_circuit.launch.py b/CustomRobots/f1/launch/simple_circuit.launch.py index 25c43ca2d..7aa874fc1 100755 --- a/CustomRobots/f1/launch/simple_circuit.launch.py +++ b/CustomRobots/f1/launch/simple_circuit.launch.py @@ -6,74 +6,87 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'simple_circuit.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "simple_circuit.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/f1/launch/simple_circuit_followingcam.launch.py b/CustomRobots/f1/launch/simple_circuit_followingcam.launch.py index fd8a9ff2b..4c5b1baa0 100755 --- a/CustomRobots/f1/launch/simple_circuit_followingcam.launch.py +++ b/CustomRobots/f1/launch/simple_circuit_followingcam.launch.py @@ -6,74 +6,87 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'simple_circuit_followingcam.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "simple_circuit_followingcam.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/pick_place/launch/ur_robotiq_sim_warehouse.launch.py b/CustomRobots/pick_place/launch/ur_robotiq_sim_warehouse.launch.py index 556c06587..bbbb4b55a 100644 --- a/CustomRobots/pick_place/launch/ur_robotiq_sim_warehouse.launch.py +++ b/CustomRobots/pick_place/launch/ur_robotiq_sim_warehouse.launch.py @@ -42,7 +42,12 @@ from launch.conditions import IfCondition, UnlessCondition from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare @@ -103,7 +108,7 @@ def launch_setup(context, *args, **kwargs): "sim_gazebo:=true", " ", "simulation_controllers:=", - initial_joint_controllers, + initial_joint_controllers, ] ) robot_description = {"robot_description": robot_description_content} @@ -127,7 +132,11 @@ def launch_setup(context, *args, **kwargs): joint_state_broadcaster_spawner = Node( package="controller_manager", executable="spawner", - arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + arguments=[ + "joint_state_broadcaster", + "--controller-manager", + "/controller_manager", + ], ) # Delay rviz start after `joint_state_broadcaster` @@ -152,21 +161,18 @@ def launch_setup(context, *args, **kwargs): arguments=[initial_joint_controller, "-c", "/controller_manager", "--stopped"], condition=UnlessCondition(start_joint_controller), ) - + # Gazebo WORLD file: ur5_ros2_warehouse = os.path.join( - get_package_share_directory('pick_place_exercise'), - 'worlds', - 'warehouse.world') - + get_package_share_directory("pick_place_exercise"), "worlds", "warehouse.world" + ) + # Gazebo nodes gazebo = IncludeLaunchDescription( PythonLaunchDescriptionSource( [FindPackageShare("gazebo_ros"), "/launch", "/gazebo.launch.py"] ), - launch_arguments={ - "gui": gazebo_gui,"world": ur5_ros2_warehouse - }.items(), + launch_arguments={"gui": gazebo_gui, "world": ur5_ros2_warehouse}.items(), ) # Spawn robot @@ -177,14 +183,15 @@ def launch_setup(context, *args, **kwargs): arguments=["-entity", "ur_gripper", "-topic", "robot_description", "-z", "0.9"], output="screen", ) - + # Depending on gui parameter, either launch joint_state_publisher or joint_state_publisher_gui joint_state_publisher_gui = Node( - package='joint_state_publisher_gui', - executable='joint_state_publisher_gui', - name='joint_state_publisher_gui', - condition=IfCondition(launch_jointstate_gui)) - + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + name="joint_state_publisher_gui", + condition=IfCondition(launch_jointstate_gui), + ) + nodes_to_start = [ robot_state_publisher_node, joint_state_broadcaster_spawner, @@ -200,14 +207,24 @@ def launch_setup(context, *args, **kwargs): def generate_launch_description(): - + declared_arguments = [] # UR specific arguments declared_arguments.append( DeclareLaunchArgument( "ur_type", description="Type/series of used UR robot.", - choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e", "ur20", "ur30"], + choices=[ + "ur3", + "ur3e", + "ur5", + "ur5e", + "ur10", + "ur10e", + "ur16e", + "ur20", + "ur30", + ], default_value="ur5e", ) ) @@ -287,10 +304,16 @@ def generate_launch_description(): ) ) declared_arguments.append( - DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?") + DeclareLaunchArgument( + "launch_rviz", default_value="true", description="Launch RViz?" + ) ) declared_arguments.append( - DeclareLaunchArgument("launch_jointstate_gui", default_value="false", description="Launch Joint State GUI?") + DeclareLaunchArgument( + "launch_jointstate_gui", + default_value="false", + description="Launch Joint State GUI?", + ) ) declared_arguments.append( DeclareLaunchArgument( @@ -298,4 +321,6 @@ def generate_launch_description(): ) ) - return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) + return LaunchDescription( + declared_arguments + [OpaqueFunction(function=launch_setup)] + ) diff --git a/CustomRobots/pick_place/launch/ur_sim_control.launch.py b/CustomRobots/pick_place/launch/ur_sim_control.launch.py index ea1dd3def..06dc6a716 100644 --- a/CustomRobots/pick_place/launch/ur_sim_control.launch.py +++ b/CustomRobots/pick_place/launch/ur_sim_control.launch.py @@ -38,7 +38,12 @@ from launch.conditions import IfCondition, UnlessCondition from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare @@ -122,7 +127,11 @@ def launch_setup(context, *args, **kwargs): joint_state_broadcaster_spawner = Node( package="controller_manager", executable="spawner", - arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + arguments=[ + "joint_state_broadcaster", + "--controller-manager", + "/controller_manager", + ], ) # Delay rviz start after `joint_state_broadcaster` @@ -187,7 +196,17 @@ def generate_launch_description(): DeclareLaunchArgument( "ur_type", description="Type/series of used UR robot.", - choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e", "ur20", "ur30"], + choices=[ + "ur3", + "ur3e", + "ur5", + "ur5e", + "ur10", + "ur10e", + "ur16e", + "ur20", + "ur30", + ], default_value="ur5e", ) ) @@ -267,7 +286,9 @@ def generate_launch_description(): ) ) declared_arguments.append( - DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?") + DeclareLaunchArgument( + "launch_rviz", default_value="true", description="Launch RViz?" + ) ) declared_arguments.append( DeclareLaunchArgument( @@ -275,4 +296,6 @@ def generate_launch_description(): ) ) - return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) + return LaunchDescription( + declared_arguments + [OpaqueFunction(function=launch_setup)] + ) diff --git a/CustomRobots/pick_place/launch/ur_sim_moveit.launch.py b/CustomRobots/pick_place/launch/ur_sim_moveit.launch.py index edae8f032..8b022a412 100644 --- a/CustomRobots/pick_place/launch/ur_sim_moveit.launch.py +++ b/CustomRobots/pick_place/launch/ur_sim_moveit.launch.py @@ -29,7 +29,11 @@ # Author: Denis Stogl from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + OpaqueFunction, +) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.substitutions import FindPackageShare @@ -51,7 +55,11 @@ def launch_setup(context, *args, **kwargs): ur_control_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( - [FindPackageShare("ur_simulation_gazebo"), "/launch", "/ur_sim_control.launch.py"] + [ + FindPackageShare("ur_simulation_gazebo"), + "/launch", + "/ur_sim_control.launch.py", + ] ), launch_arguments={ "ur_type": ur_type, @@ -98,7 +106,17 @@ def generate_launch_description(): DeclareLaunchArgument( "ur_type", description="Type/series of used UR robot.", - choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e", "ur20", "ur30"], + choices=[ + "ur3", + "ur3e", + "ur5", + "ur5e", + "ur10", + "ur10e", + "ur16e", + "ur20", + "ur30", + ], default_value="ur5e", ) ) @@ -165,4 +183,6 @@ def generate_launch_description(): ) ) - return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) + return LaunchDescription( + declared_arguments + [OpaqueFunction(function=launch_setup)] + ) diff --git a/CustomRobots/pick_place/launch/ur_sim_moveit_warehouse.launch.py b/CustomRobots/pick_place/launch/ur_sim_moveit_warehouse.launch.py index 67a0f15fe..226e1664a 100644 --- a/CustomRobots/pick_place/launch/ur_sim_moveit_warehouse.launch.py +++ b/CustomRobots/pick_place/launch/ur_sim_moveit_warehouse.launch.py @@ -29,7 +29,11 @@ # Author: Denis Stogl from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + OpaqueFunction, +) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.substitutions import FindPackageShare @@ -53,7 +57,11 @@ def launch_setup(context, *args, **kwargs): ur_control_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( - [FindPackageShare("pick_place_exercise"), "/launch", "/ur_sim_warehouse.launch.py"] + [ + FindPackageShare("pick_place_exercise"), + "/launch", + "/ur_sim_warehouse.launch.py", + ] ), launch_arguments={ "ur_type": ur_type, @@ -100,7 +108,17 @@ def generate_launch_description(): DeclareLaunchArgument( "ur_type", description="Type/series of used UR robot.", - choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e", "ur20", "ur30"], + choices=[ + "ur3", + "ur3e", + "ur5", + "ur5e", + "ur10", + "ur10e", + "ur16e", + "ur20", + "ur30", + ], default_value="ur5e", ) ) @@ -167,4 +185,6 @@ def generate_launch_description(): ) ) - return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) + return LaunchDescription( + declared_arguments + [OpaqueFunction(function=launch_setup)] + ) diff --git a/CustomRobots/pick_place/launch/ur_sim_warehouse.launch.py b/CustomRobots/pick_place/launch/ur_sim_warehouse.launch.py index ab33f63f0..ff222826a 100644 --- a/CustomRobots/pick_place/launch/ur_sim_warehouse.launch.py +++ b/CustomRobots/pick_place/launch/ur_sim_warehouse.launch.py @@ -42,7 +42,12 @@ from launch.conditions import IfCondition, UnlessCondition from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare @@ -103,7 +108,7 @@ def launch_setup(context, *args, **kwargs): "sim_gazebo:=true", " ", "simulation_controllers:=", - initial_joint_controllers, + initial_joint_controllers, ] ) robot_description = {"robot_description": robot_description_content} @@ -127,7 +132,11 @@ def launch_setup(context, *args, **kwargs): joint_state_broadcaster_spawner = Node( package="controller_manager", executable="spawner", - arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + arguments=[ + "joint_state_broadcaster", + "--controller-manager", + "/controller_manager", + ], ) # Delay rviz start after `joint_state_broadcaster` @@ -152,21 +161,18 @@ def launch_setup(context, *args, **kwargs): arguments=[initial_joint_controller, "-c", "/controller_manager", "--stopped"], condition=UnlessCondition(start_joint_controller), ) - + # Gazebo WORLD file: ur5_ros2_warehouse = os.path.join( - get_package_share_directory('pick_place_exercise'), - 'worlds', - 'warehouse.world') - + get_package_share_directory("pick_place_exercise"), "worlds", "warehouse.world" + ) + # Gazebo nodes gazebo = IncludeLaunchDescription( PythonLaunchDescriptionSource( [FindPackageShare("gazebo_ros"), "/launch", "/gazebo.launch.py"] ), - launch_arguments={ - "gui": gazebo_gui,"world": ur5_ros2_warehouse - }.items(), + launch_arguments={"gui": gazebo_gui, "world": ur5_ros2_warehouse}.items(), ) # Spawn robot @@ -177,14 +183,15 @@ def launch_setup(context, *args, **kwargs): arguments=["-entity", "ur", "-topic", "robot_description", "-z", "0.9"], output="screen", ) - + # Depending on gui parameter, either launch joint_state_publisher or joint_state_publisher_gui joint_state_publisher_gui = Node( - package='joint_state_publisher_gui', - executable='joint_state_publisher_gui', - name='joint_state_publisher_gui', - condition=IfCondition(launch_jointstate_gui)) - + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + name="joint_state_publisher_gui", + condition=IfCondition(launch_jointstate_gui), + ) + nodes_to_start = [ robot_state_publisher_node, joint_state_broadcaster_spawner, @@ -200,14 +207,24 @@ def launch_setup(context, *args, **kwargs): def generate_launch_description(): - + declared_arguments = [] # UR specific arguments declared_arguments.append( DeclareLaunchArgument( "ur_type", description="Type/series of used UR robot.", - choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e", "ur20", "ur30"], + choices=[ + "ur3", + "ur3e", + "ur5", + "ur5e", + "ur10", + "ur10e", + "ur16e", + "ur20", + "ur30", + ], default_value="ur5e", ) ) @@ -287,10 +304,16 @@ def generate_launch_description(): ) ) declared_arguments.append( - DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?") + DeclareLaunchArgument( + "launch_rviz", default_value="true", description="Launch RViz?" + ) ) declared_arguments.append( - DeclareLaunchArgument("launch_jointstate_gui", default_value="false", description="Launch Joint State GUI?") + DeclareLaunchArgument( + "launch_jointstate_gui", + default_value="false", + description="Launch Joint State GUI?", + ) ) declared_arguments.append( DeclareLaunchArgument( @@ -298,4 +321,6 @@ def generate_launch_description(): ) ) - return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) + return LaunchDescription( + declared_arguments + [OpaqueFunction(function=launch_setup)] + ) diff --git a/CustomRobots/pick_place/pick_place_exercise/FK.py b/CustomRobots/pick_place/pick_place_exercise/FK.py index b4a0b8e17..99ef515bb 100644 --- a/CustomRobots/pick_place/pick_place_exercise/FK.py +++ b/CustomRobots/pick_place/pick_place_exercise/FK.py @@ -12,31 +12,34 @@ # Inicializacion, objetos robot y escena, grupos MoveIt moveit_commander.roscpp_initialize(sys.argv) -rospy.init_node('moving_irb120_robot', anonymous=True) +rospy.init_node("moving_irb120_robot", anonymous=True) robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() arm_group = moveit_commander.MoveGroupCommander("irb_120") hand_group = moveit_commander.MoveGroupCommander("robotiq_85") -display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', - moveit_msgs.msg.DisplayTrajectory, - queue_size=20) +display_trajectory_publisher = rospy.Publisher( + "/move_group/display_planned_path", moveit_msgs.msg.DisplayTrajectory, queue_size=20 +) # Forward Kinematics (FK): movimiento del grupo robot eje a eje (para robots de 6 ejes). Angulos adaptados a grados + def move_joint_arm(joint_0, joint_1, joint_2, joint_3, joint_4, joint_5): joint_goal = arm_group.get_current_joint_values() - joint_goal[0] = joint_0 * (pi/180) - joint_goal[1] = joint_1 * (pi/180) - joint_goal[2] = joint_2 * (pi/180) - joint_goal[3] = joint_3 * (pi/180) - joint_goal[4] = joint_4 * (pi/180) - joint_goal[5] = joint_5 * (pi/180) + joint_goal[0] = joint_0 * (pi / 180) + joint_goal[1] = joint_1 * (pi / 180) + joint_goal[2] = joint_2 * (pi / 180) + joint_goal[3] = joint_3 * (pi / 180) + joint_goal[4] = joint_4 * (pi / 180) + joint_goal[5] = joint_5 * (pi / 180) arm_group.go(joint_goal, wait=True) arm_group.stop() # To guarantee no residual movement + # IDEM, para el grupo de la herramienta (un unico eje) + def move_joint_hand(gripper_finger1_joint): joint_goal = hand_group.get_current_joint_values() joint_goal[2] = gripper_finger1_joint # Gripper master axis @@ -45,14 +48,14 @@ def move_joint_hand(gripper_finger1_joint): hand_group.stop() # To guarantee no residual movement -if __name__ == '__main__': +if __name__ == "__main__": # Print estado actual del robot (opcional) print("============ Printing robot state ============") print(robot.get_current_state()) print("") - # Ejemplo: FK en un loop. Movimiento del robot y de la herramienta eje a eje. Angulos en grados (robot). + # Ejemplo: FK en un loop. Movimiento del robot y de la herramienta eje a eje. Angulos en grados (robot). for i in range(2): rospy.loginfo("Moving arm to pick pose") move_joint_arm(0, 50, -20, 0, 60, 0) diff --git a/CustomRobots/pick_place/pick_place_exercise/IK_moveJ.py b/CustomRobots/pick_place/pick_place_exercise/IK_moveJ.py index 70b8b7e0c..1250b2563 100644 --- a/CustomRobots/pick_place/pick_place_exercise/IK_moveJ.py +++ b/CustomRobots/pick_place/pick_place_exercise/IK_moveJ.py @@ -12,24 +12,26 @@ # Inicializacion, objetos robot y escena, grupos MoveIt moveit_commander.roscpp_initialize(sys.argv) -rospy.init_node('moving_irb120_robot', anonymous=True) +rospy.init_node("moving_irb120_robot", anonymous=True) robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() arm_group = moveit_commander.MoveGroupCommander("irb_120") hand_group = moveit_commander.MoveGroupCommander("robotiq_85") # Inicializacion, objetos robot y escena, grupos MoveIt -display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', - moveit_msgs.msg.DisplayTrajectory, - queue_size=20) - +display_trajectory_publisher = rospy.Publisher( + "/move_group/display_planned_path", moveit_msgs.msg.DisplayTrajectory, queue_size=20 +) # Inverse Kinematics (IK): mover TCP (brida) a una orientacion dada (Euler) y posicion x, y, z dada. Los angulos se convierten a cuaternios (mensaje de pose) + def move_pose_arm(roll, pitch, yaw, x, y, z): pose_goal = geometry_msgs.msg.Pose() - quat = quaternion_from_euler(roll*(pi/180), pitch*(pi/180), yaw*(pi/180)) + quat = quaternion_from_euler( + roll * (pi / 180), pitch * (pi / 180), yaw * (pi / 180) + ) pose_goal.orientation.x = quat[0] pose_goal.orientation.y = quat[1] pose_goal.orientation.z = quat[2] @@ -44,8 +46,10 @@ def move_pose_arm(roll, pitch, yaw, x, y, z): arm_group.stop() # To guarantee no residual movement arm_group.clear_pose_targets() + # Forward Kinematics (FK) para la herramienta + def move_joint_hand(gripper_finger1_joint): joint_goal = hand_group.get_current_joint_values() joint_goal[2] = gripper_finger1_joint # Gripper master axis @@ -53,7 +57,8 @@ def move_joint_hand(gripper_finger1_joint): hand_group.go(joint_goal, wait=True) hand_group.stop() # To guarantee no residual movement -if __name__ == '__main__': + +if __name__ == "__main__": # Ejemplo de uso de IK (en loop) for i in range(2): diff --git a/CustomRobots/pick_place/pick_place_exercise/IK_moveL.py b/CustomRobots/pick_place/pick_place_exercise/IK_moveL.py index cd3cd80c9..dc2831d21 100644 --- a/CustomRobots/pick_place/pick_place_exercise/IK_moveL.py +++ b/CustomRobots/pick_place/pick_place_exercise/IK_moveL.py @@ -12,29 +12,29 @@ # Inicializacion, objetos robot y escena, grupos MoveIt moveit_commander.roscpp_initialize(sys.argv) -rospy.init_node('moving_irb120_robot', anonymous=True) +rospy.init_node("moving_irb120_robot", anonymous=True) robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() arm_group = moveit_commander.MoveGroupCommander("irb_120") hand_group = moveit_commander.MoveGroupCommander("robotiq_85") -display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', - moveit_msgs.msg.DisplayTrajectory, - queue_size=20) +display_trajectory_publisher = rospy.Publisher( + "/move_group/display_planned_path", moveit_msgs.msg.DisplayTrajectory, queue_size=20 +) # INVERSE KINEMATICS (cartesiana, equivalente a moveL) # Creación de una trayectoria lineal PUNTO A PUNTO # Usamos una lista vacia en la que insertamos los puntos esquina de la trayectoria lineal waypoints = [] - + # Position 1, lateral (incremento en y) wpose = arm_group.get_current_pose().pose -wpose.position.y += 0.2 +wpose.position.y += 0.2 waypoints.append(copy.deepcopy(wpose)) # Position 2, lateral (coordenadas absolutas) wpose.position.x = 0 wpose.position.y = 0.5 -wpose.position.z = 0.5 +wpose.position.z = 0.5 waypoints.append(copy.deepcopy(wpose)) # Position 3, sobre el robot (coordenadas absolutas) @@ -46,19 +46,22 @@ # Position 4, otro lateral (coordenadas absolutas) wpose.position.x = 0 wpose.position.y = -0.5 -wpose.position.z = 0.5 +wpose.position.z = 0.5 waypoints.append(copy.deepcopy(wpose)) # Position 5, frontal (coordenadas absolutas) wpose.position.x = 0.5 wpose.position.y = 0 -wpose.position.z = 0.3 +wpose.position.z = 0.3 waypoints.append(copy.deepcopy(wpose)) + # Mantenemos la definicion de este método de IK equivalente al MoveJ por comodidad def move_pose_arm(roll, pitch, yaw, x, y, z): pose_goal = geometry_msgs.msg.Pose() - quat = quaternion_from_euler(roll*(pi/180), pitch*(pi/180), yaw*(pi/180)) + quat = quaternion_from_euler( + roll * (pi / 180), pitch * (pi / 180), yaw * (pi / 180) + ) pose_goal.orientation.x = quat[0] pose_goal.orientation.y = quat[1] pose_goal.orientation.z = quat[2] @@ -72,31 +75,32 @@ def move_pose_arm(roll, pitch, yaw, x, y, z): arm_group.stop() # To guarantee no residual movement arm_group.clear_pose_targets() - -if __name__ == '__main__': - - # Movimiento a posicion HOME inicial (no cartesiano) - rospy.loginfo("Moving arm to HOME point") - move_pose_arm(0, 90, 0, 0.4, 0, 0.6) - rospy.sleep(1) - - # Planificación de la trayectoria cartesiana entre waypoints - # Se calcula una configuración de robot por cada "eef_step" en metros - # El "jump_threshold" especifica la distancia maxima entre puntos consecutivos del espacio de configuracion del robot (dejar a 0) - # El metodo devuelve la fraccion de la trayectoria que se ha podido seguir - (plan, fraction) = arm_group.compute_cartesian_path( - waypoints, # waypoints a seguir - 0.01, # eef_step - 0.0) # jump_threshold - rospy.loginfo("Cartesian path planned") - - # RVIZ trajectory visualization - display_trajectory = moveit_msgs.msg.DisplayTrajectory() - display_trajectory.trajectory_start = robot.get_current_state() - display_trajectory.trajectory.append(plan) - display_trajectory_publisher.publish(display_trajectory); # Publish - - # Complete trajectory execution - arm_group.execute(plan, wait=True) - rospy.loginfo("Cartesian path finished. Shutting down") - moveit_commander.roscpp_shutdown() + + +if __name__ == "__main__": + + # Movimiento a posicion HOME inicial (no cartesiano) + rospy.loginfo("Moving arm to HOME point") + move_pose_arm(0, 90, 0, 0.4, 0, 0.6) + rospy.sleep(1) + + # Planificación de la trayectoria cartesiana entre waypoints + # Se calcula una configuración de robot por cada "eef_step" en metros + # El "jump_threshold" especifica la distancia maxima entre puntos consecutivos del espacio de configuracion del robot (dejar a 0) + # El metodo devuelve la fraccion de la trayectoria que se ha podido seguir + (plan, fraction) = arm_group.compute_cartesian_path( + waypoints, 0.01, 0.0 # waypoints a seguir # eef_step + ) # jump_threshold + rospy.loginfo("Cartesian path planned") + + # RVIZ trajectory visualization + display_trajectory = moveit_msgs.msg.DisplayTrajectory() + display_trajectory.trajectory_start = robot.get_current_state() + display_trajectory.trajectory.append(plan) + display_trajectory_publisher.publish(display_trajectory) + # Publish + + # Complete trajectory execution + arm_group.execute(plan, wait=True) + rospy.loginfo("Cartesian path finished. Shutting down") + moveit_commander.roscpp_shutdown() diff --git a/CustomRobots/pick_place/pick_place_exercise/api_pick_and_place.py b/CustomRobots/pick_place/pick_place_exercise/api_pick_and_place.py index d8bc3a85a..d0f70cce5 100644 --- a/CustomRobots/pick_place/pick_place_exercise/api_pick_and_place.py +++ b/CustomRobots/pick_place/pick_place_exercise/api_pick_and_place.py @@ -8,7 +8,7 @@ import os import numpy -from moveit_commander import RobotCommander,PlanningSceneInterface +from moveit_commander import RobotCommander, PlanningSceneInterface from moveit_commander import roscpp_initialize, roscpp_shutdown from moveit_commander.conversions import pose_to_list import moveit_commander @@ -34,7 +34,7 @@ def __init__(self, pose, height, width, shape, color): class Pick_Place: - def __init__ (self): + def __init__(self): self.object_list = {} self.arm = moveit_commander.MoveGroupCommander("irb_120") @@ -49,7 +49,7 @@ def __init__ (self): self.add_objects() self.add_table() - #self.add_ground() + # self.add_ground() self.approach_retreat_desired_dist = 0.2 self.approach_retreat_min_dist = 0.1 @@ -60,12 +60,12 @@ def __init__ (self): def pickup(self, object_name, pose): grasps = self.generate_grasps(object_name, pose) self.arm.pick(object_name, grasps) - #self.gripper.stop() + # self.gripper.stop() - rospy.loginfo('Pick up successfully') + rospy.loginfo("Pick up successfully") self.arm.detach_object(object_name) self.clean_scene(object_name) - #rospy.sleep(1) + # rospy.sleep(1) # place object to goal pose def place(self, pose): @@ -74,21 +74,20 @@ def place(self, pose): # pose.position.z -= 0.1 # self.move_pose_arm(pose) - + waypoints = [] wpose = self.arm.get_current_pose().pose wpose.position.z -= 0.15 waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = self.arm.compute_cartesian_path( - waypoints, # waypoints to follow - 0.01, # eef_step - 0.0) # jump_threshold + waypoints, 0.01, 0.0 # waypoints to follow # eef_step + ) # jump_threshold self.arm.execute(plan, wait=True) self.move_joint_hand(0) rospy.sleep(1) - + # pose.position.z += 0.1 # self.move_pose_arm(pose) @@ -98,12 +97,11 @@ def place(self, pose): waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = self.arm.compute_cartesian_path( - waypoints, # waypoints to follow - 0.01, # eef_step - 0.0) # jump_threshold + waypoints, 0.01, 0.0 # waypoints to follow # eef_step + ) # jump_threshold self.arm.execute(plan, wait=True) - rospy.loginfo('Place successfully') + rospy.loginfo("Place successfully") def get_object_pose(self, object_name): pose = self.object_list[object_name].pose @@ -121,9 +119,9 @@ def add_ground(self): p.pose.position.x = 0.0 p.pose.position.y = 0.0 p.pose.position.z = -0.01 - size = (5,5,0.02) + size = (5, 5, 0.02) - #self.scene.add_box("ground",p, size) + # self.scene.add_box("ground",p, size) rospy.sleep(2) def add_table(self): @@ -135,9 +133,9 @@ def add_table(self): p.pose.position.x = 0.8 p.pose.position.y = 0.0 p.pose.position.z = 0.1 - size = (0.8,1.5,0.028) + size = (0.8, 1.5, 0.028) - self.scene.add_box("table",p, size) + self.scene.add_box("table", p, size) rospy.sleep(2) def add_objects(self): @@ -150,11 +148,11 @@ def add_objects(self): self.clean_scene(name) p.pose.position.x = 0.5 p.pose.position.y = 0.0 - p.pose.position.z = 0.025+0.115 + p.pose.position.z = 0.025 + 0.115 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) - size = (0.05,0.05,0.05) + size = (0.05, 0.05, 0.05) self.scene.add_box(name, p, size) @@ -175,7 +173,7 @@ def add_objects(self): self.clean_scene(name) p.pose.position.x = 0.5 p.pose.position.y = 0.2 - p.pose.position.z = 0.05+0.115 + p.pose.position.z = 0.05 + 0.115 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) @@ -202,7 +200,7 @@ def add_objects(self): self.clean_scene(name) p.pose.position.x = 0.5 p.pose.position.y = -0.2 - p.pose.position.z = 0.03+0.115 + p.pose.position.z = 0.03 + 0.115 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) @@ -219,11 +217,11 @@ def add_objects(self): print("add ball") rospy.sleep(1) - #print(self.object_list) + # print(self.object_list) def pose2msg(self, roll, pitch, yaw, x, y, z): pose = geometry_msgs.msg.Pose() - quat = quaternion_from_euler(roll,pitch,yaw) + quat = quaternion_from_euler(roll, pitch, yaw) pose.orientation.x = quat[0] pose.orientation.y = quat[1] pose.orientation.z = quat[2] @@ -238,24 +236,26 @@ def msg2pose(self, pose): x = pose.position.x y = pose.position.y z = pose.position.z - quaternion = (pose.orientation.x, - pose.orientation.y, - pose.orientation.z, - pose.orientation.w) + quaternion = ( + pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + pose.orientation.w, + ) euler = euler_from_quaternion(quaternion) roll = euler[0] pitch = euler[1] yaw = euler[2] - return roll, pitch, yaw, x, y, z + return roll, pitch, yaw, x, y, z def back_to_home(self): - self.move_joint_arm(0,0,0,0,0,0) + self.move_joint_arm(0, 0, 0, 0, 0, 0) self.move_joint_hand(0) rospy.sleep(1) # Forward Kinematics (FK): move the arm by axis values - def move_joint_arm(self,joint_0,joint_1,joint_2,joint_3,joint_4,joint_5): + def move_joint_arm(self, joint_0, joint_1, joint_2, joint_3, joint_4, joint_5): joint_goal = self.arm.get_current_joint_values() joint_goal[0] = joint_0 joint_goal[1] = joint_1 @@ -265,7 +265,7 @@ def move_joint_arm(self,joint_0,joint_1,joint_2,joint_3,joint_4,joint_5): joint_goal[5] = joint_5 self.arm.go(joint_goal, wait=True) - self.arm.stop() # To guarantee no residual movement + self.arm.stop() # To guarantee no residual movement # Inverse Kinematics: Move the robot arm to desired pose def move_pose_arm(self, pose_goal): @@ -273,16 +273,16 @@ def move_pose_arm(self, pose_goal): self.arm.go(wait=True) - self.arm.stop() # To guarantee no residual movement + self.arm.stop() # To guarantee no residual movement self.arm.clear_pose_targets() # Move the Robotiq gripper by master axis - def move_joint_hand(self,gripper_finger1_joint): + def move_joint_hand(self, gripper_finger1_joint): joint_goal = self.gripper.get_current_joint_values() - joint_goal[2] = gripper_finger1_joint # Gripper master axis + joint_goal[2] = gripper_finger1_joint # Gripper master axis self.gripper.go(joint_goal, wait=True) - self.gripper.stop() # To guarantee no residual movement + self.gripper.stop() # To guarantee no residual movement def generate_grasps(self, name, pose): grasps = [] @@ -297,19 +297,22 @@ def generate_grasps(self, name, pose): # Setting pre-grasp approach grasp.pre_grasp_approach.direction.header.stamp = now - grasp.pre_grasp_approach.direction.header.frame_id = self.robot.get_planning_frame() + grasp.pre_grasp_approach.direction.header.frame_id = ( + self.robot.get_planning_frame() + ) grasp.pre_grasp_approach.direction.vector.z = -0.5 grasp.pre_grasp_approach.min_distance = self.approach_retreat_min_dist grasp.pre_grasp_approach.desired_distance = self.approach_retreat_desired_dist # Setting post-grasp retreat grasp.post_grasp_retreat.direction.header.stamp = now - grasp.post_grasp_retreat.direction.header.frame_id = self.robot.get_planning_frame() + grasp.post_grasp_retreat.direction.header.frame_id = ( + self.robot.get_planning_frame() + ) grasp.post_grasp_retreat.direction.vector.z = 0.5 grasp.post_grasp_retreat.min_distance = self.approach_retreat_min_dist grasp.post_grasp_retreat.desired_distance = self.approach_retreat_desired_dist - q = quaternion_from_euler(0.0, numpy.deg2rad(90.0), angle) grasp.grasp_pose.pose.orientation = Quaternion(*q) @@ -330,8 +333,8 @@ def generate_grasps(self, name, pose): elif name == "cylinder": traj.positions.append(0.3) - #traj.velocities.append(0.2) - #traj.effort.append(100) + # traj.velocities.append(0.2) + # traj.effort.append(100) traj.time_from_start = rospy.Duration.from_sec(5.0) grasp.grasp_posture.points.append(traj) @@ -400,10 +403,10 @@ def MyAlgorithm(self): if __name__ == "__main__": roscpp_initialize(sys.argv) - rospy.init_node('pick_and_place') + rospy.init_node("pick_and_place") p = Pick_Place() p.MyAlgorithm() rospy.spin() - roscpp_shutdown() \ No newline at end of file + roscpp_shutdown() diff --git a/CustomRobots/pick_place/pick_place_exercise/spawn_model.py b/CustomRobots/pick_place/pick_place_exercise/spawn_model.py index 6188f093f..160b1d7df 100644 --- a/CustomRobots/pick_place/pick_place_exercise/spawn_model.py +++ b/CustomRobots/pick_place/pick_place_exercise/spawn_model.py @@ -4,31 +4,38 @@ from geometry_msgs.msg import Pose, Quaternion from tf.transformations import euler_from_quaternion, quaternion_from_euler + def pose2msg(x, y, z, roll, pitch, yaw): pose = Pose() pose.position.x = x pose.position.y = y pose.position.z = z - quat = quaternion_from_euler(roll,pitch,yaw) + quat = quaternion_from_euler(roll, pitch, yaw) pose.orientation.x = quat[0] pose.orientation.y = quat[1] pose.orientation.z = quat[2] pose.orientation.w = quat[3] return pose + if __name__ == "__main__": print("spawning seleted model") - rospy.init_node('spawn_model') + rospy.init_node("spawn_model") rospy.wait_for_service("gazebo/spawn_sdf_model") spawn_model = rospy.ServiceProxy("gazebo/spawn_sdf_model", SpawnModel) model_name = "red_box" # Actualizar la ruta de la carpeta por la correcta: - with open("/home/diego/ws_moveit/src/irb120_robotiq85/irb120_robotiq85_gazebo/models/{}/model.sdf".format(model_name), "r") as f: - model_xml = f.read() - - # Posición y orientacion del objeto + with open( + "/home/diego/ws_moveit/src/irb120_robotiq85/irb120_robotiq85_gazebo/models/{}/model.sdf".format( + model_name + ), + "r", + ) as f: + model_xml = f.read() + + # Posición y orientacion del objeto model_pose = pose2msg(0.5, 0, 0, 0, 0, 0) spawn_model(model_name, model_xml, "", model_pose, "world") diff --git a/CustomRobots/pick_place/pick_place_exercise/test_pick_and_place.py b/CustomRobots/pick_place/pick_place_exercise/test_pick_and_place.py index da7727c92..8da8281bf 100644 --- a/CustomRobots/pick_place/pick_place_exercise/test_pick_and_place.py +++ b/CustomRobots/pick_place/pick_place_exercise/test_pick_and_place.py @@ -10,7 +10,7 @@ from actionlib import SimpleActionClient, GoalStatus -from moveit_commander import RobotCommander,PlanningSceneInterface +from moveit_commander import RobotCommander, PlanningSceneInterface from moveit_commander import roscpp_initialize, roscpp_shutdown from moveit_commander.conversions import pose_to_list import moveit_commander @@ -22,14 +22,16 @@ from math import pi from tf.transformations import euler_from_quaternion, quaternion_from_euler -#from moveit_python import * + +# from moveit_python import * from geometry_msgs.msg import Pose, PoseStamped, PoseArray, Quaternion from moveit_msgs.msg import PickupAction, PickupGoal from moveit_msgs.msg import PlaceAction, PlaceGoal from moveit_msgs.msg import PlaceLocation from moveit_msgs.msg import MoveItErrorCodes -#from moveit_simple_grasps.msg import GenerateGraspsAction, GenerateGraspsGoal, GraspGeneratorOptions + +# from moveit_simple_grasps.msg import GenerateGraspsAction, GenerateGraspsGoal, GraspGeneratorOptions from gazebo_msgs.srv import ( SpawnModel, @@ -39,13 +41,13 @@ # Diccionaro con codigos de error de MoveIt: moveit_error_dict = {} for name in MoveItErrorCodes.__dict__.keys(): - if not name[:1] == '_': + if not name[:1] == "_": code = MoveItErrorCodes.__dict__[name] moveit_error_dict[code] = name class Pick_Place: - def __init__ (self): + def __init__(self): self.object_name = "box" self.object_width = 0.05 @@ -65,44 +67,44 @@ def __init__ (self): self.pose_object = self.add_object(self.object_name) rospy.sleep(1.0) - - # Posicion para el PICK - self.pose_object.position.z += 0.16 # Posición vertical de la brida del robot, no del TCP de la herramienta! + + # Posicion para el PICK + self.pose_object.position.z += 0.16 # Posición vertical de la brida del robot, no del TCP de la herramienta! print(self.pose_object.position) - # Posicion para el PLACE + # Posicion para el PLACE self.pose_place = Pose() self.pose_place.position.x = 0.5 self.pose_place.position.y = 0.2 - self.pose_place.position.z = 0.01 # Security margin + self.pose_place.position.z = 0.01 # Security margin self.pose_place.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0)) - - # Parametros para la retirada de la pinza tras coger + + # Parametros para la retirada de la pinza tras coger self.approach_retreat_desired_dist = 0.2 self.approach_retreat_min_dist = 0.1 print("Starting pick and place") - # Generacion del mensaje de grasp y ejecución del PICK con la posicion y tamaño del objeto + # Generacion del mensaje de grasp y ejecución del PICK con la posicion y tamaño del objeto grasps = self.generate_grasps(self.pose_object, self.object_width) self.arm.pick(self.object_name, grasps) - rospy.loginfo('Pick up successfully') + rospy.loginfo("Pick up successfully") rospy.sleep(1) - + # Generacion del mensaje de grasp para el PLACE y ejecución del PLACE con el nombre y posición del objeto. place = self.generate_places(self.pose_place) self.arm.place(self.object_name, place) - rospy.loginfo('Place successfully') - self.move_joint_hand(0) # Abrimos la pinza + rospy.loginfo("Place successfully") + self.move_joint_hand(0) # Abrimos la pinza rospy.sleep(1) # Detach en RVIZ self.arm.detach_object(self.object_name) - rospy.loginfo("Moving arm to HOME point") - self.move_pose_arm(0,0.8,0,0.4,0,0.6) - + rospy.loginfo("Moving arm to HOME point") + self.move_pose_arm(0, 0.8, 0, 0.4, 0, 0.6) + self.clean_scene() def clean_scene(self): @@ -119,15 +121,15 @@ def add_object(self, name): q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) - size = (0.05,0.05,0.05) + size = (0.05, 0.05, 0.05) self.scene.add_box(name, p, size) return p.pose - def move_pose_arm(self, roll,pitch,yaw,x,y,z): + def move_pose_arm(self, roll, pitch, yaw, x, y, z): pose_goal = geometry_msgs.msg.Pose() - quat = quaternion_from_euler(roll,pitch,yaw) + quat = quaternion_from_euler(roll, pitch, yaw) pose_goal.orientation.x = quat[0] pose_goal.orientation.y = quat[1] pose_goal.orientation.z = quat[2] @@ -139,16 +141,17 @@ def move_pose_arm(self, roll,pitch,yaw,x,y,z): plan = self.arm.go(wait=True) - self.arm.stop() # To guarantee no residual movement + self.arm.stop() # To guarantee no residual movement self.arm.clear_pose_targets() # Movimiento eje a eje de la herramienta - def move_joint_hand(self,gripper_finger1_joint): + + def move_joint_hand(self, gripper_finger1_joint): joint_goal = self.gripper.get_current_joint_values() - joint_goal[2] = gripper_finger1_joint # Gripper master axis + joint_goal[2] = gripper_finger1_joint # Gripper master axis self.gripper.go(joint_goal, wait=True) - self.gripper.stop() # To guarantee no residual movement + self.gripper.stop() # To guarantee no residual movement def generate_grasps(self, pose, width): grasps = [] @@ -167,19 +170,27 @@ def generate_grasps(self, pose, width): # Setting pre-grasp approach grasp.pre_grasp_approach.direction.header.stamp = now - grasp.pre_grasp_approach.direction.header.frame_id = self.robot.get_planning_frame() + grasp.pre_grasp_approach.direction.header.frame_id = ( + self.robot.get_planning_frame() + ) grasp.pre_grasp_approach.direction.vector.z = -0.2 grasp.pre_grasp_approach.min_distance = self.approach_retreat_min_dist - grasp.pre_grasp_approach.desired_distance = self.approach_retreat_desired_dist + grasp.pre_grasp_approach.desired_distance = ( + self.approach_retreat_desired_dist + ) # Setting post-grasp retreat grasp.post_grasp_retreat.direction.header.stamp = now - grasp.post_grasp_retreat.direction.header.frame_id = self.robot.get_planning_frame() + grasp.post_grasp_retreat.direction.header.frame_id = ( + self.robot.get_planning_frame() + ) grasp.post_grasp_retreat.direction.vector.z = 0.2 grasp.post_grasp_retreat.min_distance = self.approach_retreat_min_dist - grasp.post_grasp_retreat.desired_distance = self.approach_retreat_desired_dist + grasp.post_grasp_retreat.desired_distance = ( + self.approach_retreat_desired_dist + ) - grasp.max_contact_force = 0 #<=0 to disable + grasp.max_contact_force = 0 # <=0 to disable grasp.pre_grasp_posture.joint_names.append("gripper_finger1_joint") traj = JointTrajectoryPoint() @@ -190,7 +201,7 @@ def generate_grasps(self, pose, width): grasp.grasp_posture.joint_names.append("gripper_finger1_joint") traj = JointTrajectoryPoint() traj.positions.append(0.55) - #traj.effort.append(100) + # traj.effort.append(100) traj.time_from_start = rospy.Duration.from_sec(1.0) grasp.grasp_posture.points.append(traj) @@ -199,10 +210,10 @@ def generate_grasps(self, pose, width): return grasps def generate_places(self, target): - #places = [] + # places = [] now = rospy.Time.now() # for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(30.0)): - # Create place location: + # Create place location: place = PlaceLocation() place.place_pose.header.stamp = now @@ -220,15 +231,19 @@ def generate_places(self, target): place.pre_place_approach.min_distance = self.approach_retreat_min_dist place.pre_place_approach.direction.header.stamp = now - place.pre_place_approach.direction.header.frame_id = self.robot.get_planning_frame() + place.pre_place_approach.direction.header.frame_id = ( + self.robot.get_planning_frame() + ) - place.pre_place_approach.direction.vector.x = 0 - place.pre_place_approach.direction.vector.y = 0 + place.pre_place_approach.direction.vector.x = 0 + place.pre_place_approach.direction.vector.y = 0 place.pre_place_approach.direction.vector.z = -0.2 # Generate post place approach: place.post_place_retreat.direction.header.stamp = now - place.post_place_retreat.direction.header.frame_id = self.robot.get_planning_frame() + place.post_place_retreat.direction.header.frame_id = ( + self.robot.get_planning_frame() + ) place.post_place_retreat.desired_distance = self.approach_retreat_desired_dist place.post_place_retreat.min_distance = self.approach_retreat_min_dist @@ -242,12 +257,12 @@ def generate_places(self, target): place.post_place_posture.joint_names.append("gripper_finger1_joint") traj = JointTrajectoryPoint() traj.positions.append(0) - #traj.effort.append(0) + # traj.effort.append(0) traj.time_from_start = rospy.Duration.from_sec(1.0) place.post_place_posture.points.append(traj) - # Add place: - #places.append(place) + # Add place: + # places.append(place) return place @@ -297,7 +312,9 @@ def pickup(self, group, target, width): state = self.pickup_action.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: - rospy.logerr('Pick up goal failed!: %s' % self.pickup_action.get_goal_status_text()) + rospy.logerr( + "Pick up goal failed!: %s" % self.pickup_action.get_goal_status_text() + ) return None result = self.pickup_action.get_result() @@ -305,7 +322,10 @@ def pickup(self, group, target, width): # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: - rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err]))) + rospy.logwarn( + "Group %s cannot pick up target %s!: %s" + % (group, target, str(moveit_error_dict[err])) + ) return False @@ -317,7 +337,9 @@ def place(self, group, target, place): state = self.place_action.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: - rospy.logerr('Pick up goal failed!: %s' % self.place_action.get_goal_status_text()) + rospy.logerr( + "Pick up goal failed!: %s" % self.place_action.get_goal_status_text() + ) return None result = self.place_action.get_result() @@ -325,15 +347,19 @@ def place(self, group, target, place): # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: - rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err]))) + rospy.logwarn( + "Group %s cannot pick up target %s!: %s" + % (group, target, str(moveit_error_dict[err])) + ) return False return True + if __name__ == "__main__": roscpp_initialize(sys.argv) - rospy.init_node('pick_and_place') + rospy.init_node("pick_and_place") p = Pick_Place() diff --git a/CustomRobots/pick_place/test/test_copyright.py b/CustomRobots/pick_place/test/test_copyright.py index 97a39196e..ceffe896d 100644 --- a/CustomRobots/pick_place/test/test_copyright.py +++ b/CustomRobots/pick_place/test/test_copyright.py @@ -17,9 +17,11 @@ # Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.skip( + reason="No copyright header has been placed in the generated source file." +) @pytest.mark.copyright @pytest.mark.linter def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/CustomRobots/pick_place/test/test_flake8.py b/CustomRobots/pick_place/test/test_flake8.py index 27ee1078f..ee79f31ac 100644 --- a/CustomRobots/pick_place/test/test_flake8.py +++ b/CustomRobots/pick_place/test/test_flake8.py @@ -20,6 +20,6 @@ @pytest.mark.linter def test_flake8(): rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) + assert rc == 0, "Found %d code style errors / warnings:\n" % len( + errors + ) + "\n".join(errors) diff --git a/CustomRobots/pick_place/test/test_pep257.py b/CustomRobots/pick_place/test/test_pep257.py index b234a3840..a2c3deb8e 100644 --- a/CustomRobots/pick_place/test/test_pep257.py +++ b/CustomRobots/pick_place/test/test_pep257.py @@ -19,5 +19,5 @@ @pytest.mark.linter @pytest.mark.pep257 def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings" diff --git a/CustomRobots/roomba_robot/launch/vaccum_cleaner.launch.py b/CustomRobots/roomba_robot/launch/vaccum_cleaner.launch.py index 16f260e13..1f306804b 100755 --- a/CustomRobots/roomba_robot/launch/vaccum_cleaner.launch.py +++ b/CustomRobots/roomba_robot/launch/vaccum_cleaner.launch.py @@ -7,74 +7,86 @@ from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare + def generate_launch_description(): - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'roomba_1_house.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - return ld + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "roomba_1_house.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/roomba_robot/launch/vaccum_cleaner_headless.launch.py b/CustomRobots/roomba_robot/launch/vaccum_cleaner_headless.launch.py index 899c62e5f..d46c3361d 100755 --- a/CustomRobots/roomba_robot/launch/vaccum_cleaner_headless.launch.py +++ b/CustomRobots/roomba_robot/launch/vaccum_cleaner_headless.launch.py @@ -7,73 +7,84 @@ from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare + def generate_launch_description(): - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'roomba_1_house.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='True', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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]))) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_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) - - return ld + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "roomba_1_house.world" + world_path = os.path.join(pkg_share, "worlds", world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", default_value="True", description="Whether to execute gzclient" + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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])), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_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) + + return ld diff --git a/CustomRobots/tello_phy/tello_camera/setup.py b/CustomRobots/tello_phy/tello_camera/setup.py index 861beb186..6888ea4a2 100755 --- a/CustomRobots/tello_phy/tello_camera/setup.py +++ b/CustomRobots/tello_phy/tello_camera/setup.py @@ -1,26 +1,23 @@ from setuptools import setup -package_name = 'tello_camera' +package_name = "tello_camera" setup( name=package_name, - version='0.0.0', + version="0.0.0", packages=[package_name], data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), ], - install_requires=['setuptools'], + install_requires=["setuptools"], zip_safe=True, - maintainer='guillermo', - maintainer_email='guillerbernal@gmail.com', - description='TODO: Package description', - license='TODO: License declaration', - tests_require=['pytest'], + maintainer="guillermo", + maintainer_email="guillerbernal@gmail.com", + description="TODO: Package description", + license="TODO: License declaration", + tests_require=["pytest"], entry_points={ - 'console_scripts': [ - 'tello_camera_node = tello_camera.tello_camera:main' - ], + "console_scripts": ["tello_camera_node = tello_camera.tello_camera:main"], }, ) diff --git a/CustomRobots/tello_phy/tello_camera/tello_camera/tello_camera.py b/CustomRobots/tello_phy/tello_camera/tello_camera/tello_camera.py index 953d256f6..f74856bd9 100755 --- a/CustomRobots/tello_phy/tello_camera/tello_camera/tello_camera.py +++ b/CustomRobots/tello_phy/tello_camera/tello_camera/tello_camera.py @@ -1,6 +1,6 @@ import rclpy from rclpy.node import Node -import cv2 +import cv2 from sensor_msgs.msg import Image from cv_bridge import CvBridge @@ -9,19 +9,17 @@ class telloCam(Node): def __init__(self): - super().__init__('tello_camera_node') + super().__init__("tello_camera_node") self.subscription = self.create_subscription( - Image, - '/image_raw', - self.listener_callback, - 10) + Image, "/image_raw", self.listener_callback, 10 + ) self.br = CvBridge() def listener_callback(self, img): print("Callback") current_frame = self.br.imgmsg_to_cv2(img) hsv_img = cv2.cvtColor(current_frame, cv2.COLOR_BGR2HSV) - cv2.imshow('HSV image', hsv_img) + cv2.imshow("HSV image", hsv_img) cv2.waitKey(1) @@ -30,7 +28,6 @@ def main(args=None): minimal_subscriber = telloCam() - rclpy.spin(minimal_subscriber) # Destroy the node explicitly @@ -40,5 +37,5 @@ def main(args=None): rclpy.shutdown() -if __name__ == '__main__': - main() \ No newline at end of file +if __name__ == "__main__": + main() diff --git a/CustomRobots/tello_phy/tello_camera/test/test_copyright.py b/CustomRobots/tello_phy/tello_camera/test/test_copyright.py index 97a39196e..ceffe896d 100755 --- a/CustomRobots/tello_phy/tello_camera/test/test_copyright.py +++ b/CustomRobots/tello_phy/tello_camera/test/test_copyright.py @@ -17,9 +17,11 @@ # Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.skip( + reason="No copyright header has been placed in the generated source file." +) @pytest.mark.copyright @pytest.mark.linter def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/CustomRobots/tello_phy/tello_camera/test/test_flake8.py b/CustomRobots/tello_phy/tello_camera/test/test_flake8.py index 27ee1078f..ee79f31ac 100755 --- a/CustomRobots/tello_phy/tello_camera/test/test_flake8.py +++ b/CustomRobots/tello_phy/tello_camera/test/test_flake8.py @@ -20,6 +20,6 @@ @pytest.mark.linter def test_flake8(): rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) + assert rc == 0, "Found %d code style errors / warnings:\n" % len( + errors + ) + "\n".join(errors) diff --git a/CustomRobots/tello_phy/tello_camera/test/test_pep257.py b/CustomRobots/tello_phy/tello_camera/test/test_pep257.py index b234a3840..a2c3deb8e 100755 --- a/CustomRobots/tello_phy/tello_camera/test/test_pep257.py +++ b/CustomRobots/tello_phy/tello_camera/test/test_pep257.py @@ -19,5 +19,5 @@ @pytest.mark.linter @pytest.mark.pep257 def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings" diff --git a/CustomRobots/tello_phy/tello_ros/tello_description/src/replace.py b/CustomRobots/tello_phy/tello_ros/tello_description/src/replace.py index bfc61e38d..d85e8b9d0 100755 --- a/CustomRobots/tello_phy/tello_ros/tello_description/src/replace.py +++ b/CustomRobots/tello_phy/tello_ros/tello_description/src/replace.py @@ -10,7 +10,7 @@ def replace(s: str, d: Dict[str, str]) -> str: for k, v in d.items(): - long_k = '${' + k + '}' + long_k = "${" + k + "}" s = s.replace(long_k, v) return s @@ -18,7 +18,7 @@ def replace(s: str, d: Dict[str, str]) -> str: def parse_args(argv: List[str]) -> Dict[str, str]: d = {} for a in argv: - p = a.split('=') + p = a.split("=") if len(p) != 2: print('ignoring "%r"' % a, file=sys.stderr) continue @@ -27,8 +27,11 @@ def parse_args(argv: List[str]) -> Dict[str, str]: if len(sys.argv) < 2: - print('usage: replace.py something.xml foo=1 bar=2.0 fee=string random="also a string"', file=sys.stderr) + print( + 'usage: replace.py something.xml foo=1 bar=2.0 fee=string random="also a string"', + file=sys.stderr, + ) sys.exit(1) -f = open(sys.argv[1], 'r') +f = open(sys.argv[1], "r") print(replace(f.read(), parse_args(sys.argv[2:]))) diff --git a/CustomRobots/tello_phy/tello_ros/tello_driver/launch/emulator_launch.py b/CustomRobots/tello_phy/tello_ros/tello_driver/launch/emulator_launch.py index 5d3056dd4..04f62dff9 100755 --- a/CustomRobots/tello_phy/tello_ros/tello_driver/launch/emulator_launch.py +++ b/CustomRobots/tello_phy/tello_ros/tello_driver/launch/emulator_launch.py @@ -7,11 +7,18 @@ def generate_launch_description(): - emulator_path = 'install/tello_driver/lib/tello_driver/tello_emulator' - tello_driver_params = [{'drone_ip': '127.0.0.1'}] + emulator_path = "install/tello_driver/lib/tello_driver/tello_emulator" + tello_driver_params = [{"drone_ip": "127.0.0.1"}] - return LaunchDescription([ - ExecuteProcess(cmd=[emulator_path], output='screen'), - Node(package='tello_driver', executable='tello_driver_main', node_name='tello_driver', - parameters=tello_driver_params, output='screen'), - ]) + return LaunchDescription( + [ + ExecuteProcess(cmd=[emulator_path], output="screen"), + Node( + package="tello_driver", + executable="tello_driver_main", + node_name="tello_driver", + parameters=tello_driver_params, + output="screen", + ), + ] + ) diff --git a/CustomRobots/tello_phy/tello_ros/tello_driver/launch/emulators_launch.py b/CustomRobots/tello_phy/tello_ros/tello_driver/launch/emulators_launch.py index 67035549a..89ed347ad 100755 --- a/CustomRobots/tello_phy/tello_ros/tello_driver/launch/emulators_launch.py +++ b/CustomRobots/tello_phy/tello_ros/tello_driver/launch/emulators_launch.py @@ -7,8 +7,8 @@ def generate_launch_description(): - emulator_path = 'install/tello_driver/lib/tello_driver/tello_emulator' - localhost = '127.0.0.1' + emulator_path = "install/tello_driver/lib/tello_driver/tello_emulator" + localhost = "127.0.0.1" dr1_cmd_port = 11001 dr2_cmd_port = 11002 @@ -22,29 +22,63 @@ def generate_launch_description(): dr1_video_port = 14001 dr2_video_port = 14002 - dr1_params = [{ - 'drone_ip': localhost, - 'drone_port': em1_port, - 'command_port': dr1_cmd_port, - 'data_port': dr1_data_port, - 'video_port': dr1_video_port - }] - - dr2_params = [{ - 'drone_ip': localhost, - 'drone_port': em2_port, - 'command_port': dr2_cmd_port, - 'data_port': dr2_data_port, - 'video_port': dr2_video_port - }] - - return LaunchDescription([ - ExecuteProcess(cmd=[emulator_path, 'em1', str(em1_port), str(dr1_data_port), str(dr1_video_port)], - output='screen'), - ExecuteProcess(cmd=[emulator_path, 'em2', str(em2_port), str(dr2_data_port), str(dr2_video_port)], - output='screen'), - Node(package='tello_driver', executable='tello_driver_main', node_name='dr1', namespace='dr1', - parameters=dr1_params, output='screen'), - Node(package='tello_driver', executable='tello_driver_main', node_name='dr2', namespace='dr2', - parameters=dr2_params, output='screen'), - ]) + dr1_params = [ + { + "drone_ip": localhost, + "drone_port": em1_port, + "command_port": dr1_cmd_port, + "data_port": dr1_data_port, + "video_port": dr1_video_port, + } + ] + + dr2_params = [ + { + "drone_ip": localhost, + "drone_port": em2_port, + "command_port": dr2_cmd_port, + "data_port": dr2_data_port, + "video_port": dr2_video_port, + } + ] + + return LaunchDescription( + [ + ExecuteProcess( + cmd=[ + emulator_path, + "em1", + str(em1_port), + str(dr1_data_port), + str(dr1_video_port), + ], + output="screen", + ), + ExecuteProcess( + cmd=[ + emulator_path, + "em2", + str(em2_port), + str(dr2_data_port), + str(dr2_video_port), + ], + output="screen", + ), + Node( + package="tello_driver", + executable="tello_driver_main", + node_name="dr1", + namespace="dr1", + parameters=dr1_params, + output="screen", + ), + Node( + package="tello_driver", + executable="tello_driver_main", + node_name="dr2", + namespace="dr2", + parameters=dr2_params, + output="screen", + ), + ] + ) diff --git a/CustomRobots/tello_phy/tello_ros/tello_driver/launch/teleop_launch.py b/CustomRobots/tello_phy/tello_ros/tello_driver/launch/teleop_launch.py index da9b8b1ed..3a79a489f 100755 --- a/CustomRobots/tello_phy/tello_ros/tello_driver/launch/teleop_launch.py +++ b/CustomRobots/tello_phy/tello_ros/tello_driver/launch/teleop_launch.py @@ -6,8 +6,12 @@ def generate_launch_description(): - return LaunchDescription([ - Node(package='joy', executable='joy_node', output='screen'), - Node(package='tello_driver', executable='tello_joy_main', output='screen'), - Node(package='tello_driver', executable='tello_driver_main', output='screen'), - ]) + return LaunchDescription( + [ + Node(package="joy", executable="joy_node", output="screen"), + Node(package="tello_driver", executable="tello_joy_main", output="screen"), + Node( + package="tello_driver", executable="tello_driver_main", output="screen" + ), + ] + ) diff --git a/CustomRobots/tello_phy/tello_ros/tello_gazebo/launch/simple_launch.py b/CustomRobots/tello_phy/tello_ros/tello_gazebo/launch/simple_launch.py index adcc13179..14ee8d49e 100755 --- a/CustomRobots/tello_phy/tello_ros/tello_gazebo/launch/simple_launch.py +++ b/CustomRobots/tello_phy/tello_ros/tello_gazebo/launch/simple_launch.py @@ -9,33 +9,51 @@ def generate_launch_description(): - ns = 'drone1' - world_path = os.path.join(get_package_share_directory('tello_gazebo'), 'worlds', 'simple.world') - urdf_path = os.path.join(get_package_share_directory('tello_description'), 'urdf', 'tello_1.urdf') - - return LaunchDescription([ - # Launch Gazebo, loading tello.world - ExecuteProcess(cmd=[ - 'gazebo', - '--verbose', - '-s', 'libgazebo_ros_init.so', # Publish /clock - '-s', 'libgazebo_ros_factory.so', # Provide gazebo_ros::Node - world_path - ], output='screen'), - - # Spawn tello.urdf - Node(package='tello_gazebo', executable='inject_entity.py', output='screen', - arguments=[urdf_path, '0', '0', '1', '0']), - - # Publish static transforms - Node(package='robot_state_publisher', executable='robot_state_publisher', output='screen', - arguments=[urdf_path]), - - # Joystick driver, generates /namespace/joy messages - Node(package='joy', executable='joy_node', output='screen', - namespace=ns), - - # Joystick controller, generates /namespace/cmd_vel messages - Node(package='tello_driver', executable='tello_joy_main', output='screen', - namespace=ns), - ]) + ns = "drone1" + world_path = os.path.join( + get_package_share_directory("tello_gazebo"), "worlds", "simple.world" + ) + urdf_path = os.path.join( + get_package_share_directory("tello_description"), "urdf", "tello_1.urdf" + ) + + return LaunchDescription( + [ + # Launch Gazebo, loading tello.world + ExecuteProcess( + cmd=[ + "gazebo", + "--verbose", + "-s", + "libgazebo_ros_init.so", # Publish /clock + "-s", + "libgazebo_ros_factory.so", # Provide gazebo_ros::Node + world_path, + ], + output="screen", + ), + # Spawn tello.urdf + Node( + package="tello_gazebo", + executable="inject_entity.py", + output="screen", + arguments=[urdf_path, "0", "0", "1", "0"], + ), + # Publish static transforms + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="screen", + arguments=[urdf_path], + ), + # Joystick driver, generates /namespace/joy messages + Node(package="joy", executable="joy_node", output="screen", namespace=ns), + # Joystick controller, generates /namespace/cmd_vel messages + Node( + package="tello_driver", + executable="tello_joy_main", + output="screen", + namespace=ns, + ), + ] + ) diff --git a/CustomRobots/tello_phy/tello_ros/tello_gazebo/launch/vlam_launch.py b/CustomRobots/tello_phy/tello_ros/tello_gazebo/launch/vlam_launch.py index e1f1c8f0a..1d945efea 100755 --- a/CustomRobots/tello_phy/tello_ros/tello_gazebo/launch/vlam_launch.py +++ b/CustomRobots/tello_phy/tello_ros/tello_gazebo/launch/vlam_launch.py @@ -11,61 +11,91 @@ def generate_launch_description(): # 1 or more drones: # drones = ['drone1', 'drone2'] - drones = ['drone1'] + drones = ["drone1"] - tello_gazebo_path = get_package_share_directory('tello_gazebo') - tello_description_path = get_package_share_directory('tello_description') + tello_gazebo_path = get_package_share_directory("tello_gazebo") + tello_description_path = get_package_share_directory("tello_description") - world_path = os.path.join(tello_gazebo_path, 'worlds', 'fiducial.world') - map_path = os.path.join(tello_gazebo_path, 'worlds', 'fiducial_map.yaml') + world_path = os.path.join(tello_gazebo_path, "worlds", "fiducial.world") + map_path = os.path.join(tello_gazebo_path, "worlds", "fiducial_map.yaml") # Global entities entities = [ # Launch Gazebo, loading tello.world - ExecuteProcess(cmd=[ - 'gazebo', - '--verbose', - '-s', 'libgazebo_ros_init.so', # Publish /clock - '-s', 'libgazebo_ros_factory.so', # Provide gazebo_ros::Node - world_path - ], output='screen'), - + ExecuteProcess( + cmd=[ + "gazebo", + "--verbose", + "-s", + "libgazebo_ros_init.so", # Publish /clock + "-s", + "libgazebo_ros_factory.so", # Provide gazebo_ros::Node + world_path, + ], + output="screen", + ), # Load and publish a known map - Node(package='fiducial_vlam', executable='vmap_main', output='screen', - name='vmap_main', parameters=[{ - 'publish_tfs': 1, # Publish marker /tf - 'marker_length': 0.1778, # Marker length - 'marker_map_load_full_filename': map_path, # Load a pre-built map from disk - 'make_not_use_map': 0}]), # Don't save a map to disk - + Node( + package="fiducial_vlam", + executable="vmap_main", + output="screen", + name="vmap_main", + parameters=[ + { + "publish_tfs": 1, # Publish marker /tf + "marker_length": 0.1778, # Marker length + "marker_map_load_full_filename": map_path, # Load a pre-built map from disk + "make_not_use_map": 0, + } + ], + ), # Don't save a map to disk # Joystick driver, generates /namespace/joy messages # Only controls the first drone - Node(package='joy', executable='joy_node', output='screen', - namespace=drones[0]), - + Node( + package="joy", executable="joy_node", output="screen", namespace=drones[0] + ), # Joystick controller, generates /namespace/cmd_vel messages - Node(package='tello_driver', executable='tello_joy_main', output='screen', - namespace=drones[0]), + Node( + package="tello_driver", + executable="tello_joy_main", + output="screen", + namespace=drones[0], + ), ] # Per-drone entities for idx, namespace in enumerate(drones): - suffix = '_' + str(idx + 1) - urdf_path = os.path.join(tello_description_path, 'urdf', 'tello' + suffix + '.urdf') - - entities.extend([ - # Add a drone to the simulation - Node(package='tello_gazebo', executable='inject_entity.py', output='screen', - arguments=[urdf_path, '0', str(idx), '1', '0']), - - # Localize this drone against the map - Node(package='fiducial_vlam', executable='vloc_main', output='screen', - name='vloc_main', namespace=namespace, parameters=[{ - 'publish_tfs': 1, - 'base_frame_id': 'base_link' + suffix, - 't_camera_base_z': -0.035, - 'camera_frame_id': 'camera_link' + suffix}]), + suffix = "_" + str(idx + 1) + urdf_path = os.path.join( + tello_description_path, "urdf", "tello" + suffix + ".urdf" + ) - ]) + entities.extend( + [ + # Add a drone to the simulation + Node( + package="tello_gazebo", + executable="inject_entity.py", + output="screen", + arguments=[urdf_path, "0", str(idx), "1", "0"], + ), + # Localize this drone against the map + Node( + package="fiducial_vlam", + executable="vloc_main", + output="screen", + name="vloc_main", + namespace=namespace, + parameters=[ + { + "publish_tfs": 1, + "base_frame_id": "base_link" + suffix, + "t_camera_base_z": -0.035, + "camera_frame_id": "camera_link" + suffix, + } + ], + ), + ] + ) return LaunchDescription(entities) diff --git a/CustomRobots/tello_phy/tello_ros/tello_gazebo/src/inject_entity.py b/CustomRobots/tello_phy/tello_ros/tello_gazebo/src/inject_entity.py index cfaa1efa6..0f8c02da4 100755 --- a/CustomRobots/tello_phy/tello_ros/tello_gazebo/src/inject_entity.py +++ b/CustomRobots/tello_phy/tello_ros/tello_gazebo/src/inject_entity.py @@ -14,11 +14,11 @@ def inject(xml: str, initial_pose: Pose): """Create a ROS node, and use it to call the SpawnEntity service""" rclpy.init() - node = rclpy.create_node('inject_node') - client = node.create_client(SpawnEntity, 'spawn_entity') + node = rclpy.create_node("inject_node") + client = node.create_client(SpawnEntity, "spawn_entity") if not client.service_is_ready(): - node.get_logger().info('waiting for spawn_entity service...') + node.get_logger().info("waiting for spawn_entity service...") client.wait_for_service() request = SpawnEntity.Request() @@ -28,19 +28,21 @@ def inject(xml: str, initial_pose: Pose): rclpy.spin_until_future_complete(node, future) if future.result() is not None: - node.get_logger().info('response: %r' % future.result()) + node.get_logger().info("response: %r" % future.result()) else: - raise RuntimeError('exception while calling service: %r' % future.exception()) + raise RuntimeError("exception while calling service: %r" % future.exception()) node.destroy_node() rclpy.shutdown() if len(sys.argv) < 6: - print('usage: ros2 run tello_gazebo inject_entity.py -- foo.urdf initial_x initial_y initial_z initial_yaw') + print( + "usage: ros2 run tello_gazebo inject_entity.py -- foo.urdf initial_x initial_y initial_z initial_yaw" + ) sys.exit(1) -f = open(sys.argv[1], 'r') +f = open(sys.argv[1], "r") p = Pose() p.position.x = float(sys.argv[2]) diff --git a/CustomRobots/tello_phy/tello_ros/tello_gazebo/worlds/build_world.py b/CustomRobots/tello_phy/tello_ros/tello_gazebo/worlds/build_world.py index e1ac5bb3f..3a3542be6 100755 --- a/CustomRobots/tello_phy/tello_ros/tello_gazebo/worlds/build_world.py +++ b/CustomRobots/tello_phy/tello_ros/tello_gazebo/worlds/build_world.py @@ -30,8 +30,9 @@ def build_world(name, markers): - world_file = open(name, 'w') - world_file.write(""" + world_file = open(name, "w") + world_file.write( + """ @@ -53,41 +54,50 @@ def build_world(name, markers): -""") +""" + ) for marker in markers: - world_file.write(f""" + world_file.write( + f""" true model://marker_{marker[0]} {marker[1]} {marker[2]} {marker[3]} {marker[4]} {marker[5]} {marker[6]} -""") - world_file.write(""" +""" + ) + world_file.write( + """ -""") +""" + ) world_file.close() def build_map(name, markers): - map_file = open(name, 'w') - map_file.write("""# Map for orca.world + map_file = open(name, "w") + map_file.write( + """# Map for orca.world # All marker locations are fixed (f: 1) marker_length: 0.1778 markers: -""") +""" + ) for marker in markers: t_marker_world = xf.euler_matrix(marker[4], marker[5], marker[6]) t_marker_map = t_marker_world @ t_world_map r_marker_map = xf.euler_from_matrix(t_marker_map) - map_file.write(f""" - id: {marker[0]} + map_file.write( + f""" - id: {marker[0]} u: 1 f: 1 xyz: [{marker[1]}, {marker[2]}, {marker[3]}] rpy: [{r_marker_map[0]}, {r_marker_map[1]}, {r_marker_map[2]}] -""") +""" + ) map_file.close() @@ -114,8 +124,8 @@ def build_map(name, markers): ] worlds = [ - ['fiducial.world', 'fiducial_map.yaml', fiducial], - ['f2.world', 'f2_map.yaml', f2], + ["fiducial.world", "fiducial_map.yaml", fiducial], + ["f2.world", "f2_map.yaml", f2], ] for world in worlds: diff --git a/CustomRobots/tello_phy/tello_simple_teleop/setup.py b/CustomRobots/tello_phy/tello_simple_teleop/setup.py index d0a47e137..8fd723222 100755 --- a/CustomRobots/tello_phy/tello_simple_teleop/setup.py +++ b/CustomRobots/tello_phy/tello_simple_teleop/setup.py @@ -1,26 +1,25 @@ from setuptools import setup -package_name = 'tello_simple_teleop' +package_name = "tello_simple_teleop" setup( name=package_name, - version='0.0.0', + version="0.0.0", packages=[package_name], data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), ], - install_requires=['setuptools'], + install_requires=["setuptools"], zip_safe=True, - maintainer='guillermo', - maintainer_email='guillerbernal@gmail.com', - description='TODO: Package description', - license='TODO: License declaration', - tests_require=['pytest'], + maintainer="guillermo", + maintainer_email="guillerbernal@gmail.com", + description="TODO: Package description", + license="TODO: License declaration", + tests_require=["pytest"], entry_points={ - 'console_scripts': [ - 'tello_simple_node = tello_simple_teleop.tello_simple_teleop:main' + "console_scripts": [ + "tello_simple_node = tello_simple_teleop.tello_simple_teleop:main" ], }, ) diff --git a/CustomRobots/tello_phy/tello_simple_teleop/tello_simple_teleop/tello_simple_teleop.py b/CustomRobots/tello_phy/tello_simple_teleop/tello_simple_teleop/tello_simple_teleop.py index 5588d9188..e8bc9527c 100755 --- a/CustomRobots/tello_phy/tello_simple_teleop/tello_simple_teleop/tello_simple_teleop.py +++ b/CustomRobots/tello_phy/tello_simple_teleop/tello_simple_teleop/tello_simple_teleop.py @@ -9,10 +9,10 @@ class MinimalClientAsync(Node): def __init__(self): - super().__init__('minimal_client_async') - self.cli = self.create_client(TelloAction, 'tello_action') + super().__init__("minimal_client_async") + self.cli = self.create_client(TelloAction, "tello_action") while not self.cli.wait_for_service(timeout_sec=1.0): - self.get_logger().info('service not available, waiting again...') + self.get_logger().info("service not available, waiting again...") self.req = TelloAction.Request() def send_request(self, cmd): @@ -25,15 +25,15 @@ def main(args=None): rclpy.init(args=args) minimal_client = MinimalClientAsync() - response = minimal_client.send_request('takeoff') - + response = minimal_client.send_request("takeoff") + time.sleep(2) - - response = minimal_client.send_request('stop') + + response = minimal_client.send_request("stop") minimal_client.destroy_node() rclpy.shutdown() -if __name__ == '__main__': - main() \ No newline at end of file +if __name__ == "__main__": + main() diff --git a/CustomRobots/tello_phy/tello_simple_teleop/test/test_copyright.py b/CustomRobots/tello_phy/tello_simple_teleop/test/test_copyright.py index 97a39196e..ceffe896d 100755 --- a/CustomRobots/tello_phy/tello_simple_teleop/test/test_copyright.py +++ b/CustomRobots/tello_phy/tello_simple_teleop/test/test_copyright.py @@ -17,9 +17,11 @@ # Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.skip( + reason="No copyright header has been placed in the generated source file." +) @pytest.mark.copyright @pytest.mark.linter def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/CustomRobots/tello_phy/tello_simple_teleop/test/test_flake8.py b/CustomRobots/tello_phy/tello_simple_teleop/test/test_flake8.py index 27ee1078f..ee79f31ac 100755 --- a/CustomRobots/tello_phy/tello_simple_teleop/test/test_flake8.py +++ b/CustomRobots/tello_phy/tello_simple_teleop/test/test_flake8.py @@ -20,6 +20,6 @@ @pytest.mark.linter def test_flake8(): rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) + assert rc == 0, "Found %d code style errors / warnings:\n" % len( + errors + ) + "\n".join(errors) diff --git a/CustomRobots/tello_phy/tello_simple_teleop/test/test_pep257.py b/CustomRobots/tello_phy/tello_simple_teleop/test/test_pep257.py index b234a3840..a2c3deb8e 100755 --- a/CustomRobots/tello_phy/tello_simple_teleop/test/test_pep257.py +++ b/CustomRobots/tello_phy/tello_simple_teleop/test/test_pep257.py @@ -19,5 +19,5 @@ @pytest.mark.linter @pytest.mark.pep257 def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings" diff --git a/Launchers/3d_reconstruction.launch.py b/Launchers/3d_reconstruction.launch.py index 6de9d7431..39d6be355 100755 --- a/Launchers/3d_reconstruction.launch.py +++ b/Launchers/3d_reconstruction.launch.py @@ -6,69 +6,79 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'kobuki_1_reconstruction3d.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "kobuki_1_reconstruction3d.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/follow_person.launch.py b/Launchers/follow_person.launch.py index 19767bd25..9204ab699 100644 --- a/Launchers/follow_person.launch.py +++ b/Launchers/follow_person.launch.py @@ -7,93 +7,113 @@ from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare + def generate_launch_description(): - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to the Turtlebot2 ROS package - pkg_share_dir = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set Turtlebot2 Arguments - x_turtlebot2_position = '0' - y_turtlebot2_position = '10' - z_turtlebot2_position = '0' - - declare_x_position_cmd = DeclareLaunchArgument( - '-x', default_value=x_turtlebot2_position, - description="Position on the axis x of Turtlebot2" - ) - declare_y_position_cmd = DeclareLaunchArgument( - '-y', default_value=y_turtlebot2_position, - description="Position on the axis y of Turtlebot2" - ) - declare_z_position_cmd = DeclareLaunchArgument( - '-z', default_value=z_turtlebot2_position, - description="Position on the axis z of Turtlebot2" - ) - - world_file_name = "hospital_follow_person.world" - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files - gazebo_models_path = os.path.join(pkg_share_dir, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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, 'pause': 'true'}.items()) - - start_turtlebot2_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(pkg_share_dir, 'launch', 'spawn_model.launch.py')), - launch_arguments = {'-x': x_turtlebot2_position, '-y': y_turtlebot2_position, '-z': z_turtlebot2_position}.items()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - ld.add_action(declare_x_position_cmd) - ld.add_action(declare_y_position_cmd) - ld.add_action(declare_z_position_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - ld.add_action(start_turtlebot2_cmd) - - return ld + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to the Turtlebot2 ROS package + pkg_share_dir = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set Turtlebot2 Arguments + x_turtlebot2_position = "0" + y_turtlebot2_position = "10" + z_turtlebot2_position = "0" + + declare_x_position_cmd = DeclareLaunchArgument( + "-x", + default_value=x_turtlebot2_position, + description="Position on the axis x of Turtlebot2", + ) + declare_y_position_cmd = DeclareLaunchArgument( + "-y", + default_value=y_turtlebot2_position, + description="Position on the axis y of Turtlebot2", + ) + declare_z_position_cmd = DeclareLaunchArgument( + "-z", + default_value=z_turtlebot2_position, + description="Position on the axis z of Turtlebot2", + ) + + world_file_name = "hospital_follow_person.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files + gazebo_models_path = os.path.join(pkg_share_dir, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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, "pause": "true"}.items(), + ) + + start_turtlebot2_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_share_dir, "launch", "spawn_model.launch.py") + ), + launch_arguments={ + "-x": x_turtlebot2_position, + "-y": y_turtlebot2_position, + "-z": z_turtlebot2_position, + }.items(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + ld.add_action(declare_x_position_cmd) + ld.add_action(declare_y_position_cmd) + ld.add_action(declare_z_position_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_turtlebot2_cmd) + + return ld diff --git a/Launchers/follow_person_followingcam.launch.py b/Launchers/follow_person_followingcam.launch.py index 4e972d0cb..a5925a467 100644 --- a/Launchers/follow_person_followingcam.launch.py +++ b/Launchers/follow_person_followingcam.launch.py @@ -7,93 +7,113 @@ from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare + def generate_launch_description(): - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to the Turtlebot2 ROS package - pkg_share_dir = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set Turtlebot2 Arguments - x_turtlebot2_position = '0' - y_turtlebot2_position = '10' - z_turtlebot2_position = '0' - - declare_x_position_cmd = DeclareLaunchArgument( - '-x', default_value=x_turtlebot2_position, - description="Position on the axis x of Turtlebot2" - ) - declare_y_position_cmd = DeclareLaunchArgument( - '-y', default_value=y_turtlebot2_position, - description="Position on the axis y of Turtlebot2" - ) - declare_z_position_cmd = DeclareLaunchArgument( - '-z', default_value=z_turtlebot2_position, - description="Position on the axis z of Turtlebot2" - ) - - world_file_name = "hospital_follow_person_followingcam.world" - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files - gazebo_models_path = os.path.join(pkg_share_dir, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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_turtlebot2_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(pkg_share_dir, 'launch', 'spawn_model.launch.py')), - launch_arguments = {'-x': x_turtlebot2_position, '-y': y_turtlebot2_position, '-z': z_turtlebot2_position}.items()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - ld.add_action(declare_x_position_cmd) - ld.add_action(declare_y_position_cmd) - ld.add_action(declare_z_position_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - ld.add_action(start_turtlebot2_cmd) - - return ld + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to the Turtlebot2 ROS package + pkg_share_dir = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set Turtlebot2 Arguments + x_turtlebot2_position = "0" + y_turtlebot2_position = "10" + z_turtlebot2_position = "0" + + declare_x_position_cmd = DeclareLaunchArgument( + "-x", + default_value=x_turtlebot2_position, + description="Position on the axis x of Turtlebot2", + ) + declare_y_position_cmd = DeclareLaunchArgument( + "-y", + default_value=y_turtlebot2_position, + description="Position on the axis y of Turtlebot2", + ) + declare_z_position_cmd = DeclareLaunchArgument( + "-z", + default_value=z_turtlebot2_position, + description="Position on the axis z of Turtlebot2", + ) + + world_file_name = "hospital_follow_person_followingcam.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files + gazebo_models_path = os.path.join(pkg_share_dir, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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_turtlebot2_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_share_dir, "launch", "spawn_model.launch.py") + ), + launch_arguments={ + "-x": x_turtlebot2_position, + "-y": y_turtlebot2_position, + "-z": z_turtlebot2_position, + }.items(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + ld.add_action(declare_x_position_cmd) + ld.add_action(declare_y_position_cmd) + ld.add_action(declare_z_position_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_turtlebot2_cmd) + + return ld diff --git a/Launchers/follow_road.launch.py b/Launchers/follow_road.launch.py index df6e96d00..97262639b 100755 --- a/Launchers/follow_road.launch.py +++ b/Launchers/follow_road.launch.py @@ -17,52 +17,61 @@ def generate_launch_description(): - custom_robots_share = get_package_share_directory('custom_robots') + custom_robots_share = get_package_share_directory("custom_robots") # same bridges file as rescue_people exercise - bridges_path = os.path.join(custom_robots_share, 'bridges', 'rescue_people.yaml') - world_path = os.path.join(custom_robots_share, 'worlds', 'follow_road_harmonic.world') + bridges_path = os.path.join(custom_robots_share, "bridges", "rescue_people.yaml") + world_path = os.path.join( + custom_robots_share, "worlds", "follow_road_harmonic.world" + ) ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## declare_use_simulator_cmd = DeclareLaunchArgument( - name='use_simulator', - default_value='True', - description='Whether to start the simulator') + name="use_simulator", + default_value="True", + description="Whether to start the simulator", + ) # Start Gazebo server gzsim = IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('jderobot_drones'), 'launch'), - '/gz_sim.launch.py']), - condition=IfCondition(LaunchConfiguration('use_simulator')), + PythonLaunchDescriptionSource( + [ + os.path.join(get_package_share_directory("jderobot_drones"), "launch"), + "/gz_sim.launch.py", + ] + ), + condition=IfCondition(LaunchConfiguration("use_simulator")), launch_arguments={ - 'namespace': 'drone0', - 'bridges_file': bridges_path, - 'world_file': world_path, + "namespace": "drone0", + "bridges_file": bridges_path, + "world_file": world_path, }.items(), ) as2 = IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('jderobot_drones'), 'launch'), - '/as2_default_gazebo_sim.launch.py']), + PythonLaunchDescriptionSource( + [ + os.path.join(get_package_share_directory("jderobot_drones"), "launch"), + "/as2_default_gazebo_sim.launch.py", + ] + ), launch_arguments={ - 'namespace': 'drone0', + "namespace": "drone0", }.items(), ) # TODO: push use_sim_time start_gazebo_frontal_image_bridge_cmd = Node( - package='ros_gz_image', - executable='image_bridge', - arguments=['/drone0/frontal_cam/image_raw'], - output='screen', + package="ros_gz_image", + executable="image_bridge", + arguments=["/drone0/frontal_cam/image_raw"], + output="screen", ) start_gazebo_ventral_image_bridge_cmd = Node( - package='ros_gz_image', - executable='image_bridge', - arguments=['/drone0/ventral_cam/image_raw'], - output='screen', + package="ros_gz_image", + executable="image_bridge", + arguments=["/drone0/ventral_cam/image_raw"], + output="screen", ) # Create the launch description and populate diff --git a/Launchers/gas_station_battery.launch.py b/Launchers/gas_station_battery.launch.py index 01857900e..f650af38a 100755 --- a/Launchers/gas_station_battery.launch.py +++ b/Launchers/gas_station_battery.launch.py @@ -6,69 +6,79 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'gas_station_battery.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "gas_station_battery.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/gas_station_battery_ackermann.launch.py b/Launchers/gas_station_battery_ackermann.launch.py index 5a7a0faf6..3a9858e77 100755 --- a/Launchers/gas_station_battery_ackermann.launch.py +++ b/Launchers/gas_station_battery_ackermann.launch.py @@ -6,69 +6,79 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'gas_station_battery_ackermann.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "gas_station_battery_ackermann.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/gas_station_line.launch.py b/Launchers/gas_station_line.launch.py index 68caeac98..0ed30eb68 100755 --- a/Launchers/gas_station_line.launch.py +++ b/Launchers/gas_station_line.launch.py @@ -6,69 +6,79 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'gas_station_line.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "gas_station_line.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/gas_station_line_ackermann.launch.py b/Launchers/gas_station_line_ackermann.launch.py index 7f09a5677..d58191ee0 100755 --- a/Launchers/gas_station_line_ackermann.launch.py +++ b/Launchers/gas_station_line_ackermann.launch.py @@ -6,69 +6,79 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'gas_station_line_ackermann.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "gas_station_line_ackermann.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/gas_station_parking.launch.py b/Launchers/gas_station_parking.launch.py index fa301e73a..2f3cc2d3a 100755 --- a/Launchers/gas_station_parking.launch.py +++ b/Launchers/gas_station_parking.launch.py @@ -6,69 +6,79 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'gas_station_parking.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "gas_station_parking.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/gas_station_parking_ackermann.launch.py b/Launchers/gas_station_parking_ackermann.launch.py index 787cae21c..24e74e3d5 100755 --- a/Launchers/gas_station_parking_ackermann.launch.py +++ b/Launchers/gas_station_parking_ackermann.launch.py @@ -6,69 +6,79 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'gas_station_parking_ackermann.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "gas_station_parking_ackermann.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/laser_mapping.launch.py b/Launchers/laser_mapping.launch.py index 98eb66665..0efef7d58 100644 --- a/Launchers/laser_mapping.launch.py +++ b/Launchers/laser_mapping.launch.py @@ -7,7 +7,7 @@ DeclareLaunchArgument, IncludeLaunchDescription, SetEnvironmentVariable, - AppendEnvironmentVariable + AppendEnvironmentVariable, ) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, Command @@ -18,89 +18,78 @@ def generate_launch_description(): - x = LaunchConfiguration('x') - y = LaunchConfiguration('y') - z = LaunchConfiguration('z') - roll = LaunchConfiguration('R') - pitch = LaunchConfiguration('P') - yaw = LaunchConfiguration('Y') + x = LaunchConfiguration("x") + y = LaunchConfiguration("y") + z = LaunchConfiguration("z") + roll = LaunchConfiguration("R") + pitch = LaunchConfiguration("P") + yaw = LaunchConfiguration("Y") - package_dir = get_package_share_directory('custom_robots') - ros_gz_sim = get_package_share_directory('ros_gz_sim') + package_dir = get_package_share_directory("custom_robots") + ros_gz_sim = get_package_share_directory("ros_gz_sim") gazebo_models_path = os.path.join(package_dir, "models") robot_launch_dir = "/opt/jderobot/Launchers/marker_visual_loc" - robot_model_dir = os.path.join(package_dir, 'models/turtlebot3_waffle') - - use_sim_time = LaunchConfiguration('use_sim_time', default='true') - x_pose = LaunchConfiguration('x_pose', default='14.25') - y_pose = LaunchConfiguration('y_pose', default='-10.75') - z_pose = LaunchConfiguration('z_pose', default='0.0') + robot_model_dir = os.path.join(package_dir, "models/turtlebot3_waffle") + + use_sim_time = LaunchConfiguration("use_sim_time", default="true") + x_pose = LaunchConfiguration("x_pose", default="14.25") + y_pose = LaunchConfiguration("y_pose", default="-10.75") + z_pose = LaunchConfiguration("z_pose", default="0.0") world_file_name = "laser_mapping.world" worlds_dir = "/opt/jderobot/Worlds" world_path = os.path.join(worlds_dir, world_file_name) gazebo_server = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(ros_gz_sim, 'launch', 'gz_sim.launch.py')), - launch_arguments={'gz_args': ['-r -s -v4 ', world_path], 'on_exit_shutdown': 'true'}.items() + os.path.join(ros_gz_sim, "launch", "gz_sim.launch.py") + ), + launch_arguments={ + "gz_args": ["-r -s -v4 ", world_path], + "on_exit_shutdown": "true", + }.items(), ) - declare_x_cmd = DeclareLaunchArgument( - 'x', default_value='14.25' - ) + declare_x_cmd = DeclareLaunchArgument("x", default_value="14.25") - declare_y_cmd = DeclareLaunchArgument( - 'y', default_value='-10.75' - ) + declare_y_cmd = DeclareLaunchArgument("y", default_value="-10.75") - declare_z_cmd = DeclareLaunchArgument( - 'z', default_value='0.0' - ) + declare_z_cmd = DeclareLaunchArgument("z", default_value="0.0") - declare_roll_cmd = DeclareLaunchArgument( - 'R', default_value='0.0' - ) + declare_roll_cmd = DeclareLaunchArgument("R", default_value="0.0") - declare_pitch_cmd = DeclareLaunchArgument( - 'P', default_value='0.0' - ) + declare_pitch_cmd = DeclareLaunchArgument("P", default_value="0.0") - declare_yaw_cmd = DeclareLaunchArgument( - 'Y', default_value='0.0' - ) + declare_yaw_cmd = DeclareLaunchArgument("Y", default_value="0.0") robot_state_publisher_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(robot_launch_dir, 'robot_state_publisher.launch.py') + os.path.join(robot_launch_dir, "robot_state_publisher.launch.py") ), - launch_arguments={'use_sim_time': use_sim_time}.items() + launch_arguments={"use_sim_time": use_sim_time}.items(), ) spawn_robot_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(robot_launch_dir, 'spawn_robot.launch.py') + os.path.join(robot_launch_dir, "spawn_robot.launch.py") ), - launch_arguments={ - 'x_pose': x_pose, - 'y_pose': y_pose, - 'z_pose': z_pose - }.items() + launch_arguments={"x_pose": x_pose, "y_pose": y_pose, "z_pose": z_pose}.items(), ) - world_entity_cmd = Node(package='ros_gz_sim', executable='create', - arguments=['-name', - 'world', - '-file', - world_path - ], - output='screen') + world_entity_cmd = Node( + package="ros_gz_sim", + executable="create", + arguments=["-name", "world", "-file", world_path], + output="screen", + ) ld = LaunchDescription() - ld.add_action(SetEnvironmentVariable('GZ_SIM_RESOURCE_PATH', gazebo_models_path)) - set_env_vars_resources = AppendEnvironmentVariable('GZ_SIM_RESOURCE_PATH', os.path.join(package_dir,'models')) + ld.add_action(SetEnvironmentVariable("GZ_SIM_RESOURCE_PATH", gazebo_models_path)) + set_env_vars_resources = AppendEnvironmentVariable( + "GZ_SIM_RESOURCE_PATH", os.path.join(package_dir, "models") + ) ld.add_action(set_env_vars_resources) ld.add_action(gazebo_server) # ld.add_action(gazebo_client) diff --git a/Launchers/marker_visual_loc.launch.py b/Launchers/marker_visual_loc.launch.py index e59c2ef29..bac40bdd4 100644 --- a/Launchers/marker_visual_loc.launch.py +++ b/Launchers/marker_visual_loc.launch.py @@ -7,7 +7,7 @@ DeclareLaunchArgument, IncludeLaunchDescription, SetEnvironmentVariable, - AppendEnvironmentVariable + AppendEnvironmentVariable, ) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, Command @@ -18,89 +18,78 @@ def generate_launch_description(): - x = LaunchConfiguration('x') - y = LaunchConfiguration('y') - z = LaunchConfiguration('z') - roll = LaunchConfiguration('R') - pitch = LaunchConfiguration('P') - yaw = LaunchConfiguration('Y') + x = LaunchConfiguration("x") + y = LaunchConfiguration("y") + z = LaunchConfiguration("z") + roll = LaunchConfiguration("R") + pitch = LaunchConfiguration("P") + yaw = LaunchConfiguration("Y") - package_dir = get_package_share_directory('custom_robots') - ros_gz_sim = get_package_share_directory('ros_gz_sim') + package_dir = get_package_share_directory("custom_robots") + ros_gz_sim = get_package_share_directory("ros_gz_sim") gazebo_models_path = os.path.join(package_dir, "models") robot_launch_dir = "/opt/jderobot/Launchers/marker_visual_loc" - robot_model_dir = os.path.join(package_dir, 'models/turtlebot3_waffle') - - use_sim_time = LaunchConfiguration('use_sim_time', default='true') - x_pose = LaunchConfiguration('x_pose', default='1.0') - y_pose = LaunchConfiguration('y_pose', default='-1.5') - z_pose = LaunchConfiguration('z_pose', default='7.1') + robot_model_dir = os.path.join(package_dir, "models/turtlebot3_waffle") + + use_sim_time = LaunchConfiguration("use_sim_time", default="true") + x_pose = LaunchConfiguration("x_pose", default="1.0") + y_pose = LaunchConfiguration("y_pose", default="-1.5") + z_pose = LaunchConfiguration("z_pose", default="7.1") world_file_name = "marker_visual_loc.world" worlds_dir = "/opt/jderobot/Worlds" world_path = os.path.join(worlds_dir, world_file_name) gazebo_server = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(ros_gz_sim, 'launch', 'gz_sim.launch.py')), - launch_arguments={'gz_args': ['-r -s -v4 ', world_path], 'on_exit_shutdown': 'true'}.items() + os.path.join(ros_gz_sim, "launch", "gz_sim.launch.py") + ), + launch_arguments={ + "gz_args": ["-r -s -v4 ", world_path], + "on_exit_shutdown": "true", + }.items(), ) - declare_x_cmd = DeclareLaunchArgument( - 'x', default_value='1.0' - ) + declare_x_cmd = DeclareLaunchArgument("x", default_value="1.0") - declare_y_cmd = DeclareLaunchArgument( - 'y', default_value='-1.5' - ) + declare_y_cmd = DeclareLaunchArgument("y", default_value="-1.5") - declare_z_cmd = DeclareLaunchArgument( - 'z', default_value='7.1' - ) + declare_z_cmd = DeclareLaunchArgument("z", default_value="7.1") - declare_roll_cmd = DeclareLaunchArgument( - 'R', default_value='0.0' - ) + declare_roll_cmd = DeclareLaunchArgument("R", default_value="0.0") - declare_pitch_cmd = DeclareLaunchArgument( - 'P', default_value='0.0' - ) + declare_pitch_cmd = DeclareLaunchArgument("P", default_value="0.0") - declare_yaw_cmd = DeclareLaunchArgument( - 'Y', default_value='1.57079' - ) + declare_yaw_cmd = DeclareLaunchArgument("Y", default_value="1.57079") robot_state_publisher_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(robot_launch_dir, 'robot_state_publisher.launch.py') + os.path.join(robot_launch_dir, "robot_state_publisher.launch.py") ), - launch_arguments={'use_sim_time': use_sim_time}.items() + launch_arguments={"use_sim_time": use_sim_time}.items(), ) spawn_robot_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(robot_launch_dir, 'spawn_robot.launch.py') + os.path.join(robot_launch_dir, "spawn_robot.launch.py") ), - launch_arguments={ - 'x_pose': x_pose, - 'y_pose': y_pose, - 'z_pose': z_pose - }.items() + launch_arguments={"x_pose": x_pose, "y_pose": y_pose, "z_pose": z_pose}.items(), ) - world_entity_cmd = Node(package='ros_gz_sim', executable='create', - arguments=['-name', - 'world', - '-file', - world_path - ], - output='screen') + world_entity_cmd = Node( + package="ros_gz_sim", + executable="create", + arguments=["-name", "world", "-file", world_path], + output="screen", + ) ld = LaunchDescription() - ld.add_action(SetEnvironmentVariable('GZ_SIM_RESOURCE_PATH', gazebo_models_path)) - set_env_vars_resources = AppendEnvironmentVariable('GZ_SIM_RESOURCE_PATH', os.path.join(package_dir,'models')) + ld.add_action(SetEnvironmentVariable("GZ_SIM_RESOURCE_PATH", gazebo_models_path)) + set_env_vars_resources = AppendEnvironmentVariable( + "GZ_SIM_RESOURCE_PATH", os.path.join(package_dir, "models") + ) ld.add_action(set_env_vars_resources) ld.add_action(gazebo_server) # ld.add_action(gazebo_client) @@ -114,4 +103,4 @@ def generate_launch_description(): ld.add_action(robot_state_publisher_cmd) ld.add_action(spawn_robot_cmd) - return ld \ No newline at end of file + return ld diff --git a/Launchers/marker_visual_loc/robot_state_publisher.launch.py b/Launchers/marker_visual_loc/robot_state_publisher.launch.py index 86b094cd0..731207c12 100644 --- a/Launchers/marker_visual_loc/robot_state_publisher.launch.py +++ b/Launchers/marker_visual_loc/robot_state_publisher.launch.py @@ -26,33 +26,33 @@ def generate_launch_description(): - use_sim_time = LaunchConfiguration('use_sim_time', default='true') - urdf_file_name = 'turtlebot3_waffle.urdf' + use_sim_time = LaunchConfiguration("use_sim_time", default="true") + urdf_file_name = "turtlebot3_waffle.urdf" - print('urdf_file_name : {}'.format(urdf_file_name)) + print("urdf_file_name : {}".format(urdf_file_name)) urdf_path = os.path.join( - get_package_share_directory('custom_robots'), - 'urdf', - urdf_file_name) + get_package_share_directory("custom_robots"), "urdf", urdf_file_name + ) - with open(urdf_path, 'r') as infp: + with open(urdf_path, "r") as infp: robot_desc = infp.read() - return LaunchDescription([ - DeclareLaunchArgument( - 'use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true'), - - Node( - package='robot_state_publisher', - executable='robot_state_publisher', - name='robot_state_publisher', - output='screen', - parameters=[{ - 'use_sim_time': use_sim_time, - 'robot_description': robot_desc - }], - ), - ]) \ No newline at end of file + return LaunchDescription( + [ + DeclareLaunchArgument( + "use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + parameters=[ + {"use_sim_time": use_sim_time, "robot_description": robot_desc} + ], + ), + ] + ) diff --git a/Launchers/marker_visual_loc/spawn_robot.launch.py b/Launchers/marker_visual_loc/spawn_robot.launch.py index d7fe201f9..5cf708d02 100644 --- a/Launchers/marker_visual_loc/spawn_robot.launch.py +++ b/Launchers/marker_visual_loc/spawn_robot.launch.py @@ -23,12 +23,12 @@ def generate_launch_description(): # Get the urdf file - model_folder = 'turtlebot3_waffle' + model_folder = "turtlebot3_waffle" urdf_path = os.path.join( - get_package_share_directory('custom_robots'), - 'models', + get_package_share_directory("custom_robots"), + "models", model_folder, - 'model.sdf' + "model.sdf", ) # Launch configuration variables specific to simulation @@ -44,7 +44,7 @@ def generate_launch_description(): # declare_y_position_cmd = DeclareLaunchArgument( # 'y_pose', default_value='-1.5', # description='Specify namespace of the robot') - + # declare_z_position_cmd = DeclareLaunchArgument( # 'z_pose', default_value='7.1', # description='Specify namespace of the robot') @@ -63,34 +63,32 @@ def generate_launch_description(): # ) bridge_params = os.path.join( - get_package_share_directory('custom_robots'), - 'params', - 'robot_params.yaml' + get_package_share_directory("custom_robots"), "params", "robot_params.yaml" ) start_gazebo_ros_bridge_cmd = Node( - package='ros_gz_bridge', - executable='parameter_bridge', + package="ros_gz_bridge", + executable="parameter_bridge", arguments=[ - '--ros-args', - '-p', - f'config_file:={bridge_params}', + "--ros-args", + "-p", + f"config_file:={bridge_params}", ], - output='screen', + output="screen", ) start_gazebo_ros_image_bridge_cmd = Node( - package='ros_gz_image', - executable='image_bridge', - arguments=['/turtlebot3/camera/image_raw'], - output='screen', + package="ros_gz_image", + executable="image_bridge", + arguments=["/turtlebot3/camera/image_raw"], + output="screen", ) start_gazebo_ros_depth_bridge_cmd = Node( - package='ros_gz_image', - executable='image_bridge', - arguments=['/turtlebot3/camera/depth'], - output='screen', + package="ros_gz_image", + executable="image_bridge", + arguments=["/turtlebot3/camera/depth"], + output="screen", ) ld = LaunchDescription() @@ -106,4 +104,4 @@ def generate_launch_description(): ld.add_action(start_gazebo_ros_image_bridge_cmd) ld.add_action(start_gazebo_ros_depth_bridge_cmd) - return ld \ No newline at end of file + return ld diff --git a/Launchers/montecarlo_laser_loc.launch.py b/Launchers/montecarlo_laser_loc.launch.py index 14ca27ac2..370281195 100755 --- a/Launchers/montecarlo_laser_loc.launch.py +++ b/Launchers/montecarlo_laser_loc.launch.py @@ -7,68 +7,78 @@ from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare + def generate_launch_description(): - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to custom_robots package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'roomba_1_house.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to custom_robots package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "roomba_1_house.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/montecarlo_laser_loc_headless.launch.py b/Launchers/montecarlo_laser_loc_headless.launch.py index ddc5c6f7a..1b04e4d12 100755 --- a/Launchers/montecarlo_laser_loc_headless.launch.py +++ b/Launchers/montecarlo_laser_loc_headless.launch.py @@ -7,68 +7,76 @@ from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare + def generate_launch_description(): - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to custom_robots package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'roomba_1_house.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='True', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to custom_robots package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "roomba_1_house.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", default_value="True", description="Whether to execute gzclient" + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/montecarlo_visual_loc.launch.py b/Launchers/montecarlo_visual_loc.launch.py index c098b5ab5..4ec03dbd9 100755 --- a/Launchers/montecarlo_visual_loc.launch.py +++ b/Launchers/montecarlo_visual_loc.launch.py @@ -7,68 +7,78 @@ from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare + def generate_launch_description(): - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to custom_robots package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'roomba_1_house_montecarlo_visual_loc.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to custom_robots package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "roomba_1_house_montecarlo_visual_loc.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/montmelo_circuit.launch.py b/Launchers/montmelo_circuit.launch.py index 384b6a93a..9b66fb26c 100755 --- a/Launchers/montmelo_circuit.launch.py +++ b/Launchers/montmelo_circuit.launch.py @@ -6,69 +6,79 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'montmelo_line.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "montmelo_line.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/montmelo_circuit_ackermann.launch.py b/Launchers/montmelo_circuit_ackermann.launch.py index 8a5bd5f94..834bf8595 100755 --- a/Launchers/montmelo_circuit_ackermann.launch.py +++ b/Launchers/montmelo_circuit_ackermann.launch.py @@ -6,69 +6,79 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'montmelo_line_ackermann.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "montmelo_line_ackermann.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/montmelo_circuit_ackermann_followingcam.launch.py b/Launchers/montmelo_circuit_ackermann_followingcam.launch.py index fb4b22314..b493fd18a 100755 --- a/Launchers/montmelo_circuit_ackermann_followingcam.launch.py +++ b/Launchers/montmelo_circuit_ackermann_followingcam.launch.py @@ -6,69 +6,79 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'montmelo_line_ackermann_followingcam.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "montmelo_line_ackermann_followingcam.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/montmelo_circuit_followingcam.launch.py b/Launchers/montmelo_circuit_followingcam.launch.py index e94449c0e..45dcd3565 100755 --- a/Launchers/montmelo_circuit_followingcam.launch.py +++ b/Launchers/montmelo_circuit_followingcam.launch.py @@ -6,69 +6,79 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'montmelo_line_followingcam.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "montmelo_line_followingcam.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/montreal_circuit.launch.py b/Launchers/montreal_circuit.launch.py index 86ffafc1d..db272b819 100755 --- a/Launchers/montreal_circuit.launch.py +++ b/Launchers/montreal_circuit.launch.py @@ -6,69 +6,79 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'montreal_line.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "montreal_line.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/montreal_circuit_ackermann.launch.py b/Launchers/montreal_circuit_ackermann.launch.py index 03f595537..ad73b6cbe 100755 --- a/Launchers/montreal_circuit_ackermann.launch.py +++ b/Launchers/montreal_circuit_ackermann.launch.py @@ -6,69 +6,79 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'montreal_line_ackermann.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "montreal_line_ackermann.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/montreal_circuit_ackermann_followingcam.launch.py b/Launchers/montreal_circuit_ackermann_followingcam.launch.py index 6d89ea2ee..f241fb4a4 100755 --- a/Launchers/montreal_circuit_ackermann_followingcam.launch.py +++ b/Launchers/montreal_circuit_ackermann_followingcam.launch.py @@ -6,69 +6,79 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'montreal_line_ackermann_followingcam.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "montreal_line_ackermann_followingcam.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/montreal_circuit_followingcam.launch.py b/Launchers/montreal_circuit_followingcam.launch.py index 2912e3988..a3a1afbe8 100755 --- a/Launchers/montreal_circuit_followingcam.launch.py +++ b/Launchers/montreal_circuit_followingcam.launch.py @@ -6,69 +6,79 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'montreal_line_followingcam.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "montreal_line_followingcam.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/nurburgring_circuit.launch.py b/Launchers/nurburgring_circuit.launch.py index e6517654c..6ddbc6941 100755 --- a/Launchers/nurburgring_circuit.launch.py +++ b/Launchers/nurburgring_circuit.launch.py @@ -6,69 +6,79 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'nurburgring_line.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "nurburgring_line.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/nurburgring_circuit_ackermann.launch.py b/Launchers/nurburgring_circuit_ackermann.launch.py index c3ff77afa..66e8b8cb2 100755 --- a/Launchers/nurburgring_circuit_ackermann.launch.py +++ b/Launchers/nurburgring_circuit_ackermann.launch.py @@ -6,69 +6,79 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'nurburgring_line_ackermann.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "nurburgring_line_ackermann.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/nurburgring_circuit_ackermann_followingcam.launch.py b/Launchers/nurburgring_circuit_ackermann_followingcam.launch.py index 340b77545..b8d35b149 100755 --- a/Launchers/nurburgring_circuit_ackermann_followingcam.launch.py +++ b/Launchers/nurburgring_circuit_ackermann_followingcam.launch.py @@ -6,69 +6,79 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'nurburgring_line_ackermann_followingcam.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "nurburgring_line_ackermann_followingcam.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/nurburgring_circuit_followingcam.launch.py b/Launchers/nurburgring_circuit_followingcam.launch.py index 68beac1b9..9a616b2c7 100755 --- a/Launchers/nurburgring_circuit_followingcam.launch.py +++ b/Launchers/nurburgring_circuit_followingcam.launch.py @@ -6,69 +6,79 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'nurburgring_line_followingcam.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "nurburgring_line_followingcam.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/pallet_warehouse.launch.py b/Launchers/pallet_warehouse.launch.py index 4cb793abd..2cc2257fc 100644 --- a/Launchers/pallet_warehouse.launch.py +++ b/Launchers/pallet_warehouse.launch.py @@ -6,69 +6,79 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'pallet_warehouse_with_robot.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "pallet_warehouse_with_robot.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/pallet_warehouse_with_ackermann_logistic_robot.launch.py b/Launchers/pallet_warehouse_with_ackermann_logistic_robot.launch.py index dac7c8ea0..2498edb63 100644 --- a/Launchers/pallet_warehouse_with_ackermann_logistic_robot.launch.py +++ b/Launchers/pallet_warehouse_with_ackermann_logistic_robot.launch.py @@ -6,69 +6,79 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'pallet_warehouse_with_ackermann_logistic_robot.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "pallet_warehouse_with_ackermann_logistic_robot.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/pick_place.launch.py b/Launchers/pick_place.launch.py index a97be74b4..377fe7a4d 100644 --- a/Launchers/pick_place.launch.py +++ b/Launchers/pick_place.launch.py @@ -33,55 +33,64 @@ # Import libraries: import os, sys, xacro, yaml -from ament_index_python.packages import get_package_share_directory, PackageNotFoundError +from ament_index_python.packages import ( + get_package_share_directory, + PackageNotFoundError, +) from launch import LaunchDescription from launch_ros.actions import Node from launch.actions import IncludeLaunchDescription, RegisterEventHandler, TimerAction from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource + # LOAD FILE: def load_file(package_name, file_path): package_path = get_package_share_directory(package_name) absolute_file_path = os.path.join(package_path, file_path) try: - with open(absolute_file_path, 'r') as file: + with open(absolute_file_path, "r") as file: return file.read() except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available. return None + + # LOAD YAML: def load_yaml(package_name, file_path): package_path = get_package_share_directory(package_name) absolute_file_path = os.path.join(package_path, file_path) try: - with open(absolute_file_path, 'r') as file: + with open(absolute_file_path, "r") as file: return yaml.safe_load(file) except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available. return None + # ===== REQUIRED TO GET THE ROBOT CONFIGURATION === # + # EVALUATE INPUT ARGUMENTS: def AssignArgument(ARGUMENT): ARGUMENTS = sys.argv for y in ARGUMENTS: if (ARGUMENT + ":=") in y: - ARG = y.replace((ARGUMENT + ":="),"") - return(ARG) + ARG = y.replace((ARGUMENT + ":="), "") + return ARG + # GET CONFIGURATION from YAML: def GetCONFIG(CONFIGURATION, PKG_PATH): - + RESULT = {"Success": False, "ID": "", "Name": "", "urdf": "", "ee": ""} - + YAML_PATH = PKG_PATH + "/config/configurations.yaml" - + if not os.path.exists(YAML_PATH): - return (RESULT) - - with open(YAML_PATH, 'r') as YAML: + return RESULT + + with open(YAML_PATH, "r") as YAML: cYAML = yaml.safe_load(YAML) for x in cYAML["Configurations"]: @@ -94,44 +103,63 @@ def GetCONFIG(CONFIGURATION, PKG_PATH): RESULT["rob"] = x["rob"] RESULT["ee"] = x["ee"] - return(RESULT) + return RESULT + # GET EE-Controllers LIST: def GetEEctr(EEName): - + RESULT = [] - PATH = os.path.join(os.path.expanduser('~'), 'dev_ws', 'src', 'ros2_SimRealRobotControl', 'ros2srrc_endeffectors', EEName, 'config') + PATH = os.path.join( + os.path.expanduser("~"), + "dev_ws", + "src", + "ros2_SimRealRobotControl", + "ros2srrc_endeffectors", + EEName, + "config", + ) YAML_PATH = PATH + "/controller_moveit2.yaml" - - with open(YAML_PATH, 'r') as YAML: + + with open(YAML_PATH, "r") as YAML: cYAML = yaml.safe_load(YAML) for x in cYAML["controller_names"]: RESULT.append(x) - return(RESULT) + return RESULT + # CHECK if CONTROLLER file exists for EE: def EEctrlEXISTS(EEName): - - PATH = os.path.join(os.path.expanduser('~'), 'dev_ws', 'src', 'ros2_SimRealRobotControl', 'ros2srrc_endeffectors', EEName, 'config') + + PATH = os.path.join( + os.path.expanduser("~"), + "dev_ws", + "src", + "ros2_SimRealRobotControl", + "ros2srrc_endeffectors", + EEName, + "config", + ) YAML_PATH = PATH + "/controller.yaml" - + RES = os.path.exists(YAML_PATH) - return(RES) + return RES + # ========== **GENERATE LAUNCH DESCRIPTION** ========== # def generate_launch_description(): LD = LaunchDescription() - + # === INPUT ARGUMENT: ROS 2 PACKAGE === # - - #PACKAGE_NAME = AssignArgument("package") + + # PACKAGE_NAME = AssignArgument("package") # DMM: fixed UR5 PACKAGE_NAME = "ros2srrc_ur5" - + if PACKAGE_NAME != None: None else: @@ -139,7 +167,7 @@ def generate_launch_description(): print("ERROR: package INPUT ARGUMENT has not been defined. Please try again.") print("Closing... BYE!") exit() - + # CHECK if -> PACKAGE EXISTS, and GET PATH: try: PKG_PATH = get_package_share_directory(PACKAGE_NAME + "_gazebo") @@ -153,7 +181,7 @@ def generate_launch_description(): print("ERROR: The defined ROS 2 Package name is not valid. Please try again.") print("Closing... BYE!") exit() - + # === INPUT ARGUMENT: CONFIGURATION === # # CONFIG = AssignArgument("config") # DMM fixed UR5 + Robotiq 85 @@ -162,9 +190,11 @@ def generate_launch_description(): if CONFIGURATION["Success"] == False: print("") - print("ERROR: config INPUT ARGUMENT has not been correctly defined. Please try again.") + print( + "ERROR: config INPUT ARGUMENT has not been correctly defined. Please try again." + ) print("Closing... BYE!") - exit() + exit() # === INPUT ARGUMENT: HMI === # HMI = AssignArgument("hmi") @@ -175,59 +205,71 @@ def generate_launch_description(): # ========== CELL INFORMATION ========== # print("") - print("===== GAZEBO: Robot Simulation + MoveIt!2 Framework (" + PACKAGE_NAME + "_moveit2) =====") + print( + "===== GAZEBO: Robot Simulation + MoveIt!2 Framework (" + + PACKAGE_NAME + + "_moveit2) =====" + ) print("Robot configuration:") print(CONFIGURATION["ID"] + " -> " + CONFIGURATION["Name"]) print("") - - # ***** GAZEBO ***** # + + # ***** GAZEBO ***** # # DECLARE Gazebo WORLD file: robot_gazebo = os.path.join( - get_package_share_directory(PACKAGE_NAME + '_gazebo'), - 'worlds', - PACKAGE_NAME + '.world') + get_package_share_directory(PACKAGE_NAME + "_gazebo"), + "worlds", + PACKAGE_NAME + ".world", + ) # DECLARE Gazebo LAUNCH file: gazebo = IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join(get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']), - launch_arguments={'world': robot_gazebo}.items(), - ) + PythonLaunchDescriptionSource( + [ + os.path.join(get_package_share_directory("gazebo_ros"), "launch"), + "/gazebo.launch.py", + ] + ), + launch_arguments={"world": robot_gazebo}.items(), + ) # ***** ROBOT DESCRIPTION ***** # # Robot Description file package: - robot_description_path = os.path.join(get_package_share_directory(PACKAGE_NAME + '_gazebo')) + robot_description_path = os.path.join( + get_package_share_directory(PACKAGE_NAME + "_gazebo") + ) # ROBOT urdf file path: - xacro_file = os.path.join(robot_description_path,'urdf',CONFIGURATION["urdf"]) + xacro_file = os.path.join(robot_description_path, "urdf", CONFIGURATION["urdf"]) # Generate ROBOT_DESCRIPTION variable: doc = xacro.parse(open(xacro_file)) - + if CONFIGURATION["ee"] == "none": EE = "false" - else: + else: EE = "true" - - xacro.process_doc(doc, mappings={ - "EE": EE, - "EE_name": CONFIGURATION["ee"], - "hmi": HMI, - }) - + + xacro.process_doc( + doc, + mappings={ + "EE": EE, + "EE_name": CONFIGURATION["ee"], + "hmi": HMI, + }, + ) + # EE -> Controller file needed? if EE == "true": if EEctrlEXISTS(CONFIGURATION["ee"]) == False: EE = "true-NOctr" - + robot_description_config = doc.toxml() - robot_description = {'robot_description': robot_description_config} + robot_description = {"robot_description": robot_description_config} # ROBOT STATE PUBLISHER NODE: node_robot_state_publisher = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - output='both', - parameters=[ - robot_description, - {"use_sim_time": True} - ] + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description, {"use_sim_time": True}], ) static_tf = Node( package="tf2_ros", @@ -238,16 +280,23 @@ def generate_launch_description(): ) # SPAWN ROBOT TO GAZEBO: - spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py', - arguments=['-topic', 'robot_description', '-entity', CONFIGURATION["rob"]], - output='both') + spawn_entity = Node( + package="gazebo_ros", + executable="spawn_entity.py", + arguments=["-topic", "robot_description", "-entity", CONFIGURATION["rob"]], + output="both", + ) # ***** CONTROLLERS ***** # # Joint STATE BROADCASTER: joint_state_broadcaster_spawner = Node( package="controller_manager", executable="spawner", - arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + arguments=[ + "joint_state_broadcaster", + "--controller-manager", + "/controller_manager", + ], ) # Joint TRAJECTORY Controller: joint_trajectory_controller_spawner = Node( @@ -270,31 +319,46 @@ def generate_launch_description(): ) ) - # *********************** MoveIt!2 *********************** # + # *********************** MoveIt!2 *********************** # # *** PLANNING CONTEXT *** # # Robot description, SRDF: - if (EE == "false"): - robot_description_semantic_config = load_file(PACKAGE_NAME + "_moveit2", "config/" + CONFIGURATION["rob"] + ".srdf") + if EE == "false": + robot_description_semantic_config = load_file( + PACKAGE_NAME + "_moveit2", "config/" + CONFIGURATION["rob"] + ".srdf" + ) else: - robot_description_semantic_config = load_file(PACKAGE_NAME + "_moveit2", "config/" + CONFIGURATION["rob"] + CONFIGURATION["ee"] + ".srdf") - - robot_description_semantic = {"robot_description_semantic": robot_description_semantic_config} + robot_description_semantic_config = load_file( + PACKAGE_NAME + "_moveit2", + "config/" + CONFIGURATION["rob"] + CONFIGURATION["ee"] + ".srdf", + ) + + robot_description_semantic = { + "robot_description_semantic": robot_description_semantic_config + } # Kinematics.yaml file: - kinematics_yaml = load_yaml("ros2srrc_robots", CONFIGURATION["rob"] + "/config/kinematics.yaml") + kinematics_yaml = load_yaml( + "ros2srrc_robots", CONFIGURATION["rob"] + "/config/kinematics.yaml" + ) robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml} # joint_limits.yaml file: if (EE == "false") or (EE == "true-NOctr"): - joint_limits_yaml = load_yaml("ros2srrc_robots", CONFIGURATION["rob"] + "/config/joint_limits.yaml") + joint_limits_yaml = load_yaml( + "ros2srrc_robots", CONFIGURATION["rob"] + "/config/joint_limits.yaml" + ) else: - YAML_ROB = load_yaml("ros2srrc_robots", CONFIGURATION["rob"] + "/config/joint_limits.yaml")["joint_limits"] - YAML_EE = load_yaml("ros2srrc_endeffectors", CONFIGURATION["ee"] + "/config/joint_limits.yaml")["joint_limits"] + YAML_ROB = load_yaml( + "ros2srrc_robots", CONFIGURATION["rob"] + "/config/joint_limits.yaml" + )["joint_limits"] + YAML_EE = load_yaml( + "ros2srrc_endeffectors", CONFIGURATION["ee"] + "/config/joint_limits.yaml" + )["joint_limits"] joint_limits_yaml = {} joint_limits_yaml["joint_limits"] = YAML_ROB | YAML_EE - - joint_limits = {'robot_description_planning': joint_limits_yaml} + + joint_limits = {"robot_description_planning": joint_limits_yaml} # pilz_planning_pipeline_config.yaml file: pilz_planning_pipeline_config = { @@ -305,15 +369,24 @@ def generate_launch_description(): "default_planner_config": "PTP", } } - pilz_cartesian_limits_yaml = load_yaml("ros2srrc_robots", CONFIGURATION["rob"] + "/config/pilz_cartesian_limits.yaml") - pilz_cartesian_limits = {'robot_description_planning': pilz_cartesian_limits_yaml} + pilz_cartesian_limits_yaml = load_yaml( + "ros2srrc_robots", CONFIGURATION["rob"] + "/config/pilz_cartesian_limits.yaml" + ) + pilz_cartesian_limits = {"robot_description_planning": pilz_cartesian_limits_yaml} # MoveIt!2 Controllers: if (EE == "false") or (EE == "true-NOctr"): - moveit_simple_controllers_yaml = load_yaml("ros2srrc_robots", CONFIGURATION["rob"] + "/config/controller_moveit2.yaml") + moveit_simple_controllers_yaml = load_yaml( + "ros2srrc_robots", CONFIGURATION["rob"] + "/config/controller_moveit2.yaml" + ) else: - YAML_ROB = load_yaml("ros2srrc_robots", CONFIGURATION["rob"] + "/config/controller_moveit2.yaml") - YAML_EE = load_yaml("ros2srrc_endeffectors", CONFIGURATION["ee"] + "/config/controller_moveit2.yaml") + YAML_ROB = load_yaml( + "ros2srrc_robots", CONFIGURATION["rob"] + "/config/controller_moveit2.yaml" + ) + YAML_EE = load_yaml( + "ros2srrc_endeffectors", + CONFIGURATION["ee"] + "/config/controller_moveit2.yaml", + ) for x in YAML_ROB["controller_names"]: YAML_EE["controller_names"].append(x) moveit_simple_controllers_yaml = YAML_ROB | YAML_EE @@ -349,12 +422,9 @@ def generate_launch_description(): robot_description, robot_description_semantic, kinematics_yaml, - pilz_planning_pipeline_config, - joint_limits, pilz_cartesian_limits, - trajectory_execution, moveit_controllers, planning_scene_monitor_parameters, @@ -364,11 +434,17 @@ def generate_launch_description(): ) # RVIZ: - rviz_base = os.path.join(get_package_share_directory(PACKAGE_NAME + "_moveit2"), "config") + rviz_base = os.path.join( + get_package_share_directory(PACKAGE_NAME + "_moveit2"), "config" + ) if EE == "false": - rviz_full_config = os.path.join(rviz_base, CONFIGURATION["rob"] + "_moveit2.rviz") + rviz_full_config = os.path.join( + rviz_base, CONFIGURATION["rob"] + "_moveit2.rviz" + ) else: - rviz_full_config = os.path.join(rviz_base, CONFIGURATION["rob"] + CONFIGURATION["ee"] + "_moveit2.rviz") + rviz_full_config = os.path.join( + rviz_base, CONFIGURATION["rob"] + CONFIGURATION["ee"] + "_moveit2.rviz" + ) rviz_node_full = Node( package="rviz2", @@ -380,18 +456,15 @@ def generate_launch_description(): robot_description, robot_description_semantic, kinematics_yaml, - pilz_planning_pipeline_config, - joint_limits, pilz_cartesian_limits, - trajectory_execution, moveit_controllers, planning_scene_monitor_parameters, move_group_capabilities, {"use_sim_time": True}, - ] + ], ) # =================================================================================================== # @@ -405,7 +478,15 @@ def generate_launch_description(): package="ros2srrc_execution", executable="move", output="screen", - parameters=[robot_description, robot_description_semantic, kinematics_yaml, {"use_sim_time": True}, {"ROB_PARAM": CONFIGURATION["rob"]}, {"EE_PARAM": CONFIGURATION["ee"]}, {"ENV_PARAM": "gazebo"}], + parameters=[ + robot_description, + robot_description_semantic, + kinematics_yaml, + {"use_sim_time": True}, + {"ROB_PARAM": CONFIGURATION["rob"]}, + {"EE_PARAM": CONFIGURATION["ee"]}, + {"ENV_PARAM": "gazebo"}, + ], ) else: @@ -415,7 +496,15 @@ def generate_launch_description(): package="ros2srrc_execution", executable="move", output="screen", - parameters=[robot_description, robot_description_semantic, kinematics_yaml, {"use_sim_time": True}, {"ROB_PARAM": CONFIGURATION["rob"]}, {"EE_PARAM": "none"}, {"ENV_PARAM": "gazebo"}], + parameters=[ + robot_description, + robot_description_semantic, + kinematics_yaml, + {"use_sim_time": True}, + {"ROB_PARAM": CONFIGURATION["rob"]}, + {"EE_PARAM": "none"}, + {"ENV_PARAM": "gazebo"}, + ], ) # RobMove and RobPose: @@ -424,16 +513,28 @@ def generate_launch_description(): package="ros2srrc_execution", executable="robmove", output="screen", - parameters=[robot_description, robot_description_semantic, kinematics_yaml, {"use_sim_time": True}, {"ROB_PARAM": CONFIGURATION["rob"]}], + parameters=[ + robot_description, + robot_description_semantic, + kinematics_yaml, + {"use_sim_time": True}, + {"ROB_PARAM": CONFIGURATION["rob"]}, + ], ) RobPoseInterface = Node( name="robpose", package="ros2srrc_execution", executable="robpose", output="screen", - parameters=[robot_description, robot_description_semantic, kinematics_yaml, {"use_sim_time": True}, {"ROB_PARAM": CONFIGURATION["rob"]}], + parameters=[ + robot_description, + robot_description_semantic, + kinematics_yaml, + {"use_sim_time": True}, + {"ROB_PARAM": CONFIGURATION["rob"]}, + ], ) - + # =============================================== # # ========== RETURN LAUNCH DESCRIPTION ========== # @@ -443,22 +544,24 @@ def generate_launch_description(): LD.add_action(static_tf) LD.add_action(spawn_entity) - LD.add_action(RegisterEventHandler( - OnProcessExit( - target_action = spawn_entity, - on_exit = [ - joint_state_broadcaster_spawner, - ] + LD.add_action( + RegisterEventHandler( + OnProcessExit( + target_action=spawn_entity, + on_exit=[ + joint_state_broadcaster_spawner, + ], ) ) ) - LD.add_action(RegisterEventHandler( - OnProcessExit( - target_action = spawn_entity, - on_exit = [ - joint_trajectory_controller_spawner, - ] + LD.add_action( + RegisterEventHandler( + OnProcessExit( + target_action=spawn_entity, + on_exit=[ + joint_trajectory_controller_spawner, + ], ) ) ) @@ -467,54 +570,53 @@ def generate_launch_description(): for x in CONTROLLER_NODES: - LD.add_action(RegisterEventHandler( - OnProcessExit( - target_action = joint_trajectory_controller_spawner, - on_exit = [ - x, - ] + LD.add_action( + RegisterEventHandler( + OnProcessExit( + target_action=joint_trajectory_controller_spawner, + on_exit=[ + x, + ], ) ) ) - LD.add_action(RegisterEventHandler( - OnProcessExit( - target_action = spawn_entity, - on_exit = [ - - # MoveIt!2: - TimerAction( - period=2.0, - actions=[ - rviz_node_full, - run_move_group_node, - ] - ), - - ] + LD.add_action( + RegisterEventHandler( + OnProcessExit( + target_action=spawn_entity, + on_exit=[ + # MoveIt!2: + TimerAction( + period=2.0, + actions=[ + rviz_node_full, + run_move_group_node, + ], + ), + ], ) ) ) - LD.add_action(RegisterEventHandler( - OnProcessExit( - target_action = spawn_entity, - on_exit = [ - - # Interfaces: - TimerAction( - period=5.0, - actions=[ - MoveInterface, - RobMoveInterface, - RobPoseInterface, - ] - ), - - ] + LD.add_action( + RegisterEventHandler( + OnProcessExit( + target_action=spawn_entity, + on_exit=[ + # Interfaces: + TimerAction( + period=5.0, + actions=[ + MoveInterface, + RobMoveInterface, + RobPoseInterface, + ], + ), + ], ) ) ) # ***** RETURN ***** # - return(LD) + return LD diff --git a/Launchers/pick_place_javier.launch.py b/Launchers/pick_place_javier.launch.py index ec9b1e53e..cb436b2d0 100755 --- a/Launchers/pick_place_javier.launch.py +++ b/Launchers/pick_place_javier.launch.py @@ -101,6 +101,7 @@ def generate_launch_description(): return ld + ################################################################################### # ROS2 + MOVEIT2 # Pick and place DEMO with 4 small objects - UR5 + Robotiq 85 diff --git a/Launchers/prius_360_autoparking.launch.py b/Launchers/prius_360_autoparking.launch.py index dc4c179fb..f59e779a5 100755 --- a/Launchers/prius_360_autoparking.launch.py +++ b/Launchers/prius_360_autoparking.launch.py @@ -6,69 +6,79 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'autoparking_prius_360lidar.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "autoparking_prius_360lidar.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/prius_autoparking.launch.py b/Launchers/prius_autoparking.launch.py index b6c510e46..54c3294ae 100755 --- a/Launchers/prius_autoparking.launch.py +++ b/Launchers/prius_autoparking.launch.py @@ -6,69 +6,79 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'autoparking_prius_3laser.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "autoparking_prius_3laser.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/prius_bateria.launch.py b/Launchers/prius_bateria.launch.py index 424c76589..fe5a9afc1 100755 --- a/Launchers/prius_bateria.launch.py +++ b/Launchers/prius_bateria.launch.py @@ -6,69 +6,79 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'bateria_prius_3laser.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "bateria_prius_3laser.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/rescue_people.launch.py b/Launchers/rescue_people.launch.py index b1932c376..ef5a49bd2 100755 --- a/Launchers/rescue_people.launch.py +++ b/Launchers/rescue_people.launch.py @@ -17,50 +17,59 @@ def generate_launch_description(): - custom_robots_share = get_package_share_directory('custom_robots') - bridges_path = os.path.join(custom_robots_share, 'bridges', 'rescue_people.yaml') - world_path = os.path.join(custom_robots_share, 'worlds', 'rescue_people_harmonic.world') + custom_robots_share = get_package_share_directory("custom_robots") + bridges_path = os.path.join(custom_robots_share, "bridges", "rescue_people.yaml") + world_path = os.path.join( + custom_robots_share, "worlds", "rescue_people_harmonic.world" + ) ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## declare_use_simulator_cmd = DeclareLaunchArgument( - name='use_simulator', - default_value='True', - description='Whether to start the simulator') + name="use_simulator", + default_value="True", + description="Whether to start the simulator", + ) # Start Gazebo server gzsim = IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('jderobot_drones'), 'launch'), - '/gz_sim.launch.py']), - condition=IfCondition(LaunchConfiguration('use_simulator')), + PythonLaunchDescriptionSource( + [ + os.path.join(get_package_share_directory("jderobot_drones"), "launch"), + "/gz_sim.launch.py", + ] + ), + condition=IfCondition(LaunchConfiguration("use_simulator")), launch_arguments={ - 'namespace': 'drone0', - 'bridges_file': bridges_path, - 'world_file': world_path, + "namespace": "drone0", + "bridges_file": bridges_path, + "world_file": world_path, }.items(), ) as2 = IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('jderobot_drones'), 'launch'), - '/as2_default_gazebo_sim.launch.py']), + PythonLaunchDescriptionSource( + [ + os.path.join(get_package_share_directory("jderobot_drones"), "launch"), + "/as2_default_gazebo_sim.launch.py", + ] + ), launch_arguments={ - 'namespace': 'drone0', + "namespace": "drone0", }.items(), ) start_gazebo_frontal_image_bridge_cmd = Node( - package='ros_gz_image', - executable='image_bridge', - arguments=['/drone0/frontal_cam/image_raw'], - output='screen', + package="ros_gz_image", + executable="image_bridge", + arguments=["/drone0/frontal_cam/image_raw"], + output="screen", ) start_gazebo_ventral_image_bridge_cmd = Node( - package='ros_gz_image', - executable='image_bridge', - arguments=['/drone0/ventral_cam/image_raw'], - output='screen', + package="ros_gz_image", + executable="image_bridge", + arguments=["/drone0/ventral_cam/image_raw"], + output="screen", ) # Create the launch description and populate diff --git a/Launchers/rescue_people/launch.py b/Launchers/rescue_people/launch.py index 174326a66..0033c722d 100644 --- a/Launchers/rescue_people/launch.py +++ b/Launchers/rescue_people/launch.py @@ -31,7 +31,7 @@ def spawn_process(args, insert_vglrun=False): return process -class Test(): +class Test: def gazebo(self): rospy.logwarn("[GAZEBO] Launching") try: @@ -43,13 +43,17 @@ def gazebo(self): def px4(self): rospy.logwarn("[PX4-SITL] Launching") start_time = rospy.get_time() - args = ["./PX4-Autopilot/build/px4_sitl_default/bin/px4-commander", - "--instance", "0", "check"] + args = [ + "./PX4-Autopilot/build/px4_sitl_default/bin/px4-commander", + "--instance", + "0", + "check", + ] while rospy.get_time() - start_time < TIMEOUT: process = spawn_process(args, insert_vglrun=False) with process.stdout: - for line in iter(process.stdout.readline, ''): - if ("Prearm check: OK" in line): + for line in iter(process.stdout.readline, ""): + if "Prearm check: OK" in line: return True rospy.sleep(2) return False @@ -63,7 +67,7 @@ def mavros(self, ns=""): return False -class Launch(): +class Launch: def __init__(self): self.test = Test() self.acceleration_enabled = check_device(DRI_PATH) @@ -76,11 +80,12 @@ def __init__(self): def start(self): ######## LAUNCH GAZEBO ######## - args = ["/opt/ros/noetic/bin/roslaunch", - "/opt/jderobot/Launchers/rescue_people/gazebo.launch", - "--wait", - "--log" - ] + args = [ + "/opt/ros/noetic/bin/roslaunch", + "/opt/jderobot/Launchers/rescue_people/gazebo.launch", + "--wait", + "--log", + ] attempt = 1 while True: @@ -93,10 +98,11 @@ def start(self): attempt = attempt + 1 ######## LAUNCH PX4 ######## - args = ["/opt/ros/noetic/bin/roslaunch", - "/opt/jderobot/Launchers/rescue_people/px4.launch", - "--log" - ] + args = [ + "/opt/ros/noetic/bin/roslaunch", + "/opt/jderobot/Launchers/rescue_people/px4.launch", + "--log", + ] attempt = 1 while True: @@ -109,10 +115,11 @@ def start(self): attempt = attempt + 1 ######## LAUNCH MAVROS ######## - args = ["/opt/ros/noetic/bin/roslaunch", - "/opt/jderobot/Launchers/rescue_people/mavros.launch", - "--log" - ] + args = [ + "/opt/ros/noetic/bin/roslaunch", + "/opt/jderobot/Launchers/rescue_people/mavros.launch", + "--log", + ] attempt = 1 while True: @@ -130,4 +137,4 @@ def start(self): launch.start() with open("/drones_launch.log", "w") as f: - f.write("success") \ No newline at end of file + f.write("success") diff --git a/Launchers/restaurant.launch.py b/Launchers/restaurant.launch.py index 44a33141d..d4d083da9 100755 --- a/Launchers/restaurant.launch.py +++ b/Launchers/restaurant.launch.py @@ -70,19 +70,17 @@ def generate_launch_description(): declare_slam_cmd = DeclareLaunchArgument("slam", default_value="True") declare_map_cmd = DeclareLaunchArgument( - 'map', default_value=os.path.join( - gazebo_models_path, - 'restaurant', - 'maps', - 'restaurant.yaml') + "map", + default_value=os.path.join( + gazebo_models_path, "restaurant", "maps", "restaurant.yaml" + ), ) declare_nav_params_cmd = DeclareLaunchArgument( - 'params_file', default_value=os.path.join( - gazebo_models_path, - 'restaurant', - 'params', - 'roomba_sim_nav_params.yaml') + "params_file", + default_value=os.path.join( + gazebo_models_path, "restaurant", "params", "roomba_sim_nav_params.yaml" + ), ) # Specify the actions @@ -105,24 +103,24 @@ def generate_launch_description(): localization_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(pkg_nav2_ros, 'launch', 'localization_launch.py') + os.path.join(pkg_nav2_ros, "launch", "localization_launch.py") ), launch_arguments={ - 'use_sim_time': use_sim_time, - 'map': map_file, - 'slam': slam, - 'params_file': params_file - }.items() + "use_sim_time": use_sim_time, + "map": map_file, + "slam": slam, + "params_file": params_file, + }.items(), ) navigation_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(pkg_nav2_ros, 'launch', 'navigation_launch.py') + os.path.join(pkg_nav2_ros, "launch", "navigation_launch.py") ), launch_arguments={ - 'use_sim_time': use_sim_time, - 'params_file': params_file - }.items() + "use_sim_time": use_sim_time, + "params_file": params_file, + }.items(), ) # Create the launch description and populate @@ -144,7 +142,7 @@ def generate_launch_description(): ld.add_action(navigation_cmd) # Remap nav2 - cmd_vel_remap = SetRemap(src='cmd_vel_nav', dst='cmd_vel') + cmd_vel_remap = SetRemap(src="cmd_vel_nav", dst="cmd_vel") ld.add_action(cmd_vel_remap) return ld diff --git a/Launchers/robots/turtlebot3/robot_state_publisher.launch.py b/Launchers/robots/turtlebot3/robot_state_publisher.launch.py index 86b094cd0..731207c12 100644 --- a/Launchers/robots/turtlebot3/robot_state_publisher.launch.py +++ b/Launchers/robots/turtlebot3/robot_state_publisher.launch.py @@ -26,33 +26,33 @@ def generate_launch_description(): - use_sim_time = LaunchConfiguration('use_sim_time', default='true') - urdf_file_name = 'turtlebot3_waffle.urdf' + use_sim_time = LaunchConfiguration("use_sim_time", default="true") + urdf_file_name = "turtlebot3_waffle.urdf" - print('urdf_file_name : {}'.format(urdf_file_name)) + print("urdf_file_name : {}".format(urdf_file_name)) urdf_path = os.path.join( - get_package_share_directory('custom_robots'), - 'urdf', - urdf_file_name) + get_package_share_directory("custom_robots"), "urdf", urdf_file_name + ) - with open(urdf_path, 'r') as infp: + with open(urdf_path, "r") as infp: robot_desc = infp.read() - return LaunchDescription([ - DeclareLaunchArgument( - 'use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true'), - - Node( - package='robot_state_publisher', - executable='robot_state_publisher', - name='robot_state_publisher', - output='screen', - parameters=[{ - 'use_sim_time': use_sim_time, - 'robot_description': robot_desc - }], - ), - ]) \ No newline at end of file + return LaunchDescription( + [ + DeclareLaunchArgument( + "use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + parameters=[ + {"use_sim_time": use_sim_time, "robot_description": robot_desc} + ], + ), + ] + ) diff --git a/Launchers/robots/turtlebot3/spawn_robot.launch.py b/Launchers/robots/turtlebot3/spawn_robot.launch.py index 15c312f24..a66c10a9d 100644 --- a/Launchers/robots/turtlebot3/spawn_robot.launch.py +++ b/Launchers/robots/turtlebot3/spawn_robot.launch.py @@ -25,33 +25,34 @@ ) from launch.launch_description_sources import PythonLaunchDescriptionSource + def generate_launch_description(): - ROBOT_X = os.environ['ROBOT_X'] - ROBOT_Y = os.environ['ROBOT_Y'] - ROBOT_Z = os.environ['ROBOT_Z'] - ROBOT_ROLL = os.environ['ROBOT_ROLL'] - ROBOT_PITCH = os.environ['ROBOT_PITCH'] - ROBOT_YAW = os.environ['ROBOT_YAW'] + ROBOT_X = os.environ["ROBOT_X"] + ROBOT_Y = os.environ["ROBOT_Y"] + ROBOT_Z = os.environ["ROBOT_Z"] + ROBOT_ROLL = os.environ["ROBOT_ROLL"] + ROBOT_PITCH = os.environ["ROBOT_PITCH"] + ROBOT_YAW = os.environ["ROBOT_YAW"] robot_launch_dir = "/opt/jderobot/Launchers/robots/turtlebot3" # Get the urdf file - model_folder = 'turtlebot3_waffle' + model_folder = "turtlebot3_waffle" urdf_path = os.path.join( - get_package_share_directory('custom_robots'), - 'models', + get_package_share_directory("custom_robots"), + "models", model_folder, - 'model.sdf' + "model.sdf", ) # Launch configuration variables specific to simulation - use_sim_time = LaunchConfiguration('use_sim_time', default='true') - x_pose = LaunchConfiguration('x_pose', default=ROBOT_X) - y_pose = LaunchConfiguration('y_pose', default=ROBOT_Y) - z_pose = LaunchConfiguration('z_pose', default=ROBOT_Z) - R_pose = LaunchConfiguration('R_pose', default=ROBOT_ROLL) - P_pose = LaunchConfiguration('P_pose', default=ROBOT_PITCH) - Y_pose = LaunchConfiguration('Y_pose', default=ROBOT_YAW) + use_sim_time = LaunchConfiguration("use_sim_time", default="true") + x_pose = LaunchConfiguration("x_pose", default=ROBOT_X) + y_pose = LaunchConfiguration("y_pose", default=ROBOT_Y) + z_pose = LaunchConfiguration("z_pose", default=ROBOT_Z) + R_pose = LaunchConfiguration("R_pose", default=ROBOT_ROLL) + P_pose = LaunchConfiguration("P_pose", default=ROBOT_PITCH) + Y_pose = LaunchConfiguration("Y_pose", default=ROBOT_YAW) # Declare the launch arguments # declare_x_position_cmd = DeclareLaunchArgument( @@ -61,11 +62,11 @@ def generate_launch_description(): # declare_y_position_cmd = DeclareLaunchArgument( # 'y_pose', default_value='-1.5', # description='Specify namespace of the robot') - + # declare_z_position_cmd = DeclareLaunchArgument( # 'z_pose', default_value='0.6', # description='Specify namespace of the robot') - + # declare_roll_cmd = DeclareLaunchArgument( # 'R', default_value='0.0' # ) @@ -78,59 +79,64 @@ def generate_launch_description(): # 'Y', default_value='1.57079' # ) - start_gazebo_ros_spawner_cmd = Node( - package='ros_gz_sim', - executable='create', + package="ros_gz_sim", + executable="create", arguments=[ - '-name', 'waffle', - '-file', urdf_path, - '-x', x_pose, - '-y', y_pose, - '-z', z_pose, - '-R', R_pose, - '-P', P_pose, - '-Y', Y_pose + "-name", + "waffle", + "-file", + urdf_path, + "-x", + x_pose, + "-y", + y_pose, + "-z", + z_pose, + "-R", + R_pose, + "-P", + P_pose, + "-Y", + Y_pose, ], - output='screen', + output="screen", ) robot_state_publisher_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(robot_launch_dir, 'robot_state_publisher.launch.py') + os.path.join(robot_launch_dir, "robot_state_publisher.launch.py") ), - launch_arguments={'use_sim_time': use_sim_time}.items() + launch_arguments={"use_sim_time": use_sim_time}.items(), ) bridge_params = os.path.join( - get_package_share_directory('custom_robots'), - 'params', - 'robot_params.yaml' + get_package_share_directory("custom_robots"), "params", "robot_params.yaml" ) start_gazebo_ros_bridge_cmd = Node( - package='ros_gz_bridge', - executable='parameter_bridge', + package="ros_gz_bridge", + executable="parameter_bridge", arguments=[ - '--ros-args', - '-p', - f'config_file:={bridge_params}', + "--ros-args", + "-p", + f"config_file:={bridge_params}", ], - output='screen', + output="screen", ) start_gazebo_ros_image_bridge_cmd = Node( - package='ros_gz_image', - executable='image_bridge', - arguments=['/turtlebot3/camera/image_raw'], - output='screen', + package="ros_gz_image", + executable="image_bridge", + arguments=["/turtlebot3/camera/image_raw"], + output="screen", ) start_gazebo_ros_depth_bridge_cmd = Node( - package='ros_gz_image', - executable='image_bridge', - arguments=['/turtlebot3/camera/depth'], - output='screen', + package="ros_gz_image", + executable="image_bridge", + arguments=["/turtlebot3/camera/depth"], + output="screen", ) ld = LaunchDescription() @@ -150,4 +156,4 @@ def generate_launch_description(): ld.add_action(start_gazebo_ros_image_bridge_cmd) ld.add_action(start_gazebo_ros_depth_bridge_cmd) - return ld \ No newline at end of file + return ld diff --git a/Launchers/simple_circuit.launch.py b/Launchers/simple_circuit.launch.py index b440ee776..17156acfa 100755 --- a/Launchers/simple_circuit.launch.py +++ b/Launchers/simple_circuit.launch.py @@ -6,69 +6,79 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'simple_circuit.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "simple_circuit.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/simple_circuit_ackermann.launch.py b/Launchers/simple_circuit_ackermann.launch.py index 8ea6ebf4a..4780aa553 100755 --- a/Launchers/simple_circuit_ackermann.launch.py +++ b/Launchers/simple_circuit_ackermann.launch.py @@ -6,69 +6,79 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'simple_circuit_ackermann.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "simple_circuit_ackermann.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/simple_circuit_ackermann_followingcam.launch.py b/Launchers/simple_circuit_ackermann_followingcam.launch.py index accea87cb..53d04e619 100755 --- a/Launchers/simple_circuit_ackermann_followingcam.launch.py +++ b/Launchers/simple_circuit_ackermann_followingcam.launch.py @@ -6,69 +6,79 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'simple_circuit_ackermann_followingcam.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "simple_circuit_ackermann_followingcam.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/simple_circuit_followingcam.launch.py b/Launchers/simple_circuit_followingcam.launch.py index 9ea1ddab7..ce00d68e0 100755 --- a/Launchers/simple_circuit_followingcam.launch.py +++ b/Launchers/simple_circuit_followingcam.launch.py @@ -6,69 +6,79 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'simple_circuit_followingcam.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "simple_circuit_followingcam.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/simple_circuit_obstacles.launch.py b/Launchers/simple_circuit_obstacles.launch.py index 6605f9ff5..caea96d3a 100755 --- a/Launchers/simple_circuit_obstacles.launch.py +++ b/Launchers/simple_circuit_obstacles.launch.py @@ -6,69 +6,79 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'simple_circuit_obstacles.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "simple_circuit_obstacles.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/simple_circuit_obstacles_followingcam.launch.py b/Launchers/simple_circuit_obstacles_followingcam.launch.py index dc2bce0ff..8bc17fcab 100755 --- a/Launchers/simple_circuit_obstacles_followingcam.launch.py +++ b/Launchers/simple_circuit_obstacles_followingcam.launch.py @@ -6,69 +6,79 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'simple_circuit_obstacles_followingcam.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "simple_circuit_obstacles_followingcam.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/small_laser_mapping.launch.py b/Launchers/small_laser_mapping.launch.py index 3c1622613..f18e14f23 100644 --- a/Launchers/small_laser_mapping.launch.py +++ b/Launchers/small_laser_mapping.launch.py @@ -7,7 +7,7 @@ DeclareLaunchArgument, IncludeLaunchDescription, SetEnvironmentVariable, - AppendEnvironmentVariable + AppendEnvironmentVariable, ) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, Command @@ -18,89 +18,78 @@ def generate_launch_description(): - x = LaunchConfiguration('x') - y = LaunchConfiguration('y') - z = LaunchConfiguration('z') - roll = LaunchConfiguration('R') - pitch = LaunchConfiguration('P') - yaw = LaunchConfiguration('Y') + x = LaunchConfiguration("x") + y = LaunchConfiguration("y") + z = LaunchConfiguration("z") + roll = LaunchConfiguration("R") + pitch = LaunchConfiguration("P") + yaw = LaunchConfiguration("Y") - package_dir = get_package_share_directory('custom_robots') - ros_gz_sim = get_package_share_directory('ros_gz_sim') + package_dir = get_package_share_directory("custom_robots") + ros_gz_sim = get_package_share_directory("ros_gz_sim") gazebo_models_path = os.path.join(package_dir, "models") robot_launch_dir = "/opt/jderobot/Launchers/marker_visual_loc" - robot_model_dir = os.path.join(package_dir, 'models/turtlebot3_waffle') - - use_sim_time = LaunchConfiguration('use_sim_time', default='true') - x_pose = LaunchConfiguration('x_pose', default='0') - y_pose = LaunchConfiguration('y_pose', default='0') - z_pose = LaunchConfiguration('z_pose', default='0.0') + robot_model_dir = os.path.join(package_dir, "models/turtlebot3_waffle") + + use_sim_time = LaunchConfiguration("use_sim_time", default="true") + x_pose = LaunchConfiguration("x_pose", default="0") + y_pose = LaunchConfiguration("y_pose", default="0") + z_pose = LaunchConfiguration("z_pose", default="0.0") world_file_name = "small_laser_mapping.world" worlds_dir = "/opt/jderobot/Worlds" world_path = os.path.join(worlds_dir, world_file_name) gazebo_server = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(ros_gz_sim, 'launch', 'gz_sim.launch.py')), - launch_arguments={'gz_args': ['-r -s -v4 ', world_path], 'on_exit_shutdown': 'true'}.items() + os.path.join(ros_gz_sim, "launch", "gz_sim.launch.py") + ), + launch_arguments={ + "gz_args": ["-r -s -v4 ", world_path], + "on_exit_shutdown": "true", + }.items(), ) - declare_x_cmd = DeclareLaunchArgument( - 'x', default_value='0' - ) + declare_x_cmd = DeclareLaunchArgument("x", default_value="0") - declare_y_cmd = DeclareLaunchArgument( - 'y', default_value='0' - ) + declare_y_cmd = DeclareLaunchArgument("y", default_value="0") - declare_z_cmd = DeclareLaunchArgument( - 'z', default_value='0.0' - ) + declare_z_cmd = DeclareLaunchArgument("z", default_value="0.0") - declare_roll_cmd = DeclareLaunchArgument( - 'R', default_value='0.0' - ) + declare_roll_cmd = DeclareLaunchArgument("R", default_value="0.0") - declare_pitch_cmd = DeclareLaunchArgument( - 'P', default_value='0.0' - ) + declare_pitch_cmd = DeclareLaunchArgument("P", default_value="0.0") - declare_yaw_cmd = DeclareLaunchArgument( - 'Y', default_value='0.0' - ) + declare_yaw_cmd = DeclareLaunchArgument("Y", default_value="0.0") robot_state_publisher_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(robot_launch_dir, 'robot_state_publisher.launch.py') + os.path.join(robot_launch_dir, "robot_state_publisher.launch.py") ), - launch_arguments={'use_sim_time': use_sim_time}.items() + launch_arguments={"use_sim_time": use_sim_time}.items(), ) spawn_robot_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(robot_launch_dir, 'spawn_robot.launch.py') + os.path.join(robot_launch_dir, "spawn_robot.launch.py") ), - launch_arguments={ - 'x_pose': x_pose, - 'y_pose': y_pose, - 'z_pose': z_pose - }.items() + launch_arguments={"x_pose": x_pose, "y_pose": y_pose, "z_pose": z_pose}.items(), ) - world_entity_cmd = Node(package='ros_gz_sim', executable='create', - arguments=['-name', - 'world', - '-file', - world_path - ], - output='screen') + world_entity_cmd = Node( + package="ros_gz_sim", + executable="create", + arguments=["-name", "world", "-file", world_path], + output="screen", + ) ld = LaunchDescription() - ld.add_action(SetEnvironmentVariable('GZ_SIM_RESOURCE_PATH', gazebo_models_path)) - set_env_vars_resources = AppendEnvironmentVariable('GZ_SIM_RESOURCE_PATH', os.path.join(package_dir,'models')) + ld.add_action(SetEnvironmentVariable("GZ_SIM_RESOURCE_PATH", gazebo_models_path)) + set_env_vars_resources = AppendEnvironmentVariable( + "GZ_SIM_RESOURCE_PATH", os.path.join(package_dir, "models") + ) ld.add_action(set_env_vars_resources) ld.add_action(gazebo_server) # ld.add_action(gazebo_client) diff --git a/Launchers/small_warehouse_with_ackermann_logistic_robot.launch.py b/Launchers/small_warehouse_with_ackermann_logistic_robot.launch.py index dab53f46b..9e02a12ed 100644 --- a/Launchers/small_warehouse_with_ackermann_logistic_robot.launch.py +++ b/Launchers/small_warehouse_with_ackermann_logistic_robot.launch.py @@ -6,69 +6,81 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'no_roof_small_warehouse_with_ackermann_logistic_robot_followingcam.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld \ No newline at end of file + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = ( + "no_roof_small_warehouse_with_ackermann_logistic_robot_followingcam.world" + ) + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/taxi_navigator.launch.py b/Launchers/taxi_navigator.launch.py index 0a0f64731..cc45d0e9b 100755 --- a/Launchers/taxi_navigator.launch.py +++ b/Launchers/taxi_navigator.launch.py @@ -6,69 +6,79 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'taxi_navigation_city_large.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "taxi_navigation_city_large.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/taxi_navigator_followingcam.launch.py b/Launchers/taxi_navigator_followingcam.launch.py index 795f627c6..ec87ac1a3 100755 --- a/Launchers/taxi_navigator_followingcam.launch.py +++ b/Launchers/taxi_navigator_followingcam.launch.py @@ -6,69 +6,79 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - + + def generate_launch_description(): - - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to this package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'taxi_navigation_city_large_followingcam.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "taxi_navigation_city_large_followingcam.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/vacuum_cleaner_headless.launch.py b/Launchers/vacuum_cleaner_headless.launch.py index ddc5c6f7a..1b04e4d12 100755 --- a/Launchers/vacuum_cleaner_headless.launch.py +++ b/Launchers/vacuum_cleaner_headless.launch.py @@ -7,68 +7,76 @@ from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare + def generate_launch_description(): - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to custom_robots package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'roomba_1_house.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='True', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to custom_robots package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "roomba_1_house.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", default_value="True", description="Whether to execute gzclient" + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/vacuum_cleaner_loc.launch.py b/Launchers/vacuum_cleaner_loc.launch.py index 14ca27ac2..370281195 100755 --- a/Launchers/vacuum_cleaner_loc.launch.py +++ b/Launchers/vacuum_cleaner_loc.launch.py @@ -7,68 +7,78 @@ from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare + def generate_launch_description(): - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to custom_robots package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'roomba_1_house.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to custom_robots package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "roomba_1_house.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/vacuum_cleaner_loc_headless.launch.py b/Launchers/vacuum_cleaner_loc_headless.launch.py index ddc5c6f7a..1b04e4d12 100755 --- a/Launchers/vacuum_cleaner_loc_headless.launch.py +++ b/Launchers/vacuum_cleaner_loc_headless.launch.py @@ -7,68 +7,76 @@ from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare + def generate_launch_description(): - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - - # Set the path to custom_robots package. - pkg_share = FindPackageShare(package='custom_robots').find('custom_robots') - - # Set the path to the world file - world_file_name = 'roomba_1_house.world' - worlds_dir = "/opt/jderobot/Worlds" - world_path = os.path.join(worlds_dir, world_file_name) - - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, 'models') - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" - - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='True', - description='Whether to execute gzclient') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - 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') - - # Specify the actions - - # 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()) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - # Add any actions - ld.add_action(start_gazebo_server_cmd) - - return ld + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to custom_robots package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "roomba_1_house.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", default_value="True", description="Whether to execute gzclient" + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + 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", + ) + + # Specify the actions + + # 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(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/jderobot_drones/jderobot_drones/drone_wrapper.py b/jderobot_drones/jderobot_drones/drone_wrapper.py index 5256f2380..2c81adf44 100644 --- a/jderobot_drones/jderobot_drones/drone_wrapper.py +++ b/jderobot_drones/jderobot_drones/drone_wrapper.py @@ -1,13 +1,16 @@ """ drone_wrapper.py """ + import asyncio import time from typing import List from numpy import ndarray from as2_python_api.drone_interface_base import DroneInterfaceBase -from as2_python_api.modules.motion_reference_handler_module import MotionReferenceHandlerModule +from as2_python_api.modules.motion_reference_handler_module import ( + MotionReferenceHandlerModule, +) from as2_msgs.srv import SetPlatformStateMachineEvent from as2_msgs.msg import PlatformStateMachineEvent, PlatformStatus @@ -15,13 +18,19 @@ from geometry_msgs.msg import TwistStamped from cv_bridge import CvBridge import rclpy -from rclpy.qos import QoSProfile, QoSHistoryPolicy, QoSReliabilityPolicy, QoSDurabilityPolicy +from rclpy.qos import ( + QoSProfile, + QoSHistoryPolicy, + QoSReliabilityPolicy, + QoSDurabilityPolicy, +) class DroneWrapper(DroneInterfaceBase): """ Drone Wrapper """ + TK_RATE = 0.1 TK_HEIGHT_MARGIN = 0.25 LAND_RATE = 0.1 @@ -32,7 +41,7 @@ class DroneWrapper(DroneInterfaceBase): def __init__(self, drone_id: str = "drone0", verbose: bool = False) -> None: super().__init__(drone_id, verbose, use_sim_time=True) - yaw_rate_topic = '/' + drone_id + '/self_localization/twist' + yaw_rate_topic = "/" + drone_id + "/self_localization/twist" self.yaw_rate = 0.0 @@ -42,20 +51,20 @@ def __init__(self, drone_id: str = "drone0", verbose: bool = False) -> None: history=QoSHistoryPolicy.KEEP_LAST, depth=10, reliability=QoSReliabilityPolicy.BEST_EFFORT, - durability=QoSDurabilityPolicy.VOLATILE + durability=QoSDurabilityPolicy.VOLATILE, ) self.yaw_subscription = self.create_subscription( - TwistStamped, - yaw_rate_topic, - self.yaw_rate_cb, - qos_profile) + TwistStamped, yaw_rate_topic, self.yaw_rate_cb, qos_profile + ) self.yaw_subscription # prevent unused variable warning self.motion_ref_handler = MotionReferenceHandlerModule(drone=self) self.state_event_service_client = self.create_client( - SetPlatformStateMachineEvent, '/' + drone_id + '/platform/state_machine_event') + SetPlatformStateMachineEvent, + "/" + drone_id + "/platform/state_machine_event", + ) def get_position(self) -> List[float]: """Get drone position (x, y, z) in m. @@ -126,19 +135,24 @@ def get_landed_state(self) -> int: def set_cmd_pos(self, x: float, y: float, z: float, az: float) -> None: """Send position command with yaw angle""" self.motion_ref_handler.position.send_position_command_with_yaw_angle( - [x, y, z], 1.0, 'earth', self.namespace + '/base_link', float(az)) + [x, y, z], 1.0, "earth", self.namespace + "/base_link", float(az) + ) def set_cmd_vel(self, vx: float, vy: float, vz: float, az: float) -> None: """Send speed command with yaw angle""" self.motion_ref_handler.speed.send_speed_command_with_yaw_speed( - [vx, vy, vz], self.namespace + '/base_link', float(az)) + [vx, vy, vz], self.namespace + "/base_link", float(az) + ) def set_cmd_mix(self, vx: float, vy: float, z: float, az: float) -> None: """Send speed in a plane command with yaw angle""" self.motion_ref_handler.speed_in_a_plane.send_speed_in_a_plane_command_with_yaw_speed( - [vx, vy], z, 'earth', self.namespace + '/base_link', float(az)) + [vx, vy], z, "earth", self.namespace + "/base_link", float(az) + ) - async def call_state_event_service(self, event_value: PlatformStateMachineEvent) -> None: + async def call_state_event_service( + self, event_value: PlatformStateMachineEvent + ) -> None: """Request aerostack to update state machine given the new event EMERGENCY -1 DISARMED 0 @@ -151,52 +165,52 @@ async def call_state_event_service(self, event_value: PlatformStateMachineEvent) request.event.event = event_value while not self.state_event_service_client.wait_for_service(timeout_sec=1.0): - self.get_logger().info('Service not available, waiting...') + self.get_logger().info("Service not available, waiting...") response = await self.state_event_service_client.call_async(request) if response is not None: - self.get_logger().info(f'Success: {response.success}') - self.get_logger().info(f'Current State: {response.current_state}') + self.get_logger().info(f"Success: {response.success}") + self.get_logger().info(f"Current State: {response.current_state}") else: - self.get_logger().error('Service call failed') + self.get_logger().error("Service call failed") def takeoff(self, height: float): """Send Takeoff command with height""" - if self.get_landed_state() == PlatformStatus.TAKING_OFF or \ - self.get_landed_state() == PlatformStatus.FLYING: - self.get_logger().info('Drone is already flying!') + if ( + self.get_landed_state() == PlatformStatus.TAKING_OFF + or self.get_landed_state() == PlatformStatus.FLYING + ): + self.get_logger().info("Drone is already flying!") return self.arm() self.offboard() # Starting to take off - asyncio.run(self.call_state_event_service( - PlatformStateMachineEvent.TAKE_OFF)) + asyncio.run(self.call_state_event_service(PlatformStateMachineEvent.TAKE_OFF)) while True: if abs(self.position[2] - height) < self.TK_HEIGHT_MARGIN: break - self.set_cmd_pos( - self.position[0], self.position[1], height, self.get_yaw()) + self.set_cmd_pos(self.position[0], self.position[1], height, self.get_yaw()) time.sleep(self.TK_RATE) # Take off finished - asyncio.run(self.call_state_event_service( - PlatformStateMachineEvent.TOOK_OFF)) + asyncio.run(self.call_state_event_service(PlatformStateMachineEvent.TOOK_OFF)) def land(self) -> None: """Send Landing command""" - if self.get_landed_state() == PlatformStatus.LANDED or \ - self.get_landed_state() == PlatformStatus.LANDING: - self.get_logger().info('Drone is already landed!') + if ( + self.get_landed_state() == PlatformStatus.LANDED + or self.get_landed_state() == PlatformStatus.LANDING + ): + self.get_logger().info("Drone is already landed!") return # Starting to land - asyncio.run(self.call_state_event_service( - PlatformStateMachineEvent.LAND)) + asyncio.run(self.call_state_event_service(PlatformStateMachineEvent.LAND)) height = self.position[2] while True: @@ -204,13 +218,14 @@ def land(self) -> None: time.sleep(self.LAND_RATE) # Check if drone has landed - if abs(self.get_velocity()[2]) < self.LAND_SPEED_MARGIN and \ - abs(self.position[2] - height) > self.LAND_HEIGHT_MARGIN: + if ( + abs(self.get_velocity()[2]) < self.LAND_SPEED_MARGIN + and abs(self.position[2] - height) > self.LAND_HEIGHT_MARGIN + ): break # Land finished - asyncio.run(self.call_state_event_service( - PlatformStateMachineEvent.LANDED)) + asyncio.run(self.call_state_event_service(PlatformStateMachineEvent.LANDED)) self.disarm() diff --git a/jderobot_drones/jderobot_drones/image_sub.py b/jderobot_drones/jderobot_drones/image_sub.py index d0eac3552..19dfaebdc 100644 --- a/jderobot_drones/jderobot_drones/image_sub.py +++ b/jderobot_drones/jderobot_drones/image_sub.py @@ -1,7 +1,12 @@ import rclpy from rclpy.node import Node from sensor_msgs.msg import Image -from rclpy.qos import QoSProfile, QoSHistoryPolicy, QoSReliabilityPolicy, QoSDurabilityPolicy +from rclpy.qos import ( + QoSProfile, + QoSHistoryPolicy, + QoSReliabilityPolicy, + QoSDurabilityPolicy, +) from cv_bridge import CvBridge from numpy import ndarray import threading @@ -10,12 +15,14 @@ class ImageSubscriberNode(Node): def __init__(self): - super().__init__('image_subscriber_node') + super().__init__("image_subscriber_node") - cam_frontal_topic = '/' + 'drone0' + \ - '/sensor_measurements/frontal_camera/image_raw' - cam_ventral_topic = '/' + 'drone0' + \ - '/sensor_measurements/ventral_camera/image_raw' + cam_frontal_topic = ( + "/" + "drone0" + "/sensor_measurements/frontal_camera/image_raw" + ) + cam_ventral_topic = ( + "/" + "drone0" + "/sensor_measurements/ventral_camera/image_raw" + ) self.frontal_image = Image() self.ventral_image = Image() @@ -31,20 +38,16 @@ def __init__(self): history=QoSHistoryPolicy.KEEP_LAST, depth=10, reliability=QoSReliabilityPolicy.BEST_EFFORT, - durability=QoSDurabilityPolicy.VOLATILE + durability=QoSDurabilityPolicy.VOLATILE, ) self.cam_frontal_subscription = self.create_subscription( - Image, - cam_frontal_topic, - self.cam_frontal_cb, - qos_profile) + Image, cam_frontal_topic, self.cam_frontal_cb, qos_profile + ) self.cam_frontal_subscription # prevent unused variable warning self.cam_ventral_subscription = self.create_subscription( - Image, - cam_ventral_topic, - self.cam_ventral_cb, - qos_profile) + Image, cam_ventral_topic, self.cam_ventral_cb, qos_profile + ) self.cam_ventral_subscription # prevent unused variable warning def __auto_spin(self) -> None: @@ -56,7 +59,6 @@ def cam_frontal_cb(self, msg): """Callback to update current Frontal Image""" self.frontal_image = msg - def cam_ventral_cb(self, msg): """Callback to update current Ventral Image""" self.ventral_image = msg @@ -82,5 +84,6 @@ def main(args=None): rclpy.spin(node) rclpy.shutdown() -if __name__ == '__main__': - main() + +if __name__ == "__main__": + main() diff --git a/jderobot_drones/launch/as2_default_classic_gazebo.launch.py b/jderobot_drones/launch/as2_default_classic_gazebo.launch.py index 1ad3ffb3d..c84f88b0f 100644 --- a/jderobot_drones/launch/as2_default_classic_gazebo.launch.py +++ b/jderobot_drones/launch/as2_default_classic_gazebo.launch.py @@ -1,10 +1,15 @@ """ as2_default_classic_gazebo.launch.py """ + import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, ExecuteProcess +from launch.actions import ( + IncludeLaunchDescription, + DeclareLaunchArgument, + ExecuteProcess, +) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration @@ -15,73 +20,98 @@ def generate_launch_description(): """ # If needed -# rviz_config = os.path.join(os.getcwd(), 'swarm_config.rviz') -# print(f'{os.path.isfile(rviz_config)=}') - micro_xrce_cmd = ['MicroXRCEAgent', 'udp4', '-p', '8888'] + # rviz_config = os.path.join(os.getcwd(), 'swarm_config.rviz') + # print(f'{os.path.isfile(rviz_config)=}') + micro_xrce_cmd = ["MicroXRCEAgent", "udp4", "-p", "8888"] - micro_xrce = ExecuteProcess( - cmd = micro_xrce_cmd, - output = 'screen' - ) + micro_xrce = ExecuteProcess(cmd=micro_xrce_cmd, output="screen") - sim_config = os.path.join(get_package_share_directory('jderobot_drones'), 'sim_config/px4_classic') + sim_config = os.path.join( + get_package_share_directory("jderobot_drones"), "sim_config/px4_classic" + ) aerial_platform = IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('as2_platform_pixhawk'), 'launch'), - '/pixhawk_launch.py']), + PythonLaunchDescriptionSource( + [ + os.path.join( + get_package_share_directory("as2_platform_pixhawk"), "launch" + ), + "/pixhawk_launch.py", + ] + ), launch_arguments={ - 'namespace': LaunchConfiguration('namespace'), - 'use_sim_time': 'false', - 'simulation_config_file': LaunchConfiguration('world_file'), - 'platform_config_file': sim_config + '/platform_config.yaml' + "namespace": LaunchConfiguration("namespace"), + "use_sim_time": "false", + "simulation_config_file": LaunchConfiguration("world_file"), + "platform_config_file": sim_config + "/platform_config.yaml", }.items(), ) state_estimator = IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('as2_state_estimator'), 'launch'), - '/state_estimator_launch.py']), + PythonLaunchDescriptionSource( + [ + os.path.join( + get_package_share_directory("as2_state_estimator"), "launch" + ), + "/state_estimator_launch.py", + ] + ), launch_arguments={ - 'namespace': LaunchConfiguration('namespace'), - 'use_sim_time': 'false', - 'plugin_name': 'raw_odometry', - 'plugin_config_file': sim_config + '/state_estimator_odom.yaml' + "namespace": LaunchConfiguration("namespace"), + "use_sim_time": "false", + "plugin_name": "raw_odometry", + "plugin_config_file": sim_config + "/state_estimator_odom.yaml", }.items(), ) motion_controller = IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('as2_motion_controller'), 'launch'), - '/controller_launch.py']), + PythonLaunchDescriptionSource( + [ + os.path.join( + get_package_share_directory("as2_motion_controller"), "launch" + ), + "/controller_launch.py", + ] + ), launch_arguments={ - 'namespace': LaunchConfiguration('namespace'), - 'use_sim_time': 'false', - 'motion_controller_config_file': sim_config + '/motion_controller.yaml', - 'plugin_name': 'pid_speed_controller', - 'plugin_config_file': sim_config + '/pid_speed_controller.yaml' + "namespace": LaunchConfiguration("namespace"), + "use_sim_time": "false", + "motion_controller_config_file": sim_config + "/motion_controller.yaml", + "plugin_name": "pid_speed_controller", + "plugin_config_file": sim_config + "/pid_speed_controller.yaml", }.items(), ) behaviors = IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('as2_behaviors_motion'), 'launch'), - '/motion_behaviors_launch.py']), + PythonLaunchDescriptionSource( + [ + os.path.join( + get_package_share_directory("as2_behaviors_motion"), "launch" + ), + "/motion_behaviors_launch.py", + ] + ), launch_arguments={ - 'namespace': LaunchConfiguration('namespace'), - 'use_sim_time': 'false', - 'takeoff_plugin_name': 'takeoff_plugin_position', - 'go_to_plugin_name': 'go_to_plugin_position', - 'follow_path_plugin_name': 'follow_path_plugin_position', - 'land_plugin_name': 'land_plugin_speed' + "namespace": LaunchConfiguration("namespace"), + "use_sim_time": "false", + "takeoff_plugin_name": "takeoff_plugin_position", + "go_to_plugin_name": "go_to_plugin_position", + "follow_path_plugin_name": "follow_path_plugin_position", + "land_plugin_name": "land_plugin_speed", }.items(), ) - return LaunchDescription([ - DeclareLaunchArgument('namespace', default_value='drone0', - description='Drone namespace.'), - DeclareLaunchArgument('world_file', default_value=sim_config + '/world.json', - description='json world file'), - micro_xrce, - aerial_platform, - state_estimator, - motion_controller - # behaviors - ]) + return LaunchDescription( + [ + DeclareLaunchArgument( + "namespace", default_value="drone0", description="Drone namespace." + ), + DeclareLaunchArgument( + "world_file", + default_value=sim_config + "/world.json", + description="json world file", + ), + micro_xrce, + aerial_platform, + state_estimator, + motion_controller, + # behaviors + ] + ) diff --git a/jderobot_drones/launch/as2_default_gazebo_sim.launch.py b/jderobot_drones/launch/as2_default_gazebo_sim.launch.py index 12fdda621..2608b8c93 100644 --- a/jderobot_drones/launch/as2_default_gazebo_sim.launch.py +++ b/jderobot_drones/launch/as2_default_gazebo_sim.launch.py @@ -1,6 +1,7 @@ """ as2_default_gazebo_sim.launch.py """ + import os from ament_index_python.packages import get_package_share_directory @@ -16,52 +17,77 @@ def generate_launch_description(): Launch aerostack2 nodes """ - as2_sim_config = os.path.join(get_package_share_directory( - 'jderobot_drones'), 'sim_config/gzsim/as2_config.yaml') + as2_sim_config = os.path.join( + get_package_share_directory("jderobot_drones"), + "sim_config/gzsim/as2_config.yaml", + ) platform_gazebo = IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('as2_platform_gazebo'), 'launch'), - '/platform_gazebo_launch.py']), + PythonLaunchDescriptionSource( + [ + os.path.join( + get_package_share_directory("as2_platform_gazebo"), "launch" + ), + "/platform_gazebo_launch.py", + ] + ), launch_arguments={ - 'namespace': LaunchConfiguration('namespace'), - 'platform_config_file': as2_sim_config, - 'create_bridges': 'false', + "namespace": LaunchConfiguration("namespace"), + "platform_config_file": as2_sim_config, + "create_bridges": "false", }.items(), ) state_estimator = IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('as2_state_estimator'), 'launch'), - '/ground_truth-state_estimator.launch.py']), + PythonLaunchDescriptionSource( + [ + os.path.join( + get_package_share_directory("as2_state_estimator"), "launch" + ), + "/ground_truth-state_estimator.launch.py", + ] + ), launch_arguments={ - 'namespace': LaunchConfiguration('namespace'), - 'config_file': as2_sim_config + "namespace": LaunchConfiguration("namespace"), + "config_file": as2_sim_config, }.items(), ) motion_controller = IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('as2_motion_controller'), 'launch'), - '/pid_speed_controller-motion_controller.launch.py']), + PythonLaunchDescriptionSource( + [ + os.path.join( + get_package_share_directory("as2_motion_controller"), "launch" + ), + "/pid_speed_controller-motion_controller.launch.py", + ] + ), launch_arguments={ - 'namespace': LaunchConfiguration('namespace'), - 'config_file': as2_sim_config + "namespace": LaunchConfiguration("namespace"), + "config_file": as2_sim_config, }.items(), ) behaviors = IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('as2_behaviors_motion'), 'launch'), - '/motion_behaviors_launch.py']), + PythonLaunchDescriptionSource( + [ + os.path.join( + get_package_share_directory("as2_behaviors_motion"), "launch" + ), + "/motion_behaviors_launch.py", + ] + ), launch_arguments={ - 'namespace': LaunchConfiguration('namespace'), - 'config_file': as2_sim_config + "namespace": LaunchConfiguration("namespace"), + "config_file": as2_sim_config, }.items(), ) - return LaunchDescription([ - DeclareLaunchArgument('namespace', default_value='drone0', - description='Drone namespace.'), - platform_gazebo, - state_estimator, - motion_controller, - # behaviors, - ]) + return LaunchDescription( + [ + DeclareLaunchArgument( + "namespace", default_value="drone0", description="Drone namespace." + ), + platform_gazebo, + state_estimator, + motion_controller, + # behaviors, + ] + ) diff --git a/jderobot_drones/launch/gz_sim.launch.py b/jderobot_drones/launch/gz_sim.launch.py index a0b6f688d..fcc832aff 100644 --- a/jderobot_drones/launch/gz_sim.launch.py +++ b/jderobot_drones/launch/gz_sim.launch.py @@ -1,6 +1,7 @@ """ gz_sim.launch.py """ + from __future__ import annotations import os @@ -16,39 +17,53 @@ def generate_launch_description(): """Entrypoint""" - launch_description = LaunchDescription([ - DeclareLaunchArgument('namespace', - description='Namespace to use'), - DeclareLaunchArgument('world_file', - description='Gazebo SDF world file'), - DeclareLaunchArgument('bridges_file', - description='ROS-GZ bridge YAML file'), - SetParameter(name='use_sim_time', value='true'), - IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('ros_gz_sim'), 'launch'), - '/gz_sim.launch.py']), - launch_arguments={'gz_args': ['-v 4 -s -r ', LaunchConfiguration('world_file')]}.items()), - Node( - package='ros_gz_bridge', - executable='parameter_bridge', - namespace=LaunchConfiguration('namespace'), - output='screen', - parameters=[ - {'config_file': LaunchConfiguration('bridges_file')}, - ] - ), - Node( - package='as2_gazebo_assets', - executable='ground_truth_bridge', - namespace=LaunchConfiguration('namespace'), - output='screen', - parameters=[ - {'name_space': LaunchConfiguration('namespace'), - 'pose_frame_id': 'earth', - 'twist_frame_id': [LaunchConfiguration('namespace'), '/base_link']}, - ] - ), - ]) + launch_description = LaunchDescription( + [ + DeclareLaunchArgument("namespace", description="Namespace to use"), + DeclareLaunchArgument("world_file", description="Gazebo SDF world file"), + DeclareLaunchArgument( + "bridges_file", description="ROS-GZ bridge YAML file" + ), + SetParameter(name="use_sim_time", value="true"), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + os.path.join( + get_package_share_directory("ros_gz_sim"), "launch" + ), + "/gz_sim.launch.py", + ] + ), + launch_arguments={ + "gz_args": ["-v 4 -s -r ", LaunchConfiguration("world_file")] + }.items(), + ), + Node( + package="ros_gz_bridge", + executable="parameter_bridge", + namespace=LaunchConfiguration("namespace"), + output="screen", + parameters=[ + {"config_file": LaunchConfiguration("bridges_file")}, + ], + ), + Node( + package="as2_gazebo_assets", + executable="ground_truth_bridge", + namespace=LaunchConfiguration("namespace"), + output="screen", + parameters=[ + { + "name_space": LaunchConfiguration("namespace"), + "pose_frame_id": "earth", + "twist_frame_id": [ + LaunchConfiguration("namespace"), + "/base_link", + ], + }, + ], + ), + ] + ) return launch_description diff --git a/jderobot_drones/setup.py b/jderobot_drones/setup.py index 5f3b68743..1640e0506 100644 --- a/jderobot_drones/setup.py +++ b/jderobot_drones/setup.py @@ -2,29 +2,33 @@ from setuptools import setup from glob import glob -package_name = 'jderobot_drones' +package_name = "jderobot_drones" setup( name=package_name, - version='0.0.0', + version="0.0.0", packages=[package_name], data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), - (os.path.join('share', package_name, 'sim_config/px4_classic'), glob('sim_config/px4_classic/*')), - (os.path.join('share', package_name, 'sim_config/gzsim'), glob('sim_config/gzsim/*')) + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + (os.path.join("share", package_name, "launch"), glob("launch/*.launch.py")), + ( + os.path.join("share", package_name, "sim_config/px4_classic"), + glob("sim_config/px4_classic/*"), + ), + ( + os.path.join("share", package_name, "sim_config/gzsim"), + glob("sim_config/gzsim/*"), + ), ], - install_requires=['setuptools'], + install_requires=["setuptools"], zip_safe=True, - maintainer='JdeRobot', - maintainer_email='pawanw17@gmail.com', - description='Jde Robot drones package to integrate Aerostack2 and pixhawk with Robotics Academy', - license='MIT', - tests_require=['pytest'], + maintainer="JdeRobot", + maintainer_email="pawanw17@gmail.com", + description="Jde Robot drones package to integrate Aerostack2 and pixhawk with Robotics Academy", + license="MIT", + tests_require=["pytest"], entry_points={ - 'console_scripts': [ - ], + "console_scripts": [], }, ) diff --git a/jderobot_drones/test/test_copyright.py b/jderobot_drones/test/test_copyright.py index 97a39196e..ceffe896d 100644 --- a/jderobot_drones/test/test_copyright.py +++ b/jderobot_drones/test/test_copyright.py @@ -17,9 +17,11 @@ # Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.skip( + reason="No copyright header has been placed in the generated source file." +) @pytest.mark.copyright @pytest.mark.linter def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/jderobot_drones/test/test_flake8.py b/jderobot_drones/test/test_flake8.py index 27ee1078f..ee79f31ac 100644 --- a/jderobot_drones/test/test_flake8.py +++ b/jderobot_drones/test/test_flake8.py @@ -20,6 +20,6 @@ @pytest.mark.linter def test_flake8(): rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) + assert rc == 0, "Found %d code style errors / warnings:\n" % len( + errors + ) + "\n".join(errors) diff --git a/jderobot_drones/test/test_pep257.py b/jderobot_drones/test/test_pep257.py index b234a3840..a2c3deb8e 100644 --- a/jderobot_drones/test/test_pep257.py +++ b/jderobot_drones/test/test_pep257.py @@ -19,5 +19,5 @@ @pytest.mark.linter @pytest.mark.pep257 def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings" diff --git a/scripts/outdated/test/camera.py b/scripts/outdated/test/camera.py index a6d01d4d4..c92faf64e 100755 --- a/scripts/outdated/test/camera.py +++ b/scripts/outdated/test/camera.py @@ -7,9 +7,10 @@ from cv_bridge import CvBridge, CvBridgeError -MAXRANGE = 8 #max length received from imageD +MAXRANGE = 8 # max length received from imageD MINRANGE = 0 + def imageMsg2Image(img, bridge): image = Image() @@ -17,16 +18,17 @@ def imageMsg2Image(img, bridge): image.width = img.width image.height = img.height image.format = "BGR8" - image.timeStamp = img.header.stamp.sec + (img.header.stamp.nanosec *1e-9) - cv_image=0 - if (img.encoding[-2:] == "C1"): + image.timeStamp = img.header.stamp.sec + (img.header.stamp.nanosec * 1e-9) + cv_image = 0 + if img.encoding[-2:] == "C1": gray_img_buff = bridge.imgmsg_to_cv2(img, img.encoding) - cv_image = depthToRGB8(gray_img_buff, img.encoding) + cv_image = depthToRGB8(gray_img_buff, img.encoding) else: cv_image = bridge.imgmsg_to_cv2(img, "bgr8") image.data = cv_image return image + import numpy as np @@ -36,54 +38,60 @@ def __init__(self): self.height = 3 # Image height [pixels] self.width = 3 # Image width [pixels] - self.timeStamp = 0 # Time stamp [s] */ - self.format = "" # Image format string (RGB8, BGR,...) - self.data = np.zeros((self.height, self.width, 3), np.uint8) # The image data itself + self.timeStamp = 0 # Time stamp [s] */ + self.format = "" # Image format string (RGB8, BGR,...) + self.data = np.zeros( + (self.height, self.width, 3), np.uint8 + ) # The image data itself self.data.shape = self.height, self.width, 3 - def __str__(self): - s = "Image: {\n height: " + str(self.height) + "\n width: " + str(self.width) - s = s + "\n format: " + self.format + "\n timeStamp: " + str(self.timeStamp) + s = ( + "Image: {\n height: " + + str(self.height) + + "\n width: " + + str(self.width) + ) + s = s + "\n format: " + self.format + "\n timeStamp: " + str(self.timeStamp) s = s + "\n data: " + str(self.data) + "\n}" - return s + return s class ListenerCamera(Node): - + def __init__(self, topic): super().__init__("camera_subscriber_node") - + self.topic = topic self.data = Image() self.sub = None self.lock = threading.Lock() - + self.bridge = CvBridge() self.start() - - def __callback (self, img): + + def __callback(self, img): image = imageMsg2Image(img, self.bridge) self.lock.acquire() self.data = image self.lock.release() - + def stop(self): self.sub.unregister() - def start (self): + def start(self): self.sub = self.create_subscription(ImageROS, self.topic, self.__callback, 10) - + def getImage(self): - + self.lock.acquire() image = self.data self.lock.release() - + return image - def hasproxy (self): - return hasattr(self,"sub") and self.sub + def hasproxy(self): + return hasattr(self, "sub") and self.sub diff --git a/scripts/outdated/test/hal.py b/scripts/outdated/test/hal.py index 051266849..1f4590d86 100755 --- a/scripts/outdated/test/hal.py +++ b/scripts/outdated/test/hal.py @@ -1,4 +1,3 @@ - import rclpy import cv2 import threading @@ -10,6 +9,7 @@ from shared.image import SharedImage from shared.value import SharedValue + # Hardware Abstraction Layer class HAL: IMG_WIDTH = 320 @@ -20,8 +20,8 @@ def __init__(self): # Shared memory variables self.shared_image = SharedImage("halimage") - #self.shared_v = SharedValue("velocity") - #self.shared_w = SharedValue("angular") + # self.shared_v = SharedValue("velocity") + # self.shared_w = SharedValue("angular") # ROS Topics self.camera = ListenerCamera("/depth_camera/image_raw") @@ -41,7 +41,7 @@ def getImage(self): # Set the velocity def setV(self, velocity): self.motors.sendV(velocity) - + def setW(self, velocity): self.motors.sendW(velocity) @@ -58,7 +58,7 @@ def __init__(self, update_function): self.update_function = update_function def run(self): - while(True): + while True: start_time = datetime.now() self.update_function() @@ -68,5 +68,5 @@ def run(self): dt = finish_time - start_time ms = (dt.days * 24 * 60 * 60 + dt.seconds) * 1000 + dt.microseconds / 1000.0 - if(ms < self.time_cycle): + if ms < self.time_cycle: time.sleep((self.time_cycle - ms) / 1000.0) diff --git a/scripts/outdated/test/motors.py b/scripts/outdated/test/motors.py index a341a82ca..76a116fd6 100755 --- a/scripts/outdated/test/motors.py +++ b/scripts/outdated/test/motors.py @@ -6,7 +6,6 @@ from .threadPublisher import ThreadPublisher - def cmdvel2Twist(vel): tw = Twist() @@ -20,28 +19,28 @@ def cmdvel2Twist(vel): return tw -class CMDVel (): +class CMDVel: def __init__(self): - self.vx = 0 # vel in x[m/s] (use this for V in wheeled robots) - self.vy = 0 # vel in y[m/s] - self.vz = 0 # vel in z[m/s] - self.ax = 0 # angular vel in X axis [rad/s] - self.ay = 0 # angular vel in X axis [rad/s] - self.az = 0 # angular vel in Z axis [rad/s] (use this for W in wheeled robots) - self.timeStamp = 0 # Time stamp [s] - + self.vx = 0 # vel in x[m/s] (use this for V in wheeled robots) + self.vy = 0 # vel in y[m/s] + self.vz = 0 # vel in z[m/s] + self.ax = 0 # angular vel in X axis [rad/s] + self.ay = 0 # angular vel in X axis [rad/s] + self.az = 0 # angular vel in Z axis [rad/s] (use this for W in wheeled robots) + self.timeStamp = 0 # Time stamp [s] def __str__(self): s = "CMDVel: {\n vx: " + str(self.vx) + "\n vy: " + str(self.vy) - s = s + "\n vz: " + str(self.vz) + "\n ax: " + str(self.ax) + s = s + "\n vz: " + str(self.vz) + "\n ax: " + str(self.ax) s = s + "\n ay: " + str(self.ay) + "\n az: " + str(self.az) - s = s + "\n timeStamp: " + str(self.timeStamp) + "\n}" - return s + s = s + "\n timeStamp: " + str(self.timeStamp) + "\n}" + return s + class PublisherMotors(Node): - + def __init__(self, topic, maxV, maxW): super().__init__("FollowLineF1") self.maxW = maxW @@ -57,32 +56,29 @@ def __init__(self, topic, maxV, maxW): self.thread.daemon = True self.start() - - def publish (self): + + def publish(self): self.lock.acquire() tw = cmdvel2Twist(self.data) self.lock.release() self.pub.publish(tw) - + def stop(self): - + self.kill_event.set() self.pub.unregister() - def start (self): + def start(self): self.kill_event.clear() self.thread.start() - - def getMaxW(self): return self.maxW def getMaxV(self): return self.maxV - def sendVelocities(self, vel): diff --git a/scripts/outdated/test/test_program.py b/scripts/outdated/test/test_program.py index ed40d8b2b..156b712cf 100755 --- a/scripts/outdated/test/test_program.py +++ b/scripts/outdated/test/test_program.py @@ -1,4 +1,5 @@ from hal import HAL + curro = HAL() diff --git a/scripts/test/check_device.py b/scripts/test/check_device.py index cca8faee4..0108aa57c 100755 --- a/scripts/test/check_device.py +++ b/scripts/test/check_device.py @@ -2,22 +2,27 @@ import os import sys + def check_device(device_path): try: return stat.S_ISCHR(os.lstat(device_path)[stat.ST_MODE]) except: return False + def usage(): - print("usage: python3 check_device.py < device path >\n\t use this program to check if a given device exists in a system") + print( + "usage: python3 check_device.py < device path >\n\t use this program to check if a given device exists in a system" + ) sys.exit(1) -if (len(sys.argv) != 2): + +if len(sys.argv) != 2: usage() device_path = sys.argv[1] print(check_device(device_path)) -if (check_device(device_path)): +if check_device(device_path): sys.exit(0) else: sys.exit(1) diff --git a/utils/model_teleoperator.py b/utils/model_teleoperator.py index ab1c410de..053809213 100755 --- a/utils/model_teleoperator.py +++ b/utils/model_teleoperator.py @@ -4,6 +4,7 @@ import sys import time + class ModelTeleoperator: def __init__(self, host): @@ -17,20 +18,30 @@ def __init__(self, host): # Function to read the message from websocket def get_message(self, client, server, message): try: - sys.stdout = open("/teleop_log.txt","w") - if (message[:4] == "#key"): - key = message[message.find('_')+1:] + sys.stdout = open("/teleop_log.txt", "w") + if message[:4] == "#key": + key = message[message.find("_") + 1 :] print("\nPRESSED KEY:" + str(key)) if key == "w": - self.model_client.sendto(str.encode("UVF"), self.model_address) # User Velocity Forward + self.model_client.sendto( + str.encode("UVF"), self.model_address + ) # User Velocity Forward elif key == "s": - self.model_client.sendto(str.encode("UVB"), self.model_address) # User Velocity Backward + self.model_client.sendto( + str.encode("UVB"), self.model_address + ) # User Velocity Backward elif key == "a": - self.model_client.sendto(str.encode("UAL"), self.model_address) # User Angular Left + self.model_client.sendto( + str.encode("UAL"), self.model_address + ) # User Angular Left elif key == "d": - self.model_client.sendto(str.encode("UAR"), self.model_address) # User Angular Right + self.model_client.sendto( + str.encode("UAR"), self.model_address + ) # User Angular Right elif key == "x": - self.model_client.sendto(str.encode("US-"), self.model_address) # User Stop model + self.model_client.sendto( + str.encode("US-"), self.model_address + ) # User Stop model except KeyboardInterrupt: sys.stdout.close() # close file.txt @@ -41,8 +52,8 @@ def handle_close(self, client, server): self.client = None self.server.allow_new_connections() try: - sys.stdout = open("/teleop_log.txt","w") - print(client, 'closed') + sys.stdout = open("/teleop_log.txt", "w") + print(client, "closed") except KeyboardInterrupt: sys.stdout.close() # close file.txt sys.stdout = sys.__stdout__ @@ -51,16 +62,15 @@ def handle_close(self, client, server): def get_client(self, client, server): self.client = client self.server.deny_new_connections() - self.model_client.sendto(str.encode("US-"), self.model_address) # User Stop + self.model_client.sendto(str.encode("US-"), self.model_address) # User Stop try: - sys.stdout = open("/teleop_log.txt","w") - print(client, 'connected') + sys.stdout = open("/teleop_log.txt", "w") + print(client, "connected") except KeyboardInterrupt: - print(client, 'error connecting with teleoperator client') + print(client, "error connecting with teleoperator client") sys.stdout.close() # close file.txt sys.stdout = sys.__stdout__ - # Activate the server def run_server(self): self.server = WebsocketServer(port=7164, host=self.host) @@ -68,7 +78,7 @@ def run_server(self): self.server.set_fn_message_received(self.get_message) self.server.set_fn_client_left(self.handle_close) - home_dir = os.path.expanduser('~') + home_dir = os.path.expanduser("~") logged = False while not logged: @@ -88,4 +98,4 @@ def run_server(self): if __name__ == "__main__": host = sys.argv[1] server = ModelTeleoperator(host) - server.run_server() \ No newline at end of file + server.run_server()