def coordinationLoop(self):
"""
Loop which continuously runs coordinating the outputs of all active behaviours
"""
if None not in [self._surgeresult, self._swayresult, self._depthresult,
self._pitchresult, self._yawresult]:
su_results, y_results, sw_results, p_results, d_results = self.copyResults()
# Combine the behaviours outputs
# Just plain adding them just now
desired_velocities = su_results + sw_results + y_results # TODO reinsert Surge and pitch!!!!!!
#desired_velocities = su_results + d_results + sw_results + y_results # TODO reinsert Surge and pitch!!!!!!
if not self._disableOutput:
print("Sending Request: [su sw dep rol pit yaw]")
print(" [{0:.2f} {1:.2f} {2:.2f} {3:.2f} {4:.2f} {5:.2f}]".format(
desired_velocities[0],
desired_velocities[1],
desired_velocities[2],
desired_velocities[3],
desired_velocities[4],
desired_velocities[5]))
if SEND_VEL_REQUEST:
# Send a velocity request msg to the vehicle pilot
msg = PilotRequest(velocity=desired_velocities)
msg.disable_axis = [0, 1, 1, 1, 1, 0]
self.pilot_vel_pub.publish(msg)
else:
# Send a force request - easiest to implement new subscriber for this in node_pilot.py??
pass
else:
pass
# ToDo: Change this to zero velocity request for Nessie real world tests?
# self.pilot_pos_pub.publish(PilotRequest(position=[0, 0, 0, 0, 0, 0.0]))
else:
print("Waiting for all actions to have outputted something ...")
# wps = [point.split(",") for point in wps_raw]
# Wait for Nav
while not rospy.is_shutdown() and _stats == None:
rospy.sleep(0.5)
print "Waiting for nav"
wps = [[0, 0, 2.35], [10, 0, 0.0, 0.0]]
nav_reset()
rospy.sleep(2)
logNav(log_nav=True, dir_path="/tmp/", file_name_descriptor=str(0))
for wp in wps:
wp_msg = PilotRequest()
wp_msg.disable_axis = [0, 1, 0, 0, 0, 0]
goal_msg = BehavioursGoal()
cnt = 0
while not rospy.is_shutdown():
if wp != wps[0]:
angle = math.atan2(wp[1] - _stats.position.east,
wp[0] - _stats.position.north)
# new_heading = environment.raw_angle_to_goal + _stats.orientation.yaw
wp_msg.position = [wp[0], wp[1], 0, 0, 0, angle]
else:
wp_msg.position = [wp[0], wp[1], 0, 0, 0, wp[2]]
goal_msg.goal_position = Vector6(
values=[wp[0], wp[1], 0, 0, 0, wp[2]])