diff --git a/src/nav2_turtlebot_simulator/launch/simulator.launch.py b/src/nav2_turtlebot_simulator/launch/simulator.launch.py index 2fcb20d..480ea60 100644 --- a/src/nav2_turtlebot_simulator/launch/simulator.launch.py +++ b/src/nav2_turtlebot_simulator/launch/simulator.launch.py @@ -33,6 +33,8 @@ LOCAL_SUN_LIGHT = ''' false0 0 10 0 0 00.8 0.8 0.8 10.2 0.2 0.2 1-0.5 0.1 -0.9 ''' +MKMINI_WORLD_SPACING_SCALE = 1.6 + def replace_house_visual_meshes_with_boxes(house_model): """Keep physical mesh collisions, but render furniture as inexpensive boxes.""" @@ -174,6 +176,53 @@ def create_flat_house_world(gazebo_dir): return flat_world_path +def create_mkmini_world(gazebo_dir): + """Expand the TurtleBot3 obstacle layout without changing obstacle sizes.""" + source_model_path = os.path.join(gazebo_dir, 'models', 'turtlebot3_world', 'model.sdf') + models_dir = os.path.join(tempfile.gettempdir(), 'nav2_mkmini_models') + model_dir = os.path.join(models_dir, 'turtlebot3_world_mkmini') + os.makedirs(model_dir, exist_ok=True) + + model_tree = ET.parse(source_model_path) + model_sdf = model_tree.getroot() + model = model_sdf.find('model') + if model is None: + raise RuntimeError('turtlebot3_world model is missing from model.sdf') + model.attrib['name'] = 'turtlebot3_world_mkmini' + + # Only object positions change. Keeping their geometry unchanged widens the + # driving lanes for the larger MKMini collision footprint. + for pose in model.findall('.//pose'): + values = pose.text.split() + if len(values) != 6: + continue + try: + values[0] = str(float(values[0]) * MKMINI_WORLD_SPACING_SCALE) + values[1] = str(float(values[1]) * MKMINI_WORLD_SPACING_SCALE) + except ValueError: + continue + pose.text = ' '.join(values) + + model_tree.write( + os.path.join(model_dir, 'model.sdf'), + encoding='utf-8', + xml_declaration=True, + ) + with open(os.path.join(model_dir, 'model.config'), 'w', encoding='utf-8') as config: + config.write( + 'turtlebot3_world_mkmini' + '1.0model.sdf' + ) + + source_world_path = os.path.join(gazebo_dir, 'worlds', 'turtlebot3_world.world') + with open(source_world_path, 'r', encoding='utf-8') as source: + world = source.read().replace('model://turtlebot3_world', 'model://turtlebot3_world_mkmini') + world_path = os.path.join(tempfile.gettempdir(), 'nav2_mkmini_world.world') + with open(world_path, 'w', encoding='utf-8') as destination: + destination.write(world) + return world_path, models_dir + + def create_custom_layout_world(): """Build a Gazebo world from the editor's persisted 2D layout.""" layout_path = os.environ.get('WORLD_LAYOUT_PATH', '/modules/world-layout.json') @@ -234,6 +283,7 @@ def launch_gazebo(context): world = LaunchConfiguration('world').perform(context) render_mode = LaunchConfiguration('render_mode').perform(context).strip().lower() platform = LaunchConfiguration('platform').perform(context).strip().lower() + extra_model_path = '' if render_mode not in {'flat', 'full'}: raise RuntimeError('render_mode must be either flat or full') @@ -242,6 +292,8 @@ def launch_gazebo(context): world_path = create_custom_layout_world() elif world == 'turtlebot3_house.world' and render_mode == 'flat': world_path = create_flat_house_world(gazebo_dir) + elif world == 'turtlebot3_world.world' and platform in {'mkmini', 'mk-mini'}: + world_path, extra_model_path = create_mkmini_world(gazebo_dir) else: world_path = os.path.join( simulator_dir if world == 'empty_world.world' else gazebo_dir, @@ -287,7 +339,7 @@ def launch_gazebo(context): return [ SetEnvironmentVariable( name='GAZEBO_MODEL_PATH', - value=os.pathsep.join(part for part in (model_directory, existing_model_path) if part), + value=os.pathsep.join(part for part in (extra_model_path, model_directory, existing_model_path) if part), ), IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(gazebo_ros_dir, 'launch', 'gzserver.launch.py')), diff --git a/src/nav2_turtlebot_simulator/models/mkmini_gazebo/model.sdf b/src/nav2_turtlebot_simulator/models/mkmini_gazebo/model.sdf index 5d59150..20fede0 100644 --- a/src/nav2_turtlebot_simulator/models/mkmini_gazebo/model.sdf +++ b/src/nav2_turtlebot_simulator/models/mkmini_gazebo/model.sdf @@ -27,12 +27,12 @@ 0 0 0.52 0 0 00.20.0010.0010.001 0.0750.100.85 0.12 0.10 10.85 0.12 0.10 1 - true10false3601-3.141593.14159161-0.260.260.2018.00.02~/out:=/carla/hero/lidar_filteredsensor_msgs/PointCloud2os_lidar + true10false3601-3.141593.14159161-0.260.260.2018.00.02~/out:=/raw/lidarsensor_msgs/PointCloud2os_lidar base_linkos_lidar - cmd_vel:=/cmd_velodom:=/odom_custom + cmd_vel:=/cmd_velodom:=/odom front_steering_jointfront_drive_joint rear_left_jointrear_right_joint 0.720.280.28