-
Notifications
You must be signed in to change notification settings - Fork 75
How to add a 2nd simulate dock? #245
Description
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 ServerDescribe 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