def set_rc_velocity(self, x_rc_vel, y_rc_vel, timeout):
"""Set fixed velocities"""
goal = MoveRobotGoal(actionID='set_rcvel',
x_rc_vel=x_rc_vel,
y_rc_vel=y_rc_vel)
self._ac.send_goal(goal)
to = rospy.Duration(secs=timeout)
res = self._ac.wait_for_result(to)
if not res:
goal = MoveRobotGoal(actionID='rc_off')
self._ac.send_goal(goal)
rospy.loginfo("Velocity set timed out")
def gate_pass(self, x_rc_vel, target_heading, target_depth, timeout):
"""Set fixed velocities"""
goal = MoveRobotGoal(actionID='gate_pass',
x_rc_vel=x_rc_vel,
target_heading=target_heading,
target_depth=target_depth)
self._ac.send_goal(goal)
to = rospy.Duration(secs=timeout)
res = self._ac.wait_for_result(to)
if not res:
goal = MoveRobotGoal(actionID='rc_off')
self._ac.send_goal(goal)
rospy.loginfo("Gate pass set timed out")
def heading_change(self, target_heading, timeout):
"""command a heading change
target_heading: desired heading in Degrees, magnetic north
"""
goal = MoveRobotGoal(actionID='heading_change',
target_heading=target_heading)
self._ac.send_goal(goal)
to = rospy.Duration(secs=timeout)
res = self._ac.wait_for_result(to)
if not res:
goal = MoveRobotGoal(actionID='rc_off')
self._ac.send_goal(goal)
rospy.loginfo("Heading change timed out")
def depth_change(self, target_depth, timeout):
"""command a depth change
target_depth: depth in meters, positive is down from surface
"""
goal = MoveRobotGoal(actionID='depth_change',
target_depth=target_depth)
self._ac.send_goal(goal)
to = rospy.Duration(secs=timeout)
res = self._ac.wait_for_result(to)
if not res:
goal = MoveRobotGoal(actionID='rc_off')
self._ac.send_goal(goal)
rospy.loginfo("Depth change timed out")
def finished(self):
"""Shut down server"""
goal = MoveRobotGoal(actionID='arm', arm=False)
self._ac.send_goal(goal)
to = rospy.Duration(secs=1.)
res = self._ac.wait_for_result(to)
self._ac.cancel_all_goals()
def object_center(self, x_rc_vel, target_heading, timeout):
"""Set fixed velocities"""
goal = MoveRobotGoal(actionID='object_center',
x_rc_vel=x_rc_vel,
target_heading=target_heading)
self._ac.send_goal(goal)
to = rospy.Duration(secs=timeout)
res = self._ac.wait_for_result(to)
is_togate = True
if not res:
is_togate=False
goal = MoveRobotGoal(actionID='rc_off')
self._ac.send_goal(goal)
rospy.loginfo("Object follow timed out")
return is_togate
def begin(self):
"""arm vehicle to begin moving"""
goal = MoveRobotGoal(actionID='arm', arm=True)
self._ac.send_goal(goal)
to = rospy.Duration(secs=1.)
res = self._ac.wait_for_result(to)