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()