now use default level on all launch file, in addition use multi_robot_launcher which can launch multiple robots in a scripted manner.

Author: piyushk

.gitignore

@@ -0,0 +1,2 @@
+*.swp
+*.swo

bwi_launch/CMakeLists.txt

@@ -6,6 +6,12 @@ catkin_package()
 install(DIRECTORY launch/
         DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch)
 
+install(DIRECTORY config/
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch)
+
+catkin_install_python(PROGRAMS scripts/multi_robot_launcher
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
 # unit tests are enabled selectively
 if (CATKIN_ENABLE_TESTING)
   find_package(roslaunch REQUIRED)

bwi_launch/config/simulation.yaml

@@ -0,0 +1,5 @@
+robots:
+  - name: marvin
+    location: [0.0, 8.0, 0.0]
+  - name: roberto 
+    location: [0.0, 10.0, 0.0]

bwi_launch/launch/multi_robot_simulation.launch

@@ -1,8 +1,6 @@
 <launch>
 
   <arg name="multimap_file" default="$(find utexas_gdc)/maps/simulation/multimap/test.yaml" />
-  <arg name="robot1" default="marvin" />
-  <arg name="robot2" default="roberto" />
 
   <!-- start the simulation environment -->
   <include file="$(find utexas_gdc)/launch/simulation_3ne.launch" />
@@ -14,16 +12,8 @@
 
   <include file="$(find segbot_simulation_apps)/launch/door_handler.launch" />
 
-  <!-- launch robot1 -->
-  <include file="$(find bwi_launch)/launch/includes/simulation_ns.launch.xml">
-    <arg name="y" value="8.0" />
-    <arg name="ns" value="$(arg robot1)" />
-  </include>
-
-  <!-- launch robot2 -->
-  <include file="$(find bwi_launch)/launch/includes/simulation_ns.launch.xml">
-    <arg name="y" value="10.0" />
-    <arg name="ns" value="$(arg robot2)" />
-  </include>
+  <node name="multi_robot_launcher" pkg="bwi_launch" type="multi_robot_launcher">
+    <rosparam command="load" param="configuration" file="$(find bwi_launch)/config/simulation.yaml" />
+  </node>
 
 </launch>

bwi_launch/launch/segbot_v1.launch

@@ -6,6 +6,9 @@
     <param name="map_file" value="$(arg multimap_file)" />
   </node>
 
+  <!-- Set the default level we'd like this robot to start on -->
+  <param name="level_mux/default_current_level" value="3rdFloor" />
+
   <include file="$(find bwi_launch)/launch/includes/segbot.launch.xml" />
 
   <include file="$(find segbot_bringup)/launch/segbot_v1.launch" />

bwi_launch/launch/segbot_v2.launch

@@ -6,6 +6,9 @@
     <param name="map_file" value="$(arg multimap_file)" />
   </node>
 
+  <!-- Set the default level we'd like this robot to start on -->
+  <param name="level_mux/default_current_level" value="3rdFloor" />
+
   <include file="$(find bwi_launch)/launch/includes/segbot.launch.xml">
     <arg name="laser_max_range" value="20.0" />
   </include>

bwi_launch/launch/simulation.launch

@@ -10,6 +10,9 @@
     <param name="map_file" value="$(arg multimap_file)" />
   </node>
 
+  <!-- Set the default level we'd like this robot to start on -->
+  <param name="level_mux/default_current_level" value="level1" />
+
   <include file="$(find segbot_simulation_apps)/launch/door_handler.launch" />
 
   <include file="$(find bwi_launch)/launch/includes/simulation.launch.xml">

bwi_launch/package.xml

@@ -18,6 +18,7 @@
 
   <buildtool_depend>catkin</buildtool_depend>
 
+  <depend>bwi_msgs</depend>
   <depend>bwi_kr_execution</depend>
   <depend>multi_level_map_server</depend>
   <depend>multi_level_map_utils</depend>

bwi_launch/scripts/multi_robot_launcher

@@ -0,0 +1,59 @@
+#!/usr/bin/env python
+
+import os
+import rospy
+import tempfile
+
+from bwi_msgs.msg import AvailableRobot, AvailableRobotArray
+from bwi_tools import start_roslaunch_process, stop_roslaunch_process
+
+def construct_launch_file(fd, config, msg):
+    msg.robots = []
+    launch_file_str = "<launch>"
+    for robot in config['robots']:
+        robot_name = robot['name']
+        robot_x = robot['location'][0]
+        robot_y = robot['location'][1]
+        robot_yaw = robot['location'][2]
+        launch_file_str += '<include file="$(find bwi_launch)/launch/includes/simulation_ns.launch.xml"><arg name="x" value="%f" /><arg name="y" value="%f" /><arg name="yaw" value="%f" /><arg name="ns" value="%s" /></include> '%(robot_x, robot_y, robot_yaw, robot_name)
+        msg.robots.append(AvailableRobot(robot_name, AvailableRobot.SEGBOT))
+    launch_file_str += '</launch>'
+    fd.write(launch_file_str)
+
+def robot_publisher():
+
+    process = None
+    temp_file_name = None
+    try:
+        pub = rospy.Publisher('/available_robots', AvailableRobotArray, latch=True)
+
+        # Create a temporary file.
+        temp_launch_file = tempfile.NamedTemporaryFile(suffix=".launch", delete=False)
+        temp_file_name = temp_launch_file.name
+
+        # Construct the launch file.
+        msg = AvailableRobotArray()
+        configuration = rospy.get_param('~configuration')
+        construct_launch_file(temp_launch_file, configuration, msg)
+
+        temp_launch_file.close()
+        start_roslaunch_process(None, temp_file_name)
+
+        pub.publish(msg)
+        rospy.spin()
+
+    except rospy.ROSInterruptException:
+        pass
+
+    if process is not None:
+        stop_roslaunch_process(process)
+
+    if temp_file_name is not None:
+        try:
+            os.remove(temp_file_name)
+        except OSError:
+            pass
+
+if __name__ == '__main__':
+        rospy.init_node('robot_publisher', anonymous=True)
+        robot_publisher()