def path_client(self, x_0, y_0, x_1, y_1, u_d, z_d, R):
"""
action client guide
https://github.com/strawlab/ros_common/blob/master/actionlib/src/actionlib/simple_action_client.py
"""
# creates a goal to send to the action server
_goal = LosPathFollowingGoal()
# create line segment
_goal.next_waypoint.x = x_1
_goal.next_waypoint.y = y_1
_goal.prev_waypoint.x = x_0
_goal.prev_waypoint.y = y_0
# set speed goal
_goal.forward_speed.linear.x = u_d
# set depth hold goal
_goal.desired_depth.z = z_d
# sphere of acceptance
_goal.sphereOfAcceptance = R
rospy.loginfo("sending goal pose to Action Server")
return _goal
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 done_cb(self, status, result):
# Gets called on transition state
if status == 1:
rospy.loginfo("active tracking")
if status == 2:
rospy.loginfo("preempted")
if status == 3:
rospy.loginfo("Goal pose reached")
if status == 4:
rospy.loginfo("Goal aborted")
if status == 5:
rospy.loginfo("Goal rejected")
# creates a goal to send to the action server
_goal = LosPathFollowingGoal()
# create line segment
_goal.next_waypoint.x = 7.0 # 6.0
_goal.next_waypoint.y = -2.0 # 2.0
_goal.prev_waypoint.x = 3.0 # 3.0
_goal.prev_waypoint.y = -2.0 #-2.0
# set speed goal
_goal.forward_speed.linear.x = 0.20
# set desired depth
_goal.desired_depth.z = -0.50
# sphere of acceptance
_goal.sphereOfAcceptance = 0.3
rospy.loginfo("sending goal pose to Action Server")
self.client.send_goal(_goal, self.done_cb, self.active_cb,
self.feedback_cb)
def path_client(self):
"""
action client guide
https://github.com/strawlab/ros_common/blob/master/actionlib/src/actionlib/simple_action_client.py
"""
# creates a goal to send to the action server
_goal = LosPathFollowingGoal()
# create line segment
_goal.next_waypoint.x = 3.0 # 3.0
_goal.next_waypoint.y = 1.0 # -2.0
_goal.prev_waypoint.x = 1.0 # 0.0
_goal.prev_waypoint.y = 1.0 # 0.0
# set speed goal
_goal.forward_speed.linear.x = 0.20
# set desired depth
_goal.desired_depth.z = -0.50
# sphere of acceptance
_goal.sphereOfAcceptance = 0.3
rospy.loginfo("sending goal pose to Action Server")
# Send goal
self.client.send_goal(_goal, self.done_cb, self.active_cb,
self.feedback_cb)
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()
def make_los_goal(x_prev,
y_prev,
x_next,
y_next,
depth,
speed=0.20,
sphere_of_acceptance=0.3):
los_goal = LosPathFollowingGoal()
los_goal.prev_waypoint.x = x_prev
los_goal.prev_waypoint.y = y_prev
los_goal.next_waypoint.x = x_next
los_goal.next_waypoint.y = y_next
los_goal.next_waypoint.z = depth
los_goal.forward_speed.linear.x = speed
los_goal.sphereOfAcceptance = sphere_of_acceptance
return los_goal
def fibonacci_client():
# Creates the SimpleActionClient, passing the type of the action
# (FibonacciAction) to the constructor.
client = actionlib.SimpleActionClient('inspect_point_srv',
LosPathFollowingAction)
# Waits until the action server has started up and started
# listening for goals.
client.wait_for_server()
# Creates a goal to send to the action server.
goal = LosPathFollowingGoal()
goal.next_waypoint.x = 2.0
goal.next_waypoint.y = 2.0
goal.sphereOfAcceptance = 2.0
goal.desired_depth.z = -0.5
# Sends the goal to the action server.
client.send_goal(goal)
# Waits for the server to finish performing the action.
client.wait_for_result()