DRAFT: Intergrating behaviors and API for Finals

misc/four_scout_one_excavatior_one_hauler.yaml

@@ -0,0 +1,30 @@
+# ------------------------------------------------------------------------------
+# Robot team configuration
+#
+# For strict competition runs, this is the ONLY part of this file that will
+# be used
+team:
+  scouts: 4
+  excavators: 1
+  haulers: 1
+
+# ------------------------------------------------------------------------------
+# Simulation Configuration
+#
+# Competitors are free to set these for their own debugging, but these
+# values will be ignored by the competition run
+#
+seed: 42
+use_gui: true
+use_noise: false
+start_power_percentage: 100.0
+start_paused: false
+duration: 86400 # seconds
+spawn_center_x: 0
+spawn_center_y: 0
+spawn_center_z: 10
+generate_heightmap: true
+heightmap_path: ~/.gazebo/runtime_heightmap.png
+system_monitor_enabled: true
+run_endless: true
+

src/scoot/launch/final_round.launch

@@ -5,4 +5,27 @@
         <arg name="search" value="searchRandomWalk"/>
         <arg name="mode" value="auto"/>
     </include>
+    <include file="$(find scoot)/launch/scoot.launch">
+        <arg name="name" value="small_scout_2"/>
+        <arg name="search" value="searchRandomWalk"/>
+        <arg name="mode" value="auto"/>
+    </include>
+    <include file="$(find scoot)/launch/scoot.launch">
+        <arg name="name" value="small_scout_3"/>
+        <arg name="search" value="searchRandomWalk"/>
+        <arg name="mode" value="auto"/>
+    </include>
+    <include file="$(find scoot)/launch/scoot.launch">
+        <arg name="name" value="small_scout_4"/>
+        <arg name="search" value="searchRandomWalk"/>
+        <arg name="mode" value="auto"/>
+    </include>
+    <include file="$(find scoot)/launch/scoot.launch">
+        <arg name="name" value="small_excavator_1"/>
+        <arg name="mode" value="auto"/>
+    </include>
+    <include file="$(find scoot)/launch/scoot.launch">
+        <arg name="name" value="small_hauler_1"/>
+        <arg name="mode" value="auto"/>
+    </include>
 </launch>

src/scoot/src/behaviors/excavator/dig.py

@@ -19,7 +19,6 @@ def main(task=None):
         scoot = Scoot("excavator")
         scoot.start(node_name='dig')
     rospy.loginfo('Dig Started')
-    scoot.brake()
     # Check Bucket status
     if scoot.bucket_info().mass_in_bucket <= 10:
         # Reset to "home"

src/scoot/src/behaviors/excavator/dropoff.py

@@ -18,7 +18,6 @@ def main(task=None):
         scoot = Scoot("excavator")
         scoot.start(node_name='dropoff')
     rospy.loginfo('Dropoff Started')
-    scoot.brake()
     # @TODO check if have anything in bucket, verify state meh can can handle a value for said state
     # @TODO: check for message from hauler
     scoot.move_mount(math.pi)

src/scoot/src/behaviors/excavator/goto_volatile.py

@@ -3,6 +3,7 @@
 
 import sys
 import rospy
+import math
 from Scoot import Scoot, Location, VolatileException, ObstacleException
 
 def main(task=None):
@@ -15,19 +16,23 @@ def main(task=None):
         scoot = Scoot("excavator")
         scoot.start(node_name='goto_volatile')
     rospy.loginfo('Goto Volatile Started')
-    vol_pose = scoot.get_closest_vol_pose()
-    try:
-        scoot.drive_to(vol_pose)
-    except ObstacleException:
-        pass
-    except VolatileException:
-        sys.exit(0)  # "succeeded" # @TODO: might retest mass as this volatile might be almost exhausted
-    # @TODO: obstacle avoidance calls should live here
-    scoot.brake()
-    if scoot.OdomLocation.at_goal(vol_pose, 2.0):
-        sys.exit(0)  # "succeeded"
-    else:
-        sys.exit(-1)  # "failed"
+    scoot.move_shoulder_yaw(-math.pi)
+    rospy.sleep(3)
+    vol_pose = scoot.get_closest_vol_point()
+    # @TODO make a while loop or change state meh
+    if vol_pose:
+        try:
+            scoot.drive_to(vol_pose)
+        except ObstacleException:
+            pass
+        except VolatileException:
+            sys.exit(0)  # "succeeded" # @TODO: might retest mass as this volatile might be almost exhausted
+        # @TODO: obstacle avoidance calls should live here
+        if scoot.OdomLocation.at_goal(vol_pose, 2.0):
+            sys.exit(0)  # "succeeded"
+    else: # no volitile
+        rospy.sleep(5)
+    sys.exit(-1)  # "failed"
 
 
 if __name__ == '__main__':

src/scoot/src/behaviors/scout/goto_repair_station.py

@@ -20,8 +20,6 @@ def main(task=None):
     result = go_to.goto(repair_station_pose.x, repair_station_pose.y, 0, 0)
 
     scoot.drive(0, ignore=Obstacles.IS_LIDAR | Obstacles.IS_VOLATILE)
-    scoot.brake()
-
     if result:
         rospy.loginfo('goto_repair_station: succeeded')
         sys.exit(0)

src/scoot/src/behaviors/scout/searchRandomWalk.py

@@ -85,7 +85,6 @@ def main(task=None):
     ignoring = Obstacles.CUBESAT | Obstacles.HOME_FIDUCIAL | Obstacles.HOME_LEG | Obstacles.VISION_VOLATILE
 
     random_walk(num_moves=50)
-    scoot.brake()
     rospy.loginfo("I'm probably lost!")  # @ TODO add a reorient state in the task state meh
     sys.exit(1)