Skip to content

How to add a 2nd simulate dock? #245

@Mechazo11

Description

@Mechazo11

Is your feature request related to a problem? Please describe.
This feature request is thematically related to turtlebot/turtlebot4_simulator#10

For the past few days I have been trying to spawn a second dock station to enable the turtlebot4 plan a mission where it traverses a large space. The idea is to have it autonomously find the closest dock and go to to recharge depending on model.

However, it appears the dock that is generated with the turtlebot4 using the turtlebot4_spawn.launch.py may have extensive hard coded sections. With Claude's help (who lead me to multiple "wild goose chases"), I think this is the pipeline of how the simulated dock detection is performed

URDF definitions
├── create3.urdf.xacro
│   └── ir_omni link @ (0.153, 0, ~0.099m) — front center of robot
└── standard_dock.urdf.xacro
    └── halo_link @ (-0.06, 0, ~0.095m)   — front face of dock
            │
            ▼ PosePublisher plugin (62 Hz)
            │  publishes full TF tree of each model
              Both models continuously broadcast their full TF trees into ROS via:
              _internal/sim_ground_truth_pose (robot tree)
              _internal/sim_ground_truth_dock_pose (dock tree)
            ▼
    pose_republisher_node
    ├── filters "ir_omni"   → world frame → ir_receiver_pose
    └── filters "halo_link" → world frame → ir_emitter_pose
            │
            ▼
    Geometric Detection IrOpcode (62 Hz)
    ├── CheckForceFieldDetection
    │   └── halo: range 0.71m (omni) / 1.11m (dir), FOV cone
    │       → CODE_IR_FORCE_FIELD
    ├── CheckBuoysDetection  
    │   └── buoys: range 1.1m/1.5m, mutual visibility
    │       → CODE_IR_BUOY_RED / GREEN
    └── PublishSensors → /ir_opcode
            │
    DockStatus (20 Hz)
    └── distance < 0.075m + yaw < 6° → /dock_status
            │
            ▼
    Nav2 Docking Action Server

Describe the solution you'd like
@roni-kreinin would you kindly guide me on what would be the right procedure and the files which should be modified to enable multi-dock simulation? Would you summarize the idea behind how you used TF-like geometry to simulate detection of the dock (at least thats what I think was happening)

Describe alternatives you've considered
Alternative is to do nothing and write a behavior tree node that upon going to a certain location, we send out message to is_docked topic to state its docked. I suspect the Undock and Dock action servers will not work appropriately in this hack method

Additional context
The following launch file helps me to spin up a second dock on the gazebo world

# Includes
from launch.substitutions import Command
from ament_index_python.packages import get_package_share_directory
from irobot_create_common_bringup.namespace import GetNamespacedName
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription, OpaqueFunction
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node, PushRosNamespace

ARGUMENTS = [
    DeclareLaunchArgument('use_sim_time', default_value='true',
                          choices=['true', 'false'],
                          description='use_sim_time'),
    DeclareLaunchArgument('namespace', default_value='',
                          description='Robot namespace'),
    DeclareLaunchArgument('dock_name', default_value='dock_2',
                          description='Name of the additional dock'),
    DeclareLaunchArgument('dock_pose', default_value='[1.0,1.0,0.0]',
                          description='2D pose as [x,y,yaw] as m,m,radians.'),
]

def spawn_dock(context, *args, **kwargs):
    import ast
    
    # Directories
    pkg_irobot_create_common_bringup = get_package_share_directory(
        'irobot_create_common_bringup')
    
    # Paths
    # dock_description_launch = PathJoinSubstitution([pkg_irobot_create_common_bringup, 'launch', 'dock_description.launch.py'])
    pkg_create3_description = get_package_share_directory('irobot_create_description')
    
    # Path to dock URDF
    dock_xacro_file = PathJoinSubstitution(
        [pkg_create3_description, 'urdf', 'dock', 'standard_dock.urdf.xacro'])
    
    # Get launch configurations
    namespace = LaunchConfiguration('namespace').perform(context)
    dock_name_arg = LaunchConfiguration('dock_name').perform(context)
    pose_str = LaunchConfiguration('dock_pose').perform(context)
    
    # Parse and validate dock_pose
    try:
        pose_list = ast.literal_eval(pose_str)
        if not isinstance(pose_list, list) or len(pose_list) != 3:
            raise ValueError("dock_pose must be [x, y, yaw] with 3 values")
        if not pose_list:
            raise ValueError("dock_pose cannot be empty")
        x, y, yaw = pose_list
    except Exception as e:
        raise ValueError(f"Invalid dock_pose format '{pose_str}': {e}")
    
    # Generate namespaced dock name
    dock_name = GetNamespacedName(namespace, dock_name_arg)
    
    return [GroupAction([
        PushRosNamespace(namespace),
        
        # Dock state publisher with unique topic
        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            name=f'{dock_name_arg}_state_publisher',
            output='screen',
            parameters=[
                {'use_sim_time': True},
                {'robot_description': Command(['xacro', ' ', dock_xacro_file, ' ',
                    'gazebo:=ignition', ' ',
                    'namespace:=', namespace, ' ',
                    'dock_name:=', dock_name_arg, ' ',
                    'visualize_rays:=true'])}
            ],
            remappings=[
                ('robot_description', f'{dock_name_arg}_description'),
                ('/tf', 'tf'),
                ('/tf_static', 'tf_static')
            ],
        ),
        
        # Spawn Dock in Gazebo
        Node(
            package='ros_gz_sim',
            executable='create',
            arguments=['-name', dock_name,
                       '-x', str(x),
                       '-y', str(y),
                       '-z', '0.0',
                       '-Y', str(yaw),
                       '-topic', f'{dock_name_arg}_description'],
            output='screen',
        ),

        # Static TF publisher - dock position in odom frame
        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            name=f'{dock_name_arg}_tf_publisher',
            arguments=[str(x), str(y), '0',
                    str(yaw), '0', '0',
                    'odom', f'{dock_name_arg}_link'],
            remappings=[
                ('/tf', 'tf'),
                ('/tf_static', 'tf_static')
            ],
            output='screen',
        ),
    ])]

def generate_launch_description():
    ld = LaunchDescription(ARGUMENTS)
    ld.add_action(OpaqueFunction(function=spawn_dock))
    return ld

Metadata

Metadata

Assignees

No one assigned

    Labels

    enhancementNew feature or request

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions