class AveragingSVR2(object):
def __init__(self):
self._action = SimpleActionServer('averaging',
AveragingAction,
auto_start=False)
self._action.register_preempt_callback(self.preempt_cb)
self._action.register_goal_callback(self.goal_cb)
self.reset_numbers()
rospy.Subscriber('number', Float32, self.execute_loop)
self._action.start()
def std_dev(self, lst):
ave = sum(lst) / len(lst)
return sum([x * x for x in lst]) / len(lst) - ave**2
def goal_cb(self):
self._goal = self._action.accept_new_goal()
rospy.loginfo('goal callback %s' % (self._goal))
def preempt_cb(self):
rospy.loginfo('preempt callback')
self.reset_numbers()
self._action.set_preempted(text='message for preempt')
def reset_numbers(self):
self._samples = []
def execute_loop(self, msg):
if (not self._action.is_active()):
return
self._samples.append(msg.data)
feedback = AveragingAction().action_feedback.feedback
feedback.sample = len(self._samples)
feedback.data = msg.data
feedback.mean = sum(self._samples) / len(self._samples)
feedback.std_dev = self.std_dev(self._samples)
self._action.publish_feedback(feedback)
## sending result
if (len(self._samples) >= self._goal.samples):
result = AveragingAction().action_result.result
result.mean = sum(self._samples) / len(self._samples)
result.std_dev = self.std_dev(self._samples)
rospy.loginfo('result: %s' % (result))
self.reset_numbers()
if (result.mean > 0.5):
self._action.set_succeeded(result=result,
text='message for succeeded')
else:
self._action.set_aborted(result=result,
text='message for aborted')
class AveragingSVR(object):
def __init__(self):
self._action = SimpleActionServer('averaging',
AveragingAction,
execute_cb=self.execute_cb,
auto_start=False)
self._action.register_preempt_callback(self.preempt_cb)
self._action.start()
def std_dev(self, lst):
ave = sum(lst) / len(lst)
return sum([x * x for x in lst]) / len(lst) - ave**2
def preempt_cb(self):
rospy.loginfo('preempt callback')
self._action.set_preempted(text='message for preempt')
def execute_cb(self, goal):
rospy.loginfo('execute callback: %s' % (goal))
feedback = AveragingAction().action_feedback.feedback
result = AveragingAction().action_result.result
## execute loop
rate = rospy.Rate(1 / (0.01 + 0.99 * random.random()))
samples = []
for i in range(goal.samples):
sample = random.random()
samples.append(sample)
feedback.sample = i
feedback.data = sample
feedback.mean = sum(samples) / len(samples)
feedback.std_dev = self.std_dev(samples)
self._action.publish_feedback(feedback)
rate.sleep()
if (not self._action.is_active()):
rospy.loginfo('not active')
return
## sending result
result.mean = sum(samples) / len(samples)
result.std_dev = self.std_dev(samples)
rospy.loginfo('result: %s' % (result))
if (result.mean > 0.5):
self._action.set_succeeded(result=result,
text='message for succeeded')
else:
self._action.set_aborted(result=result, text='message for aborted')
class PathExecutor:
goal_index = 0
reached_all_nodes = True
def __init__(self):
rospy.loginfo('__init__ start')
# create a node
rospy.init_node(NODE)
# Action server to receive goals
self.path_server = SimpleActionServer('/path_executor/execute_path', ExecutePathAction,
self.handle_path, auto_start=False)
self.path_server.start()
# publishers & clients
self.visualization_publisher = rospy.Publisher('/path_executor/current_path', Path)
# get parameters from launch file
self.use_obstacle_avoidance = rospy.get_param('~use_obstacle_avoidance', True)
# action server based on use_obstacle_avoidance
if self.use_obstacle_avoidance == False:
self.goal_client = SimpleActionClient('/motion_controller/move_to', MoveToAction)
else:
self.goal_client = SimpleActionClient('/bug2/move_to', MoveToAction)
self.goal_client.wait_for_server()
# other fields
self.goal_index = 0
self.executePathGoal = None
self.executePathResult = ExecutePathResult()
def handle_path(self, paramExecutePathGoal):
'''
Action server callback to handle following path in succession
'''
rospy.loginfo('handle_path')
self.goal_index = 0
self.executePathGoal = paramExecutePathGoal
self.executePathResult = ExecutePathResult()
if self.executePathGoal is not None:
self.visualization_publisher.publish(self.executePathGoal.path)
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
if not self.path_server.is_active():
return
if self.path_server.is_preempt_requested():
rospy.loginfo('Preempt requested...')
# Stop bug2
self.goal_client.cancel_goal()
# Stop path_server
self.path_server.set_preempted()
return
if self.goal_index < len(self.executePathGoal.path.poses):
moveto_goal = MoveToGoal()
moveto_goal.target_pose = self.executePathGoal.path.poses[self.goal_index]
self.goal_client.send_goal(moveto_goal, done_cb=self.handle_goal,
feedback_cb=self.handle_goal_preempt)
# Wait for result
while self.goal_client.get_result() is None:
if self.path_server.is_preempt_requested():
self.goal_client.cancel_goal()
else:
rospy.loginfo('Done processing goals')
self.goal_client.cancel_goal()
self.path_server.set_succeeded(self.executePathResult, 'Done processing goals')
return
rate.sleep()
self.path_server.set_aborted(self.executePathResult, 'Aborted. The node has been killed.')
def handle_goal(self, state, result):
'''
Handle goal events (succeeded, preempted, aborted, unreachable, ...)
Send feedback message
'''
feedback = ExecutePathFeedback()
feedback.pose = self.executePathGoal.path.poses[self.goal_index]
# state is GoalStatus message as shown here:
# http://docs.ros.org/diamondback/api/actionlib_msgs/html/msg/GoalStatus.html
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Succeeded finding goal %d", self.goal_index + 1)
self.executePathResult.visited.append(True)
feedback.reached = True
else:
rospy.loginfo("Failed finding goal %d", self.goal_index + 1)
self.executePathResult.visited.append(False)
feedback.reached = False
if not self.executePathGoal.skip_unreachable:
rospy.loginfo('Failed finding goal %d, not skipping...', self.goal_index + 1)
# Stop bug2
self.goal_client.cancel_goal()
# Stop path_server
self.path_server.set_succeeded(self.executePathResult,
'Unreachable goal in path. Done processing goals.')
#self.path_server.set_preempted()
#return
self.path_server.publish_feedback(feedback)
self.goal_index = self.goal_index + 1
def handle_goal_preempt(self, state):
'''
Callback for goal_client to check for preemption from path_server
'''
if self.path_server.is_preempt_requested():
self.goal_client.cancel_goal()
class FaceTracker(object):
DIST_THRESH = 0.0
def __init__(self):
self.rate = rospy.Rate(2)
self.tf = tf.TransformListener()
self.eye_speed = 0.2
self.head_speed = 0.7
self.blender_frame = "blender"
self.default_position = [1, 0, 0] # Looking straight ahead 1 metre
self.last_position = None
# Publishers for making the face and eyes look at a point
self.face_target_pub = rospy.Publisher("/blender_api/set_face_target",
Target,
queue_size=1)
self.gaze_target_pub = rospy.Publisher("/blender_api/set_gaze_target",
Target,
queue_size=1)
# Gaze action server
self.action_srv = SimpleActionServer("/gaze_action",
GazeAction,
execute_cb=self.execute_cb,
auto_start=False)
self.action_srv.start()
def execute_cb(self, goal):
rospy.loginfo("Target goal received: " + str(goal))
target_frame = goal.target
while not rospy.is_shutdown(
) and not self.action_srv.is_preempt_requested(
) and self.action_srv.is_active():
if self.tf.frameExists(target_frame) and self.tf.frameExists(
self.blender_frame):
time = self.tf.getLatestCommonTime(target_frame,
self.blender_frame)
position, quaternion = self.tf.lookupTransform(
self.blender_frame, target_frame, time)
update_target = self.last_position is None
if self.last_position is not None:
dist = FaceTracker.distance(position, self.last_position)
update_target = dist > FaceTracker.DIST_THRESH
if update_target:
self.gaze_at_point(position)
self.action_srv.publish_feedback(GazeActionFeedback())
self.rate.sleep()
# If gaze has been cancelled then set default position
self.gaze_at_point(self.default_position)
@staticmethod
def distance(p1, p2):
return math.sqrt(
math.pow(p1[0] - p2[0], 2) + math.pow(p1[1] - p2[1], 2) +
math.pow(p1[2] - p2[2], 2))
def gaze_at_point(self, position):
x = position[0]
y = position[1]
z = position[2]
self.point_eyes_at_point(x, y, z, self.eye_speed)
self.face_toward_point(x, y, z, self.head_speed)
self.last_position = position
def point_eyes_at_point(self, x, y, z, speed):
""" Turn the robot's eyes towards the given target point
:param float x: metres forward
:param float y: metres to robots left
:param float z:
:param float speed:
:return: None
"""
msg = Target()
msg.x = x
msg.y = y
msg.z = z
msg.speed = speed
self.gaze_target_pub.publish(msg)
rospy.logdebug("published gaze_at(x={}, y={}, z={}, speed={})".format(
x, y, z, speed))
def face_toward_point(self, x, y, z, speed):
""" Turn the robot's face towards the given target point.
:param float x: metres forward
:param float y: metres to robots left
:param float z:
:param float speed:
:return: None
"""
msg = Target()
msg.x = x
msg.y = y
msg.z = z
msg.speed = speed
self.face_target_pub.publish(msg)
rospy.logdebug("published face_(x={}, y={}, z={}, speed={})".format(
x, y, z, speed))
class PickUpActionServer():
def __init__(self):
# Initialize new node
rospy.init_node(NAME)#, anonymous=False)
#initialize the clients to interface with
self.arm_client = SimpleActionClient("smart_arm_action", SmartArmAction)
self.gripper_client = SimpleActionClient("smart_arm_gripper_action", SmartArmGripperAction)
self.move_client = SimpleActionClient("erratic_base_action", ErraticBaseAction)
self.move_client.wait_for_server()
self.arm_client.wait_for_server()
self.gripper_client.wait_for_server()
# Initialize tf listener (remove?)
self.tf = tf.TransformListener()
# Initialize erratic base action server
self.result = SmartArmGraspResult()
self.feedback = SmartArmGraspFeedback()
self.server = SimpleActionServer(NAME, SmartArmGraspAction, self.execute_callback)
#define the offsets
# These offsets were determines expirmentally using the simulator
# They were tested using points stamped with /map
self.xOffset = 0.025
self.yOffset = 0.0
self.zOffset = 0.12 #.05 # this does work!
rospy.loginfo("%s: Pick up Action Server is ready to accept goals", NAME)
rospy.loginfo("%s: Offsets are [%f, %f, %f]", NAME, self.xOffset, self.yOffset, self.zOffset )
def move_to(self, frame_id, position, orientation, vicinity=0.0):
goal = ErraticBaseGoal()
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.header.frame_id = frame_id
goal.target_pose.pose.position = position
goal.target_pose.pose.orientation = orientation
goal.vicinity_range = vicinity
self.move_client.send_goal(goal)
#print "going into loop"
while (not self.move_client.wait_for_result(rospy.Duration(0.01))):
# check for preemption
if self.server.is_preempt_requested():
rospy.loginfo("%s: Aborted: Action Preempted", NAME)
self.move_client.cancel_goal()
return GoalStatus.PREEMPTED
return self.move_client.get_state()
#(almost) blatent copy / past from wubble_head_action.py. Err, it's going to the wrong position
def transform_target_point(self, point, frameId):
#rospy.loginfo("%s: got %s %s %s %s", NAME, point.header.frame_id, point.point.x, point.point.y, point.point.z)
# Wait for tf info (time-out in 5 seconds)
self.tf.waitForTransform(frameId, point.header.frame_id, rospy.Time(), rospy.Duration(5.0))
# Transform target point & retrieve the pan angle
return self.tf.transformPoint(frameId, point)
#UNUSED
def move_base_feedback_cb(self, fb):
self.feedback.base_position = fb.base_position
if self.server.is_active():
self.server.publish_feedback(self.feedback)
# This moves the arm into a positions based on angles (in rads)
# It depends on the sources code in wubble_actions
def move_arm(self, shoulder_pan, shoulder_tilt, elbow_tilt, wrist_rotate):
goal = SmartArmGoal()
goal.target_joints = [shoulder_pan, shoulder_tilt, elbow_tilt, wrist_rotate]
self.arm_client.send_goal(goal, None, None, self.arm_position_feedback_cb)
self.arm_client.wait_for_goal_to_finish()
while not self.arm_client.wait_for_result(rospy.Duration(0.01)) :
# check for preemption
if self.server.is_preempt_requested():
rospy.loginfo("%s: Aborted: Action Preempted", NAME)
self.arm_client.cancel_goal()
return GoalStatus.PREEMPTED
return self.arm_client.get_state()
# This moves the wrist of the arm to the x, y, z coordinates
def reach_at(self, frame_id, x, y, z):
goal = SmartArmGoal()
goal.target_point = PointStamped()
goal.target_point.header.frame_id = frame_id
goal.target_point.point.x = x
goal.target_point.point.y = y
goal.target_point.point.z = z
rospy.loginfo("%s: Original point is '%s' [%f, %f, %f]", NAME, frame_id,\
goal.target_point.point.x, goal.target_point.point.y, goal.target_point.point.z)
goal.target_point = self.transform_target_point(goal.target_point, '/arm_base_link');
rospy.loginfo("%s: Transformed point is '/armbaselink' [%f, %f, %f]", NAME, goal.target_point.point.x, \
goal.target_point.point.y, goal.target_point.point.z)
goal.target_point.point.x = goal.target_point.point.x + self.xOffset
goal.target_point.point.y = goal.target_point.point.y + self.yOffset
goal.target_point.point.z = goal.target_point.point.z + self.zOffset
rospy.loginfo("%s: Transformed and Offset point is '/armbaselink' [%f, %f, %f]", NAME, goal.target_point.point.x, \
goal.target_point.point.y, goal.target_point.point.z)
self.arm_client.send_goal(goal, None, None, self.arm_position_feedback_cb)
self.arm_client.wait_for_goal_to_finish()
while not self.arm_client.wait_for_result(rospy.Duration(0.01)) :
# check for preemption
if self.server.is_preempt_requested():
rospy.loginfo("%s: Aborted: Action Preempted", NAME)
self.arm_client.cancel_goal()
return GoalStatus.PREEMPTED
return self.arm_client.get_state()
#This method is used passes updates from arm positions actions to the feedback
#of this server
def arm_position_feedback_cb(self, fb):
self.feedback.arm_position = fb.arm_position
if self.server.is_active():
self.server.publish_feedback(self.feedback)
#This moves the gripper to the given angles
def move_gripper(self, left_finger, right_finger):
goal = SmartArmGripperGoal()
goal.target_joints = [left_finger, right_finger]
self.gripper_client.send_goal(goal, None, None, self.gripper_position_feedback_cb)
#gripper_client.wait_for_goal_to_finish()
while not self.gripper_client.wait_for_result(rospy.Duration(0.01)) :
# check for preemption
if self.server.is_preempt_requested():
rospy.loginfo("%s: Aborted: Action Preempted", NAME)
self.gripper_client.cancel_goal()
return GoalStatus.PREEMPTED
return self.gripper_client.get_state()
#This method is used passes updates from arm positions actions to the feedback
#of this server
def gripper_position_feedback_cb(self, fb):
self.feedback.gripper_position = fb.gripper_position
if self.server.is_active():
self.server.publish_feedback(self.feedback)
#This method sets the results of the goal to the last feedback values
def set_results(self, success):
self.result.success = success
self.result.arm_position = self.feedback.arm_position
self.result.gripper_position = self.feedback.gripper_position
self.server.set_succeeded(self.result)
#This is the callback function that is executed whenever the server
#recieves a request
def execute_callback(self, goal):
rospy.loginfo("%s: Executing Grasp Action [%s, %f, %f, %f]", NAME, \
goal.target_point.header.frame_id, goal.target_point.point.x, \
goal.target_point.point.y, goal.target_point.point.z)
rospy.loginfo( "%s: Moving the arm to the cobra pose", NAME)
#move the arm into the cobra position
result = self.move_arm(0.0, 1.972222, -1.972222, 0.0)
if result == GoalStatus.PREEMPTED: #action has failed
rospy.loginfo("%s: 1st Move Arm (to cobra pose) Preempted", NAME)
self.server.set_preempted()
self.set_results(False)
return
elif result != GoalStatus.SUCCEEDED:
rospy.loginfo("%s: 1st Move Arm (to cobra pose) Failed", NAME)
self.set_results(False)
return
position = Point(x = goal.target_point.point.x, y = goal.target_point.point.y)
orientation = Quaternion(w=1.0)
self.move_to('/map', position, orientation, 0.5)
self.move_to('/map', position, orientation, 0.2)
rospy.sleep(0.2)
rospy.loginfo( "%s: Opening gripper", NAME)
#open the gripper
result = self.move_gripper(0.2, -0.2)
if result == GoalStatus.PREEMPTED: #action has failed
rospy.loginfo("%s: Open Gripper Preempted", NAME)
self.server.set_preempted()
self.set_results(False)
return
elif result != GoalStatus.SUCCEEDED:
rospy.loginfo("%s: Open Gripper Failed", NAME)
self.set_results(False)
return
rospy.loginfo( "%s: Moving the arm to the object", NAME)
#move the arm to the correct posisions
result = self.reach_at(goal.target_point.header.frame_id, \
goal.target_point.point.x, \
goal.target_point.point.y, \
goal.target_point.point.z)
if result == GoalStatus.PREEMPTED: #action has failed
rospy.loginfo("%s: 2nd Move Arm (to object) Preempted", NAME)
self.server.set_preempted()
self.set_results(False)
return
elif result != GoalStatus.SUCCEEDED:
rospy.loginfo("%s: 2nd Move Arm (to object) Failed", NAME)
self.set_results(False)
return
rospy.loginfo( "%s: Moving the elbow joint to the cobra pose", NAME)
#move the arm into the cobra position
result = self.move_arm(self.feedback.arm_position[0], self.feedback.arm_position[1], \
-3.14 / 2 - self.feedback.arm_position[1], self.feedback.arm_position[3])
if result == GoalStatus.PREEMPTED: #action has failed
rospy.loginfo("%s: Moving the elbow joint Preempted", NAME)
self.server.set_preempted()
self.set_results(False)
return
elif result != GoalStatus.SUCCEEDED:
rospy.loginfo("%s: Moving the elbow joint Failed", NAME)
self.set_results(False)
return
rospy.loginfo( "%s: Closing gripper", NAME)
#close the gripper
result = self.move_gripper(-1.5, 1.5)
if result == GoalStatus.PREEMPTED: #action has failed
rospy.loginfo("%s: Close Gripper Preempted", NAME)
self.server.set_preempted()
self.set_results(False)
return
#this actions 'succeeds' if it times out
rospy.loginfo( "%s: Moving the arm to the cobra pose", NAME)
#move the arm into the cobra position
result = self.move_arm(0.0, 1.972222, -1.972222, 0.0)
if result == GoalStatus.PREEMPTED: #action has failed
rospy.loginfo("%s: 3rd Move Arm (to cobra pose) Preempted", NAME)
self.server.set_preempted()
self.set_results(False)
return
elif result != GoalStatus.SUCCEEDED:
rospy.loginfo("%s: 3rd Move Arm (to cobra pose) Failed", NAME)
self.set_results(False)
return
#action has succeeded
rospy.loginfo("%s: Grasp Action Succeed [%s, %f, %f, %f]", NAME, \
goal.target_point.header.frame_id, goal.target_point.point.x, \
goal.target_point.point.y, goal.target_point.point.z)
self.set_results(True)
"""
class MoveEndEffectorServer(object):
def __init__(self):
self._name = "/PANDORA_CONTROL/PANDORA_END_EFFECTOR_CONTROLLER"
self.factory = ClientFactory()
self.current_clients = list()
self.current_goal = MoveEndEffectorGoal()
self.server = Server(move_end_effector_controller_topic,
MoveEndEffectorAction, self.execute_cb, False)
self.server.register_preempt_callback(self.preempt_cb)
self.server.start()
self.create_clients()
self.wait_for_servers()
def create_clients(self):
""" Imports all clients and creates a list of them """
for client in CLIENTS:
self.current_clients.append(self.factory.make_client(client))
def wait_for_servers(self):
""" Waits for every client to connect with their server """
for client in self.current_clients:
client.wait_server()
def execute_cb(self, goal):
""" Callback triggered by the arrival of a goal """
self.current_goal = goal
self.fill_goals()
self.send_goals()
self.wait_for_result()
rospy.logwarn("break from wait for result!")
self.checkGoalState()
def preempt_cb(self):
""" Preempting all goals """
for client in self.current_clients:
client.preempt_if_active()
def fill_goals(self):
""" Filling goals into every client respectively """
for client in self.current_clients:
client.fill_goal(self.current_goal)
def send_goals(self):
""" Sending goals to every client respectively """
for client in self.current_clients:
client.send_goal()
def wait_for_result(self):
for client in self.current_clients:
client.wait_result()
def success(self):
self.server.set_succeeded()
def abort(self):
self.server.set_aborted()
def preempt(self):
if self.server.is_active():
self.server.set_preempted()
else:
rospy.logerr("[" + self._name +
"] Preempt requested when server goal is not active")
def check_succeeded(self):
""" Checks if the final state of the goal must be set succeeded """
must_succeed = True
for client in self.current_clients:
must_succeed = must_succeed and client.has_succeeded()
return must_succeed
def check_aborted(self):
""" Checks if the final state of the goal must be set aborted """
must_abort = False
for client in self.current_clients:
must_abort = must_abort or client.has_aborted()
return must_abort
def check_preempted(self):
""" Checks if the final state of the goal must be set preempted """
must_preempt = False
for client in self.current_clients:
must_preempt = must_preempt or client.has_preempted()
return must_preempt
def check_recalled(self):
""" Checks if the final state of the goal must be set preempted """
must_recall = False
for client in self.current_clients:
must_recall = must_recall or client.has_been_recalled()
return must_recall
def checkGoalState(self):
""" Checking final state of goal """
if (self.check_succeeded()):
rospy.logwarn("check_succeeded")
self.success()
elif (self.check_aborted()):
rospy.logwarn("check_aborted")
self.abort()
elif (self.check_recalled()):
rospy.logwarn("check_recalled")
for client in self.current_clients:
rospy.logerr("[" + self._name + "] Client " +
client.get_name() + " is in state " +
str(client.client.get_state()))
rospy.logerr(
"[" + self._name +
"] One client at least is recalled, set server to aborted")
self.abort()
elif (self.check_preempted()):
rospy.logwarn("check_preempted")
self.preempt()
else:
rospy.logwarn("check_unexpected")
for client in self.current_clients:
rospy.logerr("[" + self._name + "] Client " +
client.get_name() + " is in state " +
str(client.client.get_state()))
rospy.logerr("[" + self._name +
"] Unexpected State, set server to aborted")
self.abort()
class ErraticBaseActionServer():
def __init__(self):
self.base_frame = '/base_footprint'
self.move_client = SimpleActionClient('move_base', MoveBaseAction)
self.move_client.wait_for_server()
self.tf = tf.TransformListener()
self.result = ErraticBaseResult()
self.feedback = ErraticBaseFeedback()
self.server = SimpleActionServer(NAME,
ErraticBaseAction,
self.execute_callback,
auto_start=False)
self.server.start()
rospy.loginfo("%s: Ready to accept goals", NAME)
def transform_target_point(self, point):
self.tf.waitForTransform(self.base_frame, point.header.frame_id,
rospy.Time(), rospy.Duration(5.0))
return self.tf.transformPoint(self.base_frame, point)
def move_to(self, target_pose):
goal = MoveBaseGoal()
goal.target_pose = target_pose
goal.target_pose.header.stamp = rospy.Time.now()
self.move_client.send_goal(goal=goal,
feedback_cb=self.move_base_feedback_cb)
while not self.move_client.wait_for_result(rospy.Duration(0.01)):
# check for preemption
if self.server.is_preempt_requested():
rospy.loginfo("%s: Aborted: Action Preempted", NAME)
self.move_client.cancel_goal()
return GoalStatus.PREEMPTED
return self.move_client.get_state()
def move_base_feedback_cb(self, fb):
self.feedback.base_position = fb.base_position
if self.server.is_active():
self.server.publish_feedback(self.feedback)
def get_vicinity_target(self, target_pose, vicinity_range):
vicinity_pose = PoseStamped()
# transform target to base_frame reference
target_point = PointStamped()
target_point.header.frame_id = target_pose.header.frame_id
target_point.point = target_pose.pose.position
self.tf.waitForTransform(self.base_frame, target_pose.header.frame_id,
rospy.Time(), rospy.Duration(5.0))
target = self.tf.transformPoint(self.base_frame, target_point)
rospy.logdebug("%s: Target at (%s, %s, %s)", NAME, target.point.x,
target.point.y, target.point.z)
# find distance to point
dist = math.sqrt(
math.pow(target.point.x, 2) + math.pow(target.point.y, 2))
if (dist < vicinity_range):
# if already within range, then no need to move
vicinity_pose.pose.position.x = 0.0
vicinity_pose.pose.position.y = 0.0
else:
# normalize vector pointing from source to target
target.point.x /= dist
target.point.y /= dist
# scale normal vector to within vicinity_range distance from target
target.point.x *= (dist - vicinity_range)
target.point.y *= (dist - vicinity_range)
# add scaled vector to source
vicinity_pose.pose.position.x = target.point.x + 0.0
vicinity_pose.pose.position.y = target.point.y + 0.0
# set orientation
ori = Quaternion()
yaw = math.atan2(target.point.y, target.point.x)
(ori.x, ori.y, ori.z,
ori.w) = tf.transformations.quaternion_from_euler(0, 0, yaw)
vicinity_pose.pose.orientation = ori
# prep header
vicinity_pose.header = target_pose.header
vicinity_pose.header.frame_id = self.base_frame
rospy.logdebug("%s: Moving to (%s, %s, %s)", NAME,
vicinity_pose.pose.position.x,
vicinity_pose.pose.position.y,
vicinity_pose.pose.position.z)
return vicinity_pose
def execute_callback(self, goal):
rospy.loginfo("%s: Executing move base", NAME)
move_base_result = None
if goal.vicinity_range == 0.0:
# go to exactly
move_base_result = self.move_to(goal.target_pose)
else:
# go near (within vicinity_range meters)
vicinity_target_pose = self.get_vicinity_target(
goal.target_pose, goal.vicinity_range)
move_base_result = self.move_to(vicinity_target_pose)
# check results
if (move_base_result == GoalStatus.SUCCEEDED):
rospy.loginfo("%s: Succeeded", NAME)
self.result.base_position = self.feedback.base_position
self.server.set_succeeded(self.result)
elif (move_base_result == GoalStatus.PREEMPTED):
rospy.loginfo("%s: Preempted", NAME)
self.server.set_preempted()
else:
rospy.loginfo("%s: Aborted", NAME)
self.server.set_aborted()
class ErraticBaseActionServer():
def __init__(self):
self.base_frame = '/base_footprint'
self.move_client = SimpleActionClient('move_base', MoveBaseAction)
self.move_client.wait_for_server()
self.tf = tf.TransformListener()
self.result = ErraticBaseResult()
self.feedback = ErraticBaseFeedback()
self.server = SimpleActionServer(NAME, ErraticBaseAction, self.execute_callback, auto_start=False)
self.server.start()
rospy.loginfo("%s: Ready to accept goals", NAME)
def transform_target_point(self, point):
self.tf.waitForTransform(self.base_frame, point.header.frame_id, rospy.Time(), rospy.Duration(5.0))
return self.tf.transformPoint(self.base_frame, point)
def move_to(self, target_pose):
goal = MoveBaseGoal()
goal.target_pose = target_pose
goal.target_pose.header.stamp = rospy.Time.now()
self.move_client.send_goal(goal=goal, feedback_cb=self.move_base_feedback_cb)
while not self.move_client.wait_for_result(rospy.Duration(0.01)):
# check for preemption
if self.server.is_preempt_requested():
rospy.loginfo("%s: Aborted: Action Preempted", NAME)
self.move_client.cancel_goal()
return GoalStatus.PREEMPTED
return self.move_client.get_state()
def move_base_feedback_cb(self, fb):
self.feedback.base_position = fb.base_position
if self.server.is_active():
self.server.publish_feedback(self.feedback)
def get_vicinity_target(self, target_pose, vicinity_range):
vicinity_pose = PoseStamped()
# transform target to base_frame reference
target_point = PointStamped()
target_point.header.frame_id = target_pose.header.frame_id
target_point.point = target_pose.pose.position
self.tf.waitForTransform(self.base_frame, target_pose.header.frame_id, rospy.Time(), rospy.Duration(5.0))
target = self.tf.transformPoint(self.base_frame, target_point)
rospy.logdebug("%s: Target at (%s, %s, %s)", NAME, target.point.x, target.point.y, target.point.z)
# find distance to point
dist = math.sqrt(math.pow(target.point.x, 2) + math.pow(target.point.y, 2))
if (dist < vicinity_range):
# if already within range, then no need to move
vicinity_pose.pose.position.x = 0.0
vicinity_pose.pose.position.y = 0.0
else:
# normalize vector pointing from source to target
target.point.x /= dist
target.point.y /= dist
# scale normal vector to within vicinity_range distance from target
target.point.x *= (dist - vicinity_range)
target.point.y *= (dist - vicinity_range)
# add scaled vector to source
vicinity_pose.pose.position.x = target.point.x + 0.0
vicinity_pose.pose.position.y = target.point.y + 0.0
# set orientation
ori = Quaternion()
yaw = math.atan2(target.point.y, target.point.x)
(ori.x, ori.y, ori.z, ori.w) = tf.transformations.quaternion_from_euler(0, 0, yaw)
vicinity_pose.pose.orientation = ori
# prep header
vicinity_pose.header = target_pose.header
vicinity_pose.header.frame_id = self.base_frame
rospy.logdebug("%s: Moving to (%s, %s, %s)", NAME, vicinity_pose.pose.position.x, vicinity_pose.pose.position.y, vicinity_pose.pose.position.z)
return vicinity_pose
def execute_callback(self, goal):
rospy.loginfo("%s: Executing move base", NAME)
move_base_result = None
if goal.vicinity_range == 0.0:
# go to exactly
move_base_result = self.move_to(goal.target_pose)
else:
# go near (within vicinity_range meters)
vicinity_target_pose = self.get_vicinity_target(goal.target_pose, goal.vicinity_range)
move_base_result = self.move_to(vicinity_target_pose)
# check results
if (move_base_result == GoalStatus.SUCCEEDED):
rospy.loginfo("%s: Succeeded", NAME)
self.result.base_position = self.feedback.base_position
self.server.set_succeeded(self.result)
elif (move_base_result == GoalStatus.PREEMPTED):
rospy.loginfo("%s: Preempted", NAME)
self.server.set_preempted()
else:
rospy.loginfo("%s: Aborted", NAME)
self.server.set_aborted()
