Localized vacuum cleaner harmonic

CustomRobots/CMakeLists.txt

@@ -86,6 +86,7 @@ install(
     f1/params
     # ROOMBA
     roomba_robot/models
+    roomba_robot/params
     # ACKERMAN CAR
     ackermann_cars/models
     # CARS

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

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

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

Worlds/roomba_1_house_harmonic.world

@@ -0,0 +1,54 @@
+<?xml version="1.0" ?>
+
+<sdf version="1.8">
+  <world name="default">
+    <plugin
+      filename="gz-sim-physics-system"
+      name="gz::sim::systems::Physics">
+    </plugin>
+    <plugin
+      filename="gz-sim-user-commands-system"
+      name="gz::sim::systems::UserCommands">
+    </plugin>
+    <plugin
+      filename="gz-sim-scene-broadcaster-system"
+      name="gz::sim::systems::SceneBroadcaster">
+    </plugin>
+    <plugin
+      filename="gz-sim-sensors-system"
+      name="gz::sim::systems::Sensors">
+      <render_engine>ogre2</render_engine>
+    </plugin>
+
+    <scene>
+      <ambient>0.4 0.4 0.4 1</ambient>
+      <background>0.7 0.7 0.7 1</background>
+      <shadows>true</shadows>
+      <grid>false</grid>
+    </scene>
+
+    <include>
+      <uri>model://roombaROS</uri>
+      <pose>-1 1.5 0 0 0 0</pose>
+    </include>
+
+    <include>
+      <uri>model://house_int2</uri>
+      <pose>0 0 0 0 0 0</pose>
+    </include>
+
+    <include>
+      <uri>model://ground_plane_sincolor</uri>
+    </include>
+
+    <light type="directional" name="sun">
+      <cast_shadows>1</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <direction>-0.5 0.1 -0.9</direction>
+      <visualize>false</visualize>
+    </light>
+
+  </world>
+</sdf>

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}