def __init__(
self,
goal_positon,
travel_speed=0.5,
sphere_of_acceptance=0.5,
action_server="/guidance_interface/los_server",
):
"""A SimpleActionState for traveling to a goal position using LOS guidance.
Args:
goal_positon (geometry_msgs/Point): position drone should travel to.
start_position (geometry_msgs/Point): position drone starts in.
travel_speed (float, optional):
forward velocity drone shoudl travel at. Defaults to 0.5.
sphere_of_acceptance (float, optional):
action returns success when drone is inside this sphere. Defaults to 0.5.
action_server (str, optional):
name of los action server. Defaults to "/guidance_interface/los_server".
"""
goal = LosPathFollowingGoal()
goal.next_waypoint = goal_positon
goal.forward_speed = travel_speed
goal.desired_depth = goal_positon.z
goal.sphereOfAcceptance = sphere_of_acceptance
SimpleActionState.__init__(self, action_server, LosPathFollowingAction,
goal)
def move_cb(self, move_goal):
"""
Converts move_goal into the proper goal type for the desired controller and sends it.
Aborts action if it is not completed within self.timeout seconds
"""
if move_goal.controller_name == "DP":
change_control_mode(POSE_HEADING_HOLD)
dp_goal = MoveBaseGoal()
dp_goal.target_pose.pose = move_goal.target_pose
self.dp_client.send_goal(dp_goal,
done_cb=self.done_cb,
feedback_cb=None)
if not self.dp_client.wait_for_result(
timeout=rospy.Duration(self.timeout)):
self.action_server.set_aborted()
rospy.loginfo('DP controller aborted action due to timeout')
elif move_goal.controller_name == "LOS":
change_control_mode(OPEN_LOOP)
los_goal = LosPathFollowingGoal()
los_goal.next_waypoint = move_goal.target_pose.position
los_goal.forward_speed.linear.x = self.transit_speed
los_goal.sphereOfAcceptance = self.sphere_of_acceptance
los_goal.desired_depth.z = move_goal.target_pose.position.z
self.los_client.send_goal(los_goal,
done_cb=self.done_cb,
feedback_cb=None)
if not self.los_client.wait_for_result(
timeout=rospy.Duration(self.timeout)):
self.action_server.set_aborted()
rospy.loginfo('LOS controller aborted action due to timeout')
else:
rospy.logerr(
'Unknown controller name sent to controller_interface')
self.action_server.set_aborted()