From efb0b49d82a3827874d4df3b977956653a2f36c7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alberto=20Le=C3=B3n=20Luengo?= Date: Mon, 6 Oct 2025 11:19:45 +0200 Subject: [PATCH 01/35] Adding first changes in global navigation migration --- CustomRobots/CMakeLists.txt | 1 + .../models/taxi_holo_ROS/model.sdf | 29 +++- .../taxi_navigator/params/taxi_holo_ROS.yaml | 28 ++++ .../global_navigation/spawn_robot.launch.py | 107 +++++++++++++ Launchers/taxi_navigator.launch.py | 135 +++++++++------- Launchers/visualization/taxi_navigator.config | 90 +++++++++++ Worlds/taxi_navigation_city_large.world | 149 +++++++++++++++--- database/universes.sql | 8 +- 8 files changed, 462 insertions(+), 85 deletions(-) create mode 100644 CustomRobots/taxi_navigator/params/taxi_holo_ROS.yaml create mode 100644 Launchers/global_navigation/spawn_robot.launch.py create mode 100644 Launchers/visualization/taxi_navigator.config diff --git a/CustomRobots/CMakeLists.txt b/CustomRobots/CMakeLists.txt index 79af8fcf0..48fc1bd06 100755 --- a/CustomRobots/CMakeLists.txt +++ b/CustomRobots/CMakeLists.txt @@ -124,6 +124,7 @@ install( drone_assets/bridges # GLOBAL_NAVIGATION taxi_navigator/models + taxi_navigator/params # 3D RECONSTRUCTION 3d_reconstruction/launch 3d_reconstruction/models diff --git a/CustomRobots/taxi_navigator/models/taxi_holo_ROS/model.sdf b/CustomRobots/taxi_navigator/models/taxi_holo_ROS/model.sdf index bcced90f2..3d834a9ef 100644 --- a/CustomRobots/taxi_navigator/models/taxi_holo_ROS/model.sdf +++ b/CustomRobots/taxi_navigator/models/taxi_holo_ROS/model.sdf @@ -31,12 +31,37 @@ - + + + + + + + + + + taxi_holo/cmd_vel + odom + + 20.0 + xf1 + + + + odom + xf1 + 20.0 + taxi_holo/odom + 3 + + \ No newline at end of file diff --git a/CustomRobots/taxi_navigator/params/taxi_holo_ROS.yaml b/CustomRobots/taxi_navigator/params/taxi_holo_ROS.yaml new file mode 100644 index 000000000..e8e8b10a5 --- /dev/null +++ b/CustomRobots/taxi_navigator/params/taxi_holo_ROS.yaml @@ -0,0 +1,28 @@ +# gz topic published by DiffDrive plugin +- ros_topic_name: "taxi_holo/odom" + gz_topic_name: "taxi_holo/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: "taxi_holo/cmd_vel" + gz_topic_name: "taxi_holo/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: "taxi_holo/laser/scan" + gz_topic_name: "taxi_holo/laser/scan" + 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: "taxi_holo/camera/camera_info" + gz_topic_name: "taxi_holo/camera/camera_info" + ros_type_name: "sensor_msgs/msg/CameraInfo" + gz_type_name: "gz.msgs.CameraInfo" + direction: GZ_TO_ROS \ No newline at end of file diff --git a/Launchers/global_navigation/spawn_robot.launch.py b/Launchers/global_navigation/spawn_robot.launch.py new file mode 100644 index 000000000..4d60d9f44 --- /dev/null +++ b/Launchers/global_navigation/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 = "turtlebot3_waffle" + 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", "taxi_holo_ROS.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/taxi_navigator.launch.py b/Launchers/taxi_navigator.launch.py index cc45d0e9b..e4ee151e0 100755 --- a/Launchers/taxi_navigator.launch.py +++ b/Launchers/taxi_navigator.launch.py @@ -1,84 +1,105 @@ 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, UnlessCondition +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + SetEnvironmentVariable, + AppendEnvironmentVariable, +) from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import Command, LaunchConfiguration, PythonExpression +from launch.substitutions import LaunchConfiguration, Command +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration 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") + 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 this package. - pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + robot_launch_dir = "/opt/jderobot/Launchers/global_navigation" - # Set the path to the world file + 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 = "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)}" + 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") - # Specify the actions + declare_yaw_cmd = DeclareLaunchArgument("Y", default_value="1.57079") - # Start Gazebo server - start_gazebo_server_cmd = IncludeLaunchDescription( + # 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(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}.items(), + launch_arguments={"x_pose": x_pose, "y_pose": y_pose, "z_pose": z_pose}.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) + world_entity_cmd = Node( + package="ros_gz_sim", + executable="create", + arguments=["-name", "world", "-file", world_path], + output="screen", + ) - # Add any actions - ld.add_action(start_gazebo_server_cmd) + ld = LaunchDescription() - return ld + 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 \ No newline at end of file diff --git a/Launchers/visualization/taxi_navigator.config b/Launchers/visualization/taxi_navigator.config new file mode 100644 index 000000000..2fb50e0eb --- /dev/null +++ b/Launchers/visualization/taxi_navigator.config @@ -0,0 +1,90 @@ + + + + + + + 1024 + 768 +