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)
rospy.sleep(0.5)
print "Waiting for nav"
#wps = [[0, 0], [5, 5]]
idx = 1
for wp in wps:
wp_msg = WorldWaypointReq()
print wp[0]
wp_msg.position.north = float(wp[0])
wp_msg.position.east = float(wp[1])
wp_msg.position.depth = 0
wp_msg.goal.id = idx
# Wait until the waypoint has been reached
if _stats != None and wp_msg != None:
while not (rospy.is_shutdown() and utils.epsilonEqualsNED(
_stats.position, wp_msg.position, 0.5, depth_e=0.6)):
check = utils.epsilonEqualsNED(_stats.position,
wp_msg.position,
0.5,
depth_e=0.6)
print(
"Waypoint Achieved: %s" % utils.epsilonEqualsNED(
_stats.position, wp_msg.position, 0.5, depth_e=0.6))
print("Sending point: N: %s, E: %s" %
(wp_msg.position.north, wp_msg.position.east))
pub_wp.publish(wp_msg)
rospy.sleep(1)
if check == True:
break
if rospy.is_shutdown():
break
logNav(log_nav=True, file_name_descriptor=str(idx))
disable_behaviours(False)
# north = float(wp[0])
# east = float(wp[1])
north = 10.0
east = 10.0
wp_msg = WorldWaypointReq()
# print wp[0]
wp_msg.position.north = north
wp_msg.position.east = east
wp_msg.position.depth = 0
wp_msg.goal.id = idx
# Wait until the waypoint has been reached
if _stats != None and wp_msg != None:
while not (rospy.is_shutdown() and utils.epsilonEqualsNED(
_stats.position, wp_msg.position, 0.5, depth_e=10.0)):
check = utils.epsilonEqualsNED(_stats.position,
wp_msg.position,
0.5,
depth_e=10.0)
print("Waypoint Achieved: {0}".format(check))
print("Sending Goal Point: [{0}, {1}]".format(
wp_msg.position.north, wp_msg.position.east))
goal_pub.publish(Vector6(values=[north, east, 0, 0, 0, wp]))
rospy.sleep(1)
if check == True:
break
if rospy.is_shutdown():
break
else:
print "something was None!!"
for wp in wps:
north = float(wp[0])
east = float(wp[1])
wp_msg = WorldWaypointReq()
print wp[0]
wp_msg.position.north = north
wp_msg.position.east = east
wp_msg.position.depth = 0
wp_msg.goal.id = idx
# Wait until the waypoint has been reached
# depth error set to 10.0 atm due to depth not being controlled currently
if _stats != None and wp_msg != None:
while not (rospy.is_shutdown()
and utils.epsilonEqualsNED(_stats.position,
wp_msg.position,
GOAL_PRECISION,
depth_e=10.0)):
check = utils.epsilonEqualsNED(_stats.position,
wp_msg.position,
GOAL_PRECISION,
depth_e=10.0)
print("Waypoint Achieved: {0}".format(check))
print("Sending Goal Point: [{0}, {1}]".format(
wp_msg.position.north, wp_msg.position.east))
goal_pub.publish(Vector6(values=[north, east, 0, 0, 0, 0]))
rospy.sleep(1)
if check == True:
break
if rospy.is_shutdown():
break
else: