def loop(self):
# distance away from point in NE
t = rospy.Time.now().to_sec() % self.circle_period
added_circle = np.array([self.circle_radius * np.cos(2*np.pi * t / self.circle_period),
self.circle_radius * np.sin(2*np.pi * t / self.circle_period)])
if self.point_source == SOURCE_NAV_REAL:
if self.point_ll is None:
return
self.ne = fm.geo2ne(self.point_ll, self.circler_origin, self.geo_radius)
elif self.point_source == SOURCE_NAV_SIM:
if self.ne is None:
return
# elif self.point_source == SOURCE_STATIC:
# pass
req_position = np.zeros(6)
req_position[0:2] = self.ne + added_circle
pr = PilotRequest()
pr.header.stamp = rospy.Time.now()
pr.position = req_position
self.pos_req_pub.publish(pr)
def loop(self):
if self.switch is False:
return
if self.points is None:
return
if self.index >= len(self.points):
rospy.loginfo('%s: Path complete!', self.name)
self.clear_path()
return
if rospy.Time.now().to_sec() - self.t_start > self.timeout:
rospy.logwarn('%s: Timeout! Clearing the path!', self.name)
self.clear_path()
self.send_zero_req()
return
des_point = self.points[self.index]
if self.point_reached(des_point):
rospy.loginfo('%s: Point reached: %s.', self.name, des_point[0:2])
self.index += 1
else:
pr = PilotRequest()
pr.header.stamp = rospy.Time.now()
pr.position = des_point
self.position_pub.publish(pr)
def parse_body_req(self, payload_type, id, dispatch_time, body, origin_address):
values = struct.unpack(FORMAT[payload_type], body)
pilot_msg = PilotRequest()
pilot_msg.header.stamp = rospy.Time.from_sec(dispatch_time)
pilot_msg.position = list(values[0:6])
self.pub_body.publish(pilot_msg)
def coordinationLoop(self):
"""
Loop which continuously runs coordinating the outputs of all active behaviours
"""
if None not in [self._surgeresult, self._swayresult, self._yawresult]:
su_results, y_results, sw_results = self.copyResults()
# Combine the behaviours outputs
# Just plain adding them just now
desired_velocities = sw_results + su_results + y_results
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]))
self.pilot_vel_pub.publish(
PilotRequest(velocity=desired_velocities))
else:
pass
# self.pilot_pos_pub.publish(PilotRequest(position = [0, 0, 0, 0, 0, 1.0]))
else:
print("Waiting for all actions to have outputted something ...")
def _do_hover_callback(self, req):
goal = req.goal.values
rospy.loginfo(
"DoHover: received goal [{0}, {1}, {2}, {4}, {5}]".format(
goal[0], goal[1], goal[2], goal[3], goal[4], goal[5]))
action_start_time = rospy.get_time()
timeoutReached = False
# take the received goal and push it to the motor controls semantics
# get a publisher
pub = rospy.Publisher("/pilot/position_req", PilotRequest)
goalNED = NED()
goalNED.north, goalNED.east, goalNED.depth = goal[0], goal[1], goal[2]
goalRPY = RPY()
goalRPY.roll, goalRPY.pitch, goalRPY.yaw = goal[3], goal[4], goal[5]
# repeatedly call world waypoint req to move the robot to the goal
# while the robot is not at teh desired location
while not (rospy.is_shutdown()
or self._nav != None and utils.epsilonEqualsNED(
self._nav.position, goalNED, 0.5, depth_e=0.6)
and utils.epsilonEqualsY(self._nav.orientation, goalRPY, .2)
# we compare on just pitch and yaw for 5 axis robot
):
# publish the goal repeatedly
pilotMsg = PilotRequest()
pilotMsg.position = list(goal)
pub.publish(pilotMsg)
# rospy.loginfo("Sent Goal!!")
rospy.sleep(0.5)
if timeoutReached:
return DoHoverResponse(False)
print("Sleeping for a while ...")
rospy.sleep(4)
# pub and subscriber will be removed at the end of executes context
rospy.loginfo("DoHover: complete")
return DoHoverResponse(True)
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 ...")
def loop(self):
if self.timer >= SPIN_RATE:
print("Disabling Coordinator and keeping position ...")
if self.timer < 3 * SPIN_RATE:
self.coordinatorDisable(True)
self.disabled = True
if self.orig_position is None:
self.orig_position = [deepcopy(self._nav.position.north),
deepcopy(self._nav.position.east),
0.0, #deepcopy(self._nav.position.depth),
0.0, 0.0,
deepcopy(self._nav.orientation.yaw)]
self.pilot_request.publish(PilotRequest(position=self.orig_position,
disable_axis=[0,0,0,0,0,0], priority=127))
else:
print("running ok")
if self.disabled:
self.coordinatorDisable(False)
self.disabled = False
self.orig_position = None
self.timer += 1
def coordinationLoop(self):
"""
Loop which continuously runs coordinating the outputs of all active behaviours
"""
if (self._yresult is not None and self._tresult is not None):
# Combine the behaviours outputs
# Just plain adding them just now #TODO: weighted combination of behaviours outputs
r_results, y_results = self.copyResults()
desired_velocities = r_results + y_results
# desired_velocities = np.array(deepcopy(self._yresult))
if not self._disableOutput:
print("Sending Surge Request: {0}".format(desired_velocities[0]))
print("Sending Yaw Request: {0}".format(desired_velocities[-1]))
self.pilot_vel_pub.publish(PilotRequest(velocity = desired_velocities))
else:
pass
# self.pilot_pos_pub.publish(PilotRequest(position = [0, 0, 0, 0, 0, 1.0]))
else:
rospy.loginfo("Waiting for all actions to have outputted something ...")
# wps_raw = f.read().splitlines()
# 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]])
def pilotPublishPositionRequest(self, pose):
request = PilotRequest()
request.position = pose
self.pilot_position_request.publish(request)
def send_zero_req(self):
br = PilotRequest()
br.header.stamp = rospy.Time.now()
br.position = np.zeros(6).tolist()
rospy.loginfo('%s: Sending stay request at: %s', self.name, self.pose[0:2])
self.body_pub.publish(br)