diff --git a/CustomRobots/CMakeLists.txt b/CustomRobots/CMakeLists.txt index b08432108..0d69f3659 100755 --- a/CustomRobots/CMakeLists.txt +++ b/CustomRobots/CMakeLists.txt @@ -86,6 +86,7 @@ install( f1/params # ROOMBA roomba_robot/models + roomba_robot/params # ACKERMAN CAR ackermann_cars/models # CARS diff --git a/CustomRobots/roomba_robot/params/roombaROS.yaml b/CustomRobots/roomba_robot/params/roombaROS.yaml new file mode 100644 index 000000000..d0dd5d8b4 --- /dev/null +++ b/CustomRobots/roomba_robot/params/roombaROS.yaml @@ -0,0 +1,39 @@ +# gz topic published by DiffDrive plugin +- ros_topic_name: "odom" + gz_topic_name: "/roombaROS/odom" + ros_type_name: "nav_msgs/msg/Odometry" + gz_type_name: "gz.msgs.Odometry" + direction: GZ_TO_ROS + + +# gz topic subscribed to by DiffDrive plugin +- ros_topic_name: "cmd_vel" + gz_topic_name: "/roombaROS/cmd_vel" + ros_type_name: "geometry_msgs/msg/Twist" + gz_type_name: "gz.msgs.Twist" + direction: ROS_TO_GZ + +# gz topic published by Sensors plugin (LIDAR) +- ros_topic_name: "/roombaROS/laser/scan" + gz_topic_name: "/roombaROS/laser/scan" + ros_type_name: "sensor_msgs/msg/LaserScan" + gz_type_name: "gz.msgs.LaserScan" + direction: GZ_TO_ROS + +- ros_topic_name: "/roombaROS/events/center_bumper" + gz_topic_name: "/roombaROS/events/center_bumper" + ros_type_name: "ros_gz_interfaces/msg/Contacts" + gz_type_name: "gz.msgs.Contacts" + direction: GZ_TO_ROS + +- ros_topic_name: "/roombaROS/events/right_bumper" + gz_topic_name: "/roombaROS/events/right_bumper" + ros_type_name: "ros_gz_interfaces/msg/Contacts" + gz_type_name: "gz.msgs.Contacts" + direction: GZ_TO_ROS + +- ros_topic_name: "/roombaROS/events/left_bumper" + gz_topic_name: "/roombaROS/events/left_bumper" + ros_type_name: "ros_gz_interfaces/msg/Contacts" + gz_type_name: "gz.msgs.Contacts" + direction: GZ_TO_ROS \ No newline at end of file diff --git a/Launchers/localized_vacuum_cleaner/spawn_robot.launch.py b/Launchers/localized_vacuum_cleaner/spawn_robot.launch.py new file mode 100644 index 000000000..f90a8b557 --- /dev/null +++ b/Launchers/localized_vacuum_cleaner/spawn_robot.launch.py @@ -0,0 +1,107 @@ +# Copyright 2019 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + # Get the urdf file + model_folder = "roombaROS" + urdf_path = os.path.join( + get_package_share_directory("custom_robots"), + "models", + model_folder, + "model.sdf", + ) + + # Launch configuration variables specific to simulation + # x_pose = LaunchConfiguration('x_pose', default='1.0') + # y_pose = LaunchConfiguration('y_pose', default='-1.5') + # z_pose = LaunchConfiguration('z_pose', default='7.1') + + # Declare the launch arguments + # declare_x_position_cmd = DeclareLaunchArgument( + # 'x_pose', default_value='1.0', + # description='Specify namespace of the robot') + + # 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') + + # start_gazebo_ros_spawner_cmd = Node( + # package='ros_gz_sim', + # executable='create', + # arguments=[ + # '-name', 'waffle', + # '-file', urdf_path, + # '-x', x_pose, + # '-y', y_pose, + # '-z', z_pose + # ], + # output='screen', + # ) + + bridge_params = os.path.join( + get_package_share_directory("custom_robots"), "params", "roombaROS.yaml" + ) + + start_gazebo_ros_bridge_cmd = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + arguments=[ + "--ros-args", + "-p", + f"config_file:={bridge_params}", + ], + output="screen", + ) + + # start_gazebo_ros_image_bridge_cmd = Node( + # 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", + # ) + + ld = LaunchDescription() + + # Declare the launch options + # ld.add_action(declare_x_position_cmd) + # ld.add_action(declare_y_position_cmd) + # ld.add_action(declare_z_position_cmd) + + # Add any conditioned actions + # ld.add_action(start_gazebo_ros_spawner_cmd) + ld.add_action(start_gazebo_ros_bridge_cmd) + # 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 diff --git a/Launchers/vacuum_cleaner.launch.py b/Launchers/vacuum_cleaner.launch.py index d07ffd8f4..16e38ab96 100755 --- a/Launchers/vacuum_cleaner.launch.py +++ b/Launchers/vacuum_cleaner.launch.py @@ -1,85 +1,105 @@ -"""Launch file for starting the vacuum cleaner simulation in Gazebo.""" - import os + +from ament_index_python.packages import get_package_share_directory + from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.conditions import IfCondition +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + SetEnvironmentVariable, + AppendEnvironmentVariable, +) from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, Command +from launch_ros.actions import Node from launch.substitutions import LaunchConfiguration -from launch_ros.substitutions import FindPackageShare +from launch_ros.actions import Node def generate_launch_description(): - """Generate the launch description for the vacuum cleaner Gazebo simulation.""" - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + 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") + + gazebo_models_path = os.path.join(package_dir, "models") - # Set the path to the Turtlebot2 ROS package - pkg_share_dir = FindPackageShare(package="custom_robots").find("custom_robots") + robot_launch_dir = "/opt/jderobot/Launchers/localized_vacuum_cleaner" - world_file_name = "roomba_1_house.world" + 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 = "roomba_1_house_harmonic.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)}" + 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(), ) - ########### 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_x_cmd = DeclareLaunchArgument("x", default_value="1.0") - declare_use_sim_time_cmd = DeclareLaunchArgument( - name="use_sim_time", - default_value="true", - description="Use simulation (Gazebo) clock if true", - ) + declare_y_cmd = DeclareLaunchArgument("y", default_value="-1.5") - declare_use_simulator_cmd = DeclareLaunchArgument( - name="use_simulator", - default_value="True", - description="Whether to start the simulator", - ) + declare_z_cmd = DeclareLaunchArgument("z", default_value="7.1") - declare_world_cmd = DeclareLaunchArgument( - name="world", - default_value=world_path, - description="Full path to the world model file to load", - ) + declare_roll_cmd = DeclareLaunchArgument("R", default_value="0.0") + + declare_pitch_cmd = DeclareLaunchArgument("P", default_value="0.0") + + declare_yaw_cmd = DeclareLaunchArgument("Y", default_value="1.57079") - # Specify the actions + # robot_state_publisher_cmd = IncludeLaunchDescription( + # PythonLaunchDescriptionSource( + # os.path.join(robot_launch_dir, "robot_state_publisher.launch.py") + # ), + # launch_arguments={"use_sim_time": use_sim_time}.items(), + # ) - # Start Gazebo server - start_gazebo_server_cmd = IncludeLaunchDescription( + spawn_robot_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(pkg_gazebo_ros, "launch", "gzserver.launch.py") + os.path.join(robot_launch_dir, "spawn_robot.launch.py") ), - condition=IfCondition(use_simulator), - launch_arguments={"world": world, "pause": "true"}.items(), + launch_arguments={"x_pose": x_pose, "y_pose": y_pose, "z_pose": z_pose}.items(), ) - # Create the launch description and populate - ld = LaunchDescription() + world_entity_cmd = Node( + package="ros_gz_sim", + executable="create", + arguments=["-name", "world", "-file", world_path], + output="screen", + ) - # 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 = LaunchDescription() - # Add any actions - ld.add_action(start_gazebo_server_cmd) + 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) + ld.add_action(declare_x_cmd) + ld.add_action(declare_y_cmd) + ld.add_action(declare_z_cmd) + ld.add_action(declare_roll_cmd) + ld.add_action(declare_pitch_cmd) + ld.add_action(declare_yaw_cmd) + ld.add_action(world_entity_cmd) + # ld.add_action(robot_state_publisher_cmd) + ld.add_action(spawn_robot_cmd) return ld diff --git a/Launchers/visualization/localized_vacuum_cleaner.config b/Launchers/visualization/localized_vacuum_cleaner.config new file mode 100644 index 000000000..e69de29bb diff --git a/Worlds/roomba_1_house_harmonic.world b/Worlds/roomba_1_house_harmonic.world new file mode 100644 index 000000000..666c85161 --- /dev/null +++ b/Worlds/roomba_1_house_harmonic.world @@ -0,0 +1,54 @@ + + + + + + + + + + + + ogre2 + + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + true + false + + + + model://roombaROS + -1 1.5 0 0 0 0 + + + + model://house_int2 + 0 0 0 0 0 0 + + + + model://ground_plane_sincolor + + + + 1 + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + -0.5 0.1 -0.9 + false + + + + \ No newline at end of file diff --git a/database/universes.sql b/database/universes.sql index 8147c3422..244f24cdf 100644 --- a/database/universes.sql +++ b/database/universes.sql @@ -198,7 +198,7 @@ COPY public.worlds (id, name, launch_file_path, tools_config, ros_version, type, 21 Restaurant /opt/jderobot/Launchers/restaurant.launch.py None ROS2 gazebo {0.0,0.0,0.0,0.0,0.0,0.0} 22 Simple Ackermann Circuit /opt/jderobot/Launchers/simple_circuit_ackermann.launch.py None ROS2 gazebo {0.0,0.0,0.0,0.0,0.0,0.0} 23 Simple Circuit /opt/jderobot/Launchers/simple_circuit.launch.py None ROS2 gazebo {0.0,0.0,0.0,0.0,0.0,0.0} -24 Vacuums House /opt/jderobot/Launchers/vacuum_cleaner.launch.py None ROS2 gazebo {0.0,0.0,0.0,0.0,0.0,0.0} +24 Vacuums House /opt/jderobot/Launchers/vacuum_cleaner.launch.py None ROS2 gz {0.0,0.0,0.0,0.0,0.0,0.0} 25 Vacuums House Markers /opt/jderobot/Launchers/marker_visual_loc.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/marker_visual_loc.config"} ROS2 gz {1,-1.5,0.6,0,0,0} 26 Vacuums House Roof /opt/jderobot/Launchers/montecarlo_visual_loc.launch.py None ROS2 gazebo {0.0,0.0,0.0,0.0,0.0,0.0} 27 Warehouse 1 /opt/jderobot/Launchers/amazon_robot.launch.py None ROS2 gazebo {0.0,0.0,0.0,0.0,0.0,0.0}