Added worlds and models for autopark migration

CustomRobots/CMakeLists.txt

@@ -118,8 +118,10 @@ install(
     autoparking/launch
     autoparking/models
     autoparking/worlds
-    autopark_harmonic/worlds
+    # AUTOPARK_HARMONIC
     autopark_harmonic/models
+    autopark_harmonic/params
+    autopark_harmonic/worlds
     # DRONE_ASSETS
     drone_assets/models
     drone_assets/worlds

CustomRobots/autopark_harmonic/params/waymo.yaml

@@ -0,0 +1,48 @@
+# gz topic published by DiffDrive plugin
+- ros_topic_name: "odom"
+  gz_topic_name: "/waymo/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: "/waymo/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: "/waymo/lidar"
+  gz_topic_name: "/waymo/lidar"
+  ros_type_name: "sensor_msgs/msg/LaserScan"
+  gz_type_name: "gz.msgs.LaserScan"
+  direction: GZ_TO_ROS
+
+# gz topic published by Sensors plugin (LIDAR)
+- ros_topic_name: "/waymo/ray"
+  gz_topic_name: "/waymo/ray"
+  ros_type_name: "sensor_msgs/msg/LaserScan"
+  gz_type_name: "gz.msgs.LaserScan"
+  direction: GZ_TO_ROS
+
+# gz topic published by Sensors plugin (Camera)
+- ros_topic_name: "/waymo/camera_right_side"
+  gz_topic_name: "/waymo/camera_right_side"
+  ros_type_name: "sensor_msgs/msg/CameraInfo"
+  gz_type_name: "gz.msgs.CameraInfo"
+  direction: GZ_TO_ROS
+
+# gz topic published by Sensors plugin (Camera)
+- ros_topic_name: "/waymo/camera_left_side"
+  gz_topic_name: "/waymo/camera_left_side"
+  ros_type_name: "sensor_msgs/msg/CameraInfo"
+  gz_type_name: "gz.msgs.CameraInfo"
+  direction: GZ_TO_ROS
+
+# gz topic published by Sensors plugin (Camera)
+- ros_topic_name: "/waymo/camera_rear"
+  gz_topic_name: "/waymo/camera_rear"
+  ros_type_name: "sensor_msgs/msg/CameraInfo"
+  gz_type_name: "gz.msgs.CameraInfo"
+  direction: GZ_TO_ROS

Launchers/autopark_battery.launch.py

@@ -18,57 +18,88 @@
 
 def generate_launch_description():
 
-    # Rutas a directorios
-    package_dir = get_package_share_directory("custom_robots") 
+    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")
 
-    # Directorio de los launchers específicos del robot (para robot_state_publisher)
-    robot_launch_dir = "/opt/jderobot/Launchers/autopark_battery" 
+    robot_launch_dir = "/opt/jderobot/Launchers/autopark_battery"
 
     use_sim_time = LaunchConfiguration("use_sim_time", default="true")
-
-
-    # Nombre y ruta del mundo
-    world_file_name = "autopark_battery.world" 
-    worlds_dir = "/opt/jderobot/Worlds" 
+    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 = "autopark_battery.world"
+    worlds_dir = "/opt/jderobot/Worlds"
     world_path = os.path.join(worlds_dir, world_file_name)
 
-    # Iniciar el servidor de Gazebo/Ignition y cargar el mundo
     gazebo_server = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(ros_gz_sim, "launch", "gz_sim.launch.py")
         ),
         launch_arguments={
-            "gz_args": ["-r -s -v4 ", world_path], 
+            "gz_args": ["-r -s -v4 ", world_path],
             "on_exit_shutdown": "true",
         }.items(),
     )
 
-    # Lanzar el robot_state_publisher (Necesario para las transformaciones del robot)
-    robot_state_publisher_cmd = IncludeLaunchDescription(
+    declare_x_cmd = DeclareLaunchArgument("x", default_value="1.0")
+
+    declare_y_cmd = DeclareLaunchArgument("y", default_value="-1.5")
+
+    declare_z_cmd = DeclareLaunchArgument("z", default_value="7.1")
+
+    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")
+
+    # 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(),
+    # )
+
+    spawn_robot_cmd = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            os.path.join(robot_launch_dir, "robot_state_publisher.launch.py")
+            os.path.join(robot_launch_dir, "spawn_robot.launch.py")
         ),
-        launch_arguments={"use_sim_time": use_sim_time}.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",
     )
 
-    # Configuración de LaunchDescription
     ld = LaunchDescription()
 
-    # Variables de entorno
     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)
-
-    # Servidor de simulación
     ld.add_action(gazebo_server)
-
-
-    # Componentes del robot
-    ld.add_action(robot_state_publisher_cmd)
+    # 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

Launchers/autopark_battery/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 = "waymo"
+    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", "waymo.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/autopark_line.launch.py

@@ -18,57 +18,88 @@
 
 def generate_launch_description():
 
-    # Rutas a directorios
-    package_dir = get_package_share_directory("custom_robots") 
+    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")
 
-    # Directorio de los launchers específicos del robot (para robot_state_publisher)
-    robot_launch_dir = "/opt/jderobot/Launchers/autopark_line" 
+    robot_launch_dir = "/opt/jderobot/Launchers/autopark_line"
 
     use_sim_time = LaunchConfiguration("use_sim_time", default="true")
-
-
-    # Nombre y ruta del mundo
-    world_file_name = "autopark_line.world" 
-    worlds_dir = "/opt/jderobot/Worlds" 
+    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 = "autopark_line.world"
+    worlds_dir = "/opt/jderobot/Worlds"
     world_path = os.path.join(worlds_dir, world_file_name)
 
-    # Iniciar el servidor de Gazebo/Ignition y cargar el mundo
     gazebo_server = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(ros_gz_sim, "launch", "gz_sim.launch.py")
         ),
         launch_arguments={
-            "gz_args": ["-r -s -v4 ", world_path], 
+            "gz_args": ["-r -s -v4 ", world_path],
             "on_exit_shutdown": "true",
         }.items(),
     )
 
-    # Lanzar el robot_state_publisher (Necesario para las transformaciones del robot)
-    robot_state_publisher_cmd = IncludeLaunchDescription(
+    declare_x_cmd = DeclareLaunchArgument("x", default_value="1.0")
+
+    declare_y_cmd = DeclareLaunchArgument("y", default_value="-1.5")
+
+    declare_z_cmd = DeclareLaunchArgument("z", default_value="7.1")
+
+    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")
+
+    # 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(),
+    # )
+
+    spawn_robot_cmd = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            os.path.join(robot_launch_dir, "robot_state_publisher.launch.py")
+            os.path.join(robot_launch_dir, "spawn_robot.launch.py")
         ),
-        launch_arguments={"use_sim_time": use_sim_time}.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",
     )
 
-    # Configuración de LaunchDescription
     ld = LaunchDescription()
 
-    # Variables de entorno
     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)
-
-    # Servidor de simulación
     ld.add_action(gazebo_server)
-
-
-    # Componentes del robot
-    ld.add_action(robot_state_publisher_cmd)
+    # 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