mirror of
https://github.com/6-robot/wpr_simulation.git
synced 2025-09-15 12:59:01 +08:00
95 lines
6.3 KiB
XML
95 lines
6.3 KiB
XML
<launch>
|
|
|
|
<!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
|
|
<include file="$(find gazebo_ros)/launch/empty_world.launch">
|
|
<arg name="world_name" value="$(find wpr_simulation)/worlds/robocup_home.world"/>
|
|
<arg name="paused" value="false"/>
|
|
<arg name="use_sim_time" value="true"/>
|
|
<arg name="gui" value="true"/>
|
|
<arg name="recording" value="false"/>
|
|
<arg name="debug" value="false"/>
|
|
</include>
|
|
|
|
<!-- Spawn the objects into Gazebo -->
|
|
<node name="bed" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/bed.model -x 5.0 -y -3.9 -z 0 -Y 3.14159 -urdf -model bed" />
|
|
<node name="sofa" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/sofa.model -x -1.0 -y -3.9 -z 0 -Y 1.57 -urdf -model sofa" />
|
|
<node name="tea_table" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/tea_table.model -x -2.1 -y -2.2 -z 0 -Y 1.57 -urdf -model tea_table" />
|
|
<node name="bookshelft" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/bookshelft.model -x 2.0 -y -0.55 -z 0 -Y -1.57 -urdf -model bookshelft" />
|
|
|
|
<node name="kitchen_table" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/table.model -x -3.5 -y 3.7 -z 0 -Y 1.57 -urdf -model kitchen_table" />
|
|
<node name="bottle_0" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/bottles/red_bottle.model -x -3.3 -y 3.55 -z 10.0 -Y 0 -urdf -model bottle_0" />
|
|
<node name="bottle_1" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/bottles/green_bottle.model -x -3.6 -y 3.55 -z 10.0 -Y 0 -urdf -model bottle_1" />
|
|
|
|
<node name="cupboard_0" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/cupboard.model -x -2.0 -y 0.7 -z 0 -Y 1.57 -urdf -model cupboard_0" />
|
|
<node name="cupboard_1" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/cupboard.model -x -1.3 -y 3.7 -z 0 -Y -1.57 -urdf -model cupboard_1" />
|
|
|
|
<node name="dinning_table_0" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/table.model -x 1.5 -y 1.5 -z 0 -Y 1.57 -urdf -model dinning_table_0" />
|
|
<node name="dinning_table_1" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/table.model -x 1.5 -y 2.0 -z 0 -Y 1.57 -urdf -model dinning_table_1" />
|
|
<node name="dinning_table_2" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/table.model -x 2.7 -y 1.5 -z 0 -Y 1.57 -urdf -model dinning_table_2" />
|
|
<node name="dinning_table_3" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/table.model -x 2.7 -y 2.0 -z 0 -Y 1.57 -urdf -model dinning_table_3" />
|
|
|
|
<node name="chair_0" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/chair.model -x 1.5 -y 1.2 -z 0 -Y 1.57 -urdf -model chair_0" />
|
|
<node name="chair_1" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/chair.model -x 1.5 -y 2.3 -z 0 -Y -1.57 -urdf -model chair_1" />
|
|
<node name="chair_2" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/chair.model -x 2.7 -y 1.2 -z 0 -Y 1.57 -urdf -model chair_2" />
|
|
<node name="chair_3" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/chair.model -x 2.7 -y 2.3 -z 0 -Y -1.57 -urdf -model chair_3" />
|
|
|
|
|
|
<!-- Spawn a robot into Gazebo -->
|
|
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/wpb_home_mani.model -urdf -x -6.0 -y -0.5 -model wpb_home_mani" />
|
|
|
|
<!-- load the controllers of WPB_HOME -->
|
|
<include file="$(find wpr_simulation)/launch/wpb_home_controllers.launch"/>
|
|
|
|
<!-- lidar filter-->
|
|
<node pkg="wpr_simulation" type="lidar_filter" name="lidar_filter" />
|
|
|
|
<!-- wpb_home_behaviors -->
|
|
<node pkg="wpb_home_behaviors" type="wpb_home_grab_server" name="wpb_home_grab_server" />
|
|
|
|
<!-- wpb_home_objects_3d -->
|
|
<node pkg="wpb_home_behaviors" type="wpb_home_objects_3d" name="wpb_home_objects_3d" output="screen">
|
|
<param name="topic" type="string" value="/kinect2/sd/points"/>
|
|
</node>
|
|
|
|
<!-- wpb_home_grab_action -->
|
|
<node pkg="wpb_home_behaviors" type="wpb_home_grab_action" name="wpb_home_grab_action">
|
|
<rosparam file="$(find wpb_home_bringup)/config/wpb_home.yaml" command="load" />
|
|
</node>
|
|
|
|
<!-- Run the map server -->
|
|
<node name="map_server" pkg="map_server" type="map_server" args="$(find wpr_simulation)/maps/map.yaml"/>
|
|
|
|
<!--- Run AMCL -->
|
|
<include file="$(find wpb_home_tutorials)/nav_lidar/amcl_omni.launch" />
|
|
|
|
<!--- Run move base -->
|
|
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
|
|
<rosparam file="$(find wpb_home_tutorials)/nav_lidar/costmap_common_params.yaml" command="load" ns="global_costmap" />
|
|
<rosparam file="$(find wpb_home_tutorials)/nav_lidar/costmap_common_params.yaml" command="load" ns="local_costmap" />
|
|
<rosparam file="$(find wpb_home_tutorials)/nav_lidar/global_costmap_params.yaml" command="load" />
|
|
<rosparam file="$(find wpb_home_tutorials)/nav_lidar/local_costmap_params.yaml" command="load" />
|
|
<rosparam file="$(find wpb_home_tutorials)/nav_lidar/local_planner_params.yaml" command="load" />
|
|
<param name="base_global_planner" value="global_planner/GlobalPlanner" />
|
|
<param name="use_dijkstra" value="true"/>
|
|
<param name="base_local_planner" value="wpbh_local_planner/WpbhLocalPlanner" />
|
|
<param name= "controller_frequency" value="10" type="double"/>
|
|
</node>
|
|
|
|
<!-- Map tools -->
|
|
<node pkg="waterplus_map_tools" type="wp_manager" name="wp_manager" output="screen" />
|
|
|
|
<!-- Navi server -->
|
|
<node pkg="waterplus_map_tools" type="wp_navi_server" name="wp_navi_server" output="screen" />
|
|
|
|
<!-- RViz and TF tree -->
|
|
<arg name="model" default="$(find wpb_home_bringup)/urdf/wpb_home_mani.urdf"/>
|
|
<arg name="gui" default="false" />
|
|
<arg name="rvizconfig" default="$(find wpr_simulation)/rviz/map_tool.rviz" />
|
|
|
|
<param name="robot_description" command="$(find xacro)/xacro $(arg model)" />
|
|
<param name="use_gui" value="$(arg gui)"/>
|
|
|
|
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
|
|
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
|
|
|
</launch> |