diff --git a/misc/four_scout_one_excavatior_one_hauler.yaml b/misc/four_scout_one_excavatior_one_hauler.yaml
new file mode 100644
index 0000000..1041d2f
--- /dev/null
+++ b/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
+
diff --git a/src/scoot/launch/final_round.launch b/src/scoot/launch/final_round.launch
index 3f13e8c..3dfd558 100644
--- a/src/scoot/launch/final_round.launch
+++ b/src/scoot/launch/final_round.launch
@@ -5,4 +5,27 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/scoot/src/Scoot.py b/src/scoot/src/Scoot.py
index 0fb7d06..247396e 100755
--- a/src/scoot/src/Scoot.py
+++ b/src/scoot/src/Scoot.py
@@ -221,10 +221,10 @@ def start(self, **kwargs):
self.sensor_yaw_control_topic = rospy.Publisher('/' + self.rover_name + '/sensor/yaw/command/position', Float64,
queue_size=10)
# Connect to services.
- rospy.loginfo("Waiting for control service")
+ rospy.loginfo(self.rover_name + ": Waiting for control service")
rospy.wait_for_service('/' + self.rover_name + '/control')
self.control = rospy.ServiceProxy('/' + self.rover_name + '/control', Core)
- rospy.loginfo("Done waiting for control service")
+ rospy.loginfo(self.rover_name + ": Done waiting for control service")
rospy.wait_for_service('/' + self.rover_name + '/spot_light')
self.light_service = rospy.ServiceProxy('/' + self.rover_name + '/spot_light', srv.SpotLightSrv)
@@ -235,7 +235,7 @@ def start(self, **kwargs):
self.power_saver_service = rospy.ServiceProxy('/' + self.rover_name + '/system_monitor/power_saver',
srv.SystemPowerSaveSrv)
- rospy.loginfo("Done waiting for general services")
+ rospy.loginfo(self.rover_name + ": Done waiting for general services")
if self.rover_type == "scout":
pass # rospy.wait_for_service('/vol_detected_service')
elif self.rover_type == "excavator":
@@ -257,7 +257,7 @@ def start(self, **kwargs):
queue_size=10)
# rospy.Subscriber('/' + self.rover_name + '/bin_info', msg.HaulerMsg, self._bin_info)
- rospy.loginfo("Done waiting for rover specific services")
+ rospy.loginfo(self.rover_name + ": Done waiting for rover specific services")
# Subscribe to topics.
rospy.Subscriber('/' + self.rover_name + '/odometry/filtered', Odometry, self._odom)
rospy.Subscriber('/' + self.rover_name + '/system_monitor', msg.SystemMonitorMsg, self._health)
@@ -266,7 +266,7 @@ def start(self, **kwargs):
# Transform listener. Use this to transform between coordinate spaces.
# Transform messages must predate any sensor messages so initialize this first.
self.xform = tf.TransformListener()
- rospy.loginfo("Scoot Ready")
+ rospy.loginfo(self.rover_name + ": Scoot Ready")
@Sync(odom_lock)
def _odom(self, message):
@@ -294,8 +294,8 @@ def get_joint_states(self):
def get_joint_pos(self, joint_name):
if joint_name in self.joint_states.name:
return self.joint_states.position[self.joint_states.name.index(joint_name)]
- rospy.logerr("get_joint_state: unknown joint:" + str(joint_name))
- rospy.loginfo("get_joint_state: valid joints" + str(self.joint_states.name))
+ rospy.logerr(self.rover_name + ": get_joint_state: unknown joint:" + str(joint_name))
+ rospy.loginfo(self.rover_name + ": get_joint_state: valid joints" + str(self.joint_states.name))
@Sync(odom_lock)
def get_odom_location(self):
@@ -304,7 +304,7 @@ def get_odom_location(self):
@Sync(odom_lock)
def get_true_pose(self):
if self.truePoseCalled:
- rospy.logwarn("True pose already called once.")
+ rospy.logwarn(self.rover_name + ": True pose already called once.")
# @TODO if the rover has moved 2m+ might be more useful to apply the offset to odom and return that
# as this assumes the rover has not moved since the prior call
return self.true_pose_got
@@ -312,7 +312,7 @@ def get_true_pose(self):
try:
self.truePoseCalled = True
self.true_pose_got = self.localization_service(call=True).pose
- rospy.logwarn("true_pose_got:")
+ rospy.logwarn(self.rover_name + ": true_pose_got:")
rospy.logwarn(self.true_pose_got.position)
odom_p = self.OdomLocation.Odometry.pose.pose.position
odom_o = self.OdomLocation.Odometry.pose.pose.orientation
@@ -325,30 +325,30 @@ def get_true_pose(self):
return self.true_pose_got # @TODO might save this as a rosparam so if scoot crashes
except (rospy.ServiceException, AttributeError) as exc:
- rospy.logwarn("Service did not process request: " + str(exc))
+ rospy.logwarn(self.rover_name + ": Service did not process request: " + str(exc))
@Sync(health_lock)
def get_power_level(self):
if self.health:
return self.health.power_level
- rospy.logerr("No rover system_monitor messages received " +
+ rospy.logerr(self.rover_name + ": No rover system_monitor messages received " +
"please check system_monitor_enabled is set to true in the yaml")
@Sync(health_lock)
def is_solar_charging(self):
if self.health:
return self.health.solar_ok
- rospy.logerr("No rover system_monitor messages received " +
+ rospy.logerr(self.rover_name + ": No rover system_monitor messages received " +
"please check system_monitor_enabled is set to true in the yaml")
def _power_saver(self, enabled):
self.power_saver_state = enabled
try:
result = self.power_saver_service(power_save=enabled)
- rospy.loginfo(result.message)
+ rospy.loginfo(self.rover_name + result.message)
return result.success
except (rospy.ServiceException, AttributeError) as exc:
- rospy.logwarn("power_saver_enable service did not process request: " + str(exc))
+ rospy.logwarn(self.rover_name + ": power_saver_enable service did not process request: " + str(exc))
def power_saver_on(self):
self._power_saver(True)
@@ -441,7 +441,10 @@ def drive_to(self, place, forward_offset=0, **kwargs):
math.atan2(place.y - loc.y,
place.x - loc.x))
effective_dist = dist - forward_offset
-
+
+ if abs(angle) > .05:
+ self.turn(angle, **kwargs)
+
if effective_dist < 0:
# The driver API skips the turn state if the request distance is
# negative. This ensures the rover will perform the turn before
@@ -451,7 +454,7 @@ def drive_to(self, place, forward_offset=0, **kwargs):
req = MoveRequest(
theta=angle,
- r=effective_dist,
+ x=effective_dist,
)
return self.__drive(req, **kwargs)
@@ -589,12 +592,12 @@ def _light(self, state):
try:
self.light_service.call(range=state)
except rospy.ServiceException:
- rospy.logerr("Light Service Exception: Light Service Failed to Respond")
+ rospy.logerr(self.rover_name + ": Light Service Exception: Light Service Failed to Respond")
try:
self.light_service.call(range=state)
- rospy.logwarn("Second attempt to use lights was successful")
+ rospy.logwarn(self.rover_name + ": Second attempt to use lights was successful")
except rospy.ServiceException:
- rospy.logerr("Light Service Exception: Second attempt failed to use lights")
+ rospy.logerr(self.rover_name + ": Light Service Exception: Second attempt failed to use lights")
def light_on(self):
self._light(20)
@@ -612,7 +615,7 @@ def light_intensity(self, intensity):
# # # EXCAVATOR SPECIFIC CODE # # #
def bucket_info(self):
if self.rover_type != "excavator":
- rospy.logerr("bucket_info:" + self.rover_type + " is not an excavator")
+ rospy.logerr(self.rover_name + ": bucket_info:" + self.rover_type + " is not an excavator")
return self.bucket_info_msg # last message from the bucket_info topic bucket_info srcp2_msgs/ExcavatorMsg
@Sync(joint_lock)
@@ -622,13 +625,15 @@ def move_shoulder_yaw(self, angle):
@NOTE: Effort Limits are ignored
"""
if self.rover_type != "excavator":
- rospy.logerr("move_shoulder_yaw:" + self.rover_type + " is not an excavator")
+ rospy.logerr(self.rover_name + ": move_shoulder_yaw:" + self.rover_type + " is not an excavator")
return
if angle > math.pi:
- rospy.logerr("move_shoulder_yaw:" + str(angle) + " exceeds allowed limits moving to max position")
+ rospy.logerr(self.rover_name + ": move_shoulder_yaw:" + str(angle) + "exceeds allowed limits moving to "
+ "max position")
angle = math.pi # max
elif angle < -math.pi:
- rospy.logerr("move_shoulder_yaw:" + str(angle) + " exceeds allowed limits moving to minimum position")
+ rospy.logerr(self.rover_name + ": move_shoulder_yaw:" + str(angle) + "exceeds allowed limits moving to "
+ "minimum position")
angle = -math.pi # min
self.shoulder_yaw_control.publish(angle) # publishes angle on the shoulder_yaw_joint_controller/command topic
@@ -640,14 +645,16 @@ def move_shoulder_pitch(self, angle):
@NOTE: Effort Limits are ignored
"""
if self.rover_type != "excavator":
- rospy.logerr("move_shoulder_pitch:" + self.rover_type + " is not an excavator")
+ rospy.logerr(self.rover_name + ": move_shoulder_pitch:" + self.rover_type + " is not an excavator")
return
# checking bounds
if angle > (3 * math.pi / 8.0):
- rospy.logerr("move_shoulder_pitch:" + str(angle) + " exceeds allowed limits moving to max position")
+ rospy.logerr(self.rover_name + ": move_shoulder_pitch:" + str(angle) + "exceeds allowed limits moving to "
+ "max position")
angle = 3 * math.pi / 8.0 # max
elif angle < (-3 * math.pi / 8.0):
- rospy.logerr("move_shoulder_pitch:" + str(angle) + " exceeds allowed limits moving to minimum position")
+ rospy.logerr(self.rover_name + ": move_shoulder_pitch:" + str(angle) + "exceeds allowed limits moving to "
+ "minimum position")
angle = -3 * math.pi / 8.0 # min
self.shoulder_pitch_control.publish(angle)
@@ -658,14 +665,16 @@ def move_elbow_pitch(self, angle):
@NOTE: Effort Limits are ignored
"""
if self.rover_type != "excavator":
- rospy.logerr("move_elbow_pitch:" + self.rover_type + " is not an excavator")
+ rospy.logerr(self.rover_name + ": move_elbow_pitch:" + self.rover_type + " is not an excavator")
return
# checking bounds
if angle > (7 * math.pi / 8.0): # @NOTE: or its elbow_pitch_joint: 7pi/8 != pi/4 as the wiki says
- rospy.logerr("move_shoulder_pitch:" + str(angle) + " exceeds allowed limits moving to max position")
+ rospy.logerr(self.rover_name + ": move_shoulder_pitch:" + str(angle) + "exceeds allowed limits moving to "
+ "max position")
angle = 7 * math.pi / 8.0 # max
elif angle < (-math.pi / 4.0):
- rospy.logerr("move_shoulder_pitch:" + str(angle) + " exceeds allowed limits moving to minimum position")
+ rospy.logerr(self.rover_name + ": move_shoulder_pitch:" + str(angle) + "exceeds allowed limits moving to "
+ "minimum position")
angle = -math.pi / 4.0 # min
self.elbow_pitch_control.publish(angle)
@@ -674,18 +683,20 @@ def move_bucket(self, angle):
# checking bounds
if self.rover_type == "excavator":
if angle > (2 * math.pi / 3.0):
- rospy.logerr("move_bucket:" + str(angle) + " exceeds allowed limits moving to max position")
+ rospy.logerr(self.rover_name + ": move_bucket:" + str(angle) + "exceeds allowed limits moving to max "
+ "position")
angle = 2 * math.pi / 3.0 # max
elif angle < (-2 * math.pi / 3.0):
- rospy.logerr("move_bucket:" + str(angle) + " exceeds allowed limits moving to minimum position")
+ rospy.logerr(self.rover_name + ": move_bucket:" + str(angle) + "exceeds allowed limits moving to "
+ "minimum position")
angle = -2 * math.pi / 3.0 # min
self.bucket_control.publish(angle)
return
- rospy.logerr("move_bucket:" + self.rover_type + " is not an excavator")
+ rospy.logerr(self.rover_name + ": move_bucket:" + self.rover_type + " is not an excavator")
def get_shoulder_yaw_angle(self):
if self.rover_type != "excavator":
- rospy.logerr("get_shoulder_yaw_angle:" + self.rover_type + " is not an excavator")
+ rospy.logerr(self.rover_name + ": get_shoulder_yaw_angle:" + self.rover_type + " is not an excavator")
return
return self.get_joint_pos("shoulder_yaw_joint")
@@ -712,14 +723,14 @@ def get_bucket_angle(self):
# # # HAULER SPECIFIC CODE # # #
def bin_info(self):
if self.rover_type != "hauler":
- rospy.logerr("bin_info:" + self.rover_type + " is not a hauler")
+ rospy.logerr(self.rover_name + ": bin_info:" + self.rover_type + " is not a hauler")
return
return self.score.hauler_volatile_score
@Sync(joint_lock)
def move_bin(self, angle):
if self.rover_type != "hauler":
- rospy.logerr("move_bin:" + self.rover_type + " is not a hauler")
+ rospy.logerr(self.rover_name + ": move_bin:" + self.rover_type + " is not a hauler")
return
if angle < 0:
angle = 0
@@ -735,7 +746,7 @@ def bin_dump(self):
def get_bin_angle(self):
if self.rover_type != "hauler":
- rospy.logerr("get_bin_angle:" + self.rover_type + " is not a hauler")
+ rospy.logerr(self.rover_name + ": get_bin_angle:" + self.rover_type + " is not a hauler")
return
return self.get_joint_pos("bin_joint")
@@ -760,7 +771,7 @@ def get_next_best_vol_pose(self):
hydrogen_sulfite, sulfur_dioxide, ice
"""
- def get_closest_vol_pose(self):
+ def get_closest_vol_point(self):
try:
while rospy.get_param("/volatile_locations_latch", default=False):
rospy.sleep(0.2) # wait for it be be unlatched
@@ -774,15 +785,16 @@ def get_closest_vol_pose(self):
closest_vol_pose = min(volatile_locations,
key=lambda k: math.sqrt((k['x'] - rover_pose.x) ** 2 + (k['y'] - rover_pose.y) ** 2))
- rospy.loginfo("rover pose: x:" + str(rover_pose.x) + ", y:" + str(rover_pose.y))
- rospy.loginfo("get_closest_vol_pose: x:" + str(closest_vol_pose['x']) + ", y:" + str(closest_vol_pose['y']))
- return closest_vol_pose
+ rospy.loginfo(self.rover_name + ": rover pose: x:" + str(rover_pose.x) + ", y:" + str(rover_pose.y))
+ rospy.loginfo(self.rover_name + ": get_closest_vol_pose: x:" + str(closest_vol_pose['x']) + ", y:" +
+ str(closest_vol_pose['y']))
+ return Point(closest_vol_pose['x'], closest_vol_pose['y'], 0)
# If we wanted to return all the elements we would get the index from min or find the index
# where the min pose is at
# (vol_list.poses[index], vol_list.is_shadowed[index], vol_list.starting_mass[index],
# vol_list.volatile_type[index])
- def remove_closest_vol_pose(self):
+ def remove_closest_vol(self):
try:
while rospy.get_param("/volatile_locations_latch", default=False):
rospy.sleep(0.2) # wait for it be be unlatched
diff --git a/src/scoot/src/behaviors/excavator/dig.py b/src/scoot/src/behaviors/excavator/dig.py
index bdc4219..02b750a 100755
--- a/src/scoot/src/behaviors/excavator/dig.py
+++ b/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"
diff --git a/src/scoot/src/behaviors/excavator/dropoff.py b/src/scoot/src/behaviors/excavator/dropoff.py
index f4f4642..07ab4bc 100755
--- a/src/scoot/src/behaviors/excavator/dropoff.py
+++ b/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)
diff --git a/src/scoot/src/behaviors/excavator/goto_volatile.py b/src/scoot/src/behaviors/excavator/goto_volatile.py
index 0509246..dd3066d 100755
--- a/src/scoot/src/behaviors/excavator/goto_volatile.py
+++ b/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__':
diff --git a/src/scoot/src/behaviors/scout/goto_repair_station.py b/src/scoot/src/behaviors/scout/goto_repair_station.py
index 5fe28e7..ec190b8 100644
--- a/src/scoot/src/behaviors/scout/goto_repair_station.py
+++ b/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)
diff --git a/src/scoot/src/behaviors/scout/searchRandomWalk.py b/src/scoot/src/behaviors/scout/searchRandomWalk.py
index d3e083d..cee0dc3 100755
--- a/src/scoot/src/behaviors/scout/searchRandomWalk.py
+++ b/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)