class ReadyArmActionServer: def __init__(self): self.move_arm_client = SimpleActionClient('/move_left_arm', MoveArmAction) self.ready_arm_server = SimpleActionServer(ACTION_NAME, ReadyArmAction, execute_cb=self.ready_arm, auto_start=False) def initialize(self): rospy.loginfo('%s: waiting for move_left_arm action server', ACTION_NAME) self.move_arm_client.wait_for_server() rospy.loginfo('%s: connected to move_left_arm action server', ACTION_NAME) self.ready_arm_server.start() def ready_arm(self, goal): if self.ready_arm_server.is_preempt_requested(): rospy.loginfo('%s: preempted' % ACTION_NAME) self.move_arm_client.cancel_goal() self.ready_arm_server.set_preempted() if move_arm_joint_goal(self.move_arm_client, ARM_JOINTS, READY_POSITION, collision_operations=goal.collision_operations): self.ready_arm_server.set_succeeded() else: rospy.logerr('%s: failed to ready arm, aborting', ACTION_NAME) self.ready_arm_server.set_aborted()
class BaseRotate(): def __init__(self): self._action_name = 'base_rotate' #initialize base controller topic_name = '/base_controller/command' self._base_publisher = rospy.Publisher(topic_name, Twist) #initialize this client self._as = SimpleActionServer(self._action_name, BaseRotateAction, execute_cb=self.run, auto_start=False) self._as.start() rospy.loginfo('%s: started' % self._action_name) def run(self, goal): rospy.loginfo('Rotating base') count = 0 r = rospy.Rate(10) while count < 70: if self._as.is_preempt_requested(): self._as.set_preempted() return twist_msg = Twist() twist_msg.linear = Vector3(0.0, 0.0, 0.0) twist_msg.angular = Vector3(0.0, 0.0, 1.0) self._base_publisher.publish(twist_msg) count = count + 1 r.sleep() self._as.set_succeeded()
class TrajectoryExecution: def __init__(self, side_prefix): traj_controller_name = '/' + side_prefix + '_arm_controller/joint_trajectory_action' self.traj_action_client = SimpleActionClient(traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory action server arm: ' + side_prefix) self.traj_action_client.wait_for_server() rospy.loginfo('Trajectory executor is ready for arm: ' + side_prefix) motion_request_name = '/' + side_prefix + '_arm_motion_request/joint_trajectory_action' self.action_server = SimpleActionServer(motion_request_name, JointTrajectoryAction, execute_cb=self.move_to_joints) self.action_server.start() self.has_goal = False def move_to_joints(self, traj_goal): rospy.loginfo('Receieved a trajectory execution request.') traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1)) self.traj_action_client.send_goal(traj_goal) rospy.sleep(1) is_terminated = False while(not is_terminated): action_state = self.traj_action_client.get_state() if (action_state == GoalStatus.SUCCEEDED): self.action_server.set_succeeded() is_terminated = True elif (action_state == GoalStatus.ABORTED): self.action_server.set_aborted() is_terminated = True elif (action_state == GoalStatus.PREEMPTED): self.action_server.set_preempted() is_terminated = True rospy.loginfo('Trajectory completed.')
class ReadyArmActionServer: def __init__(self): self.move_arm_client = SimpleActionClient('/move_left_arm', MoveArmAction) self.ready_arm_server = SimpleActionServer(ACTION_NAME, ReadyArmAction, execute_cb=self.ready_arm, auto_start=False) def initialize(self): rospy.loginfo('%s: waiting for move_left_arm action server', ACTION_NAME) self.move_arm_client.wait_for_server() rospy.loginfo('%s: connected to move_left_arm action server', ACTION_NAME) self.ready_arm_server.start() def ready_arm(self, goal): if self.ready_arm_server.is_preempt_requested(): rospy.loginfo('%s: preempted' % ACTION_NAME) self.move_arm_client.cancel_goal() self.ready_arm_server.set_preempted() if move_arm_joint_goal(self.move_arm_client, ARM_JOINTS, READY_POSITION, collision_operations=goal.collision_operations): self.ready_arm_server.set_succeeded() else: rospy.logerr('%s: failed to ready arm, aborting', ACTION_NAME) self.ready_arm_server.set_aborted()
class PipolFollower(): def __init__(self): rospy.loginfo("Creating Pipol follower AS: '" + PIPOL_FOLLOWER_AS + "'") self._as = SimpleActionServer(PIPOL_FOLLOWER_AS, PipolFollowAction, execute_cb = self.execute_cb, preempt_callback = self.preempt_cb, auto_start = False) rospy.loginfo("Starting " + PIPOL_FOLLOWER_AS) self._as.start() def execute_cb(self, goal): print "goal is: " + str(goal) # helper variables success = True # start executing the action for i in xrange(1, goal.order): # check that preempt has not been requested by the client if self._as.is_preempt_requested(): rospy.loginfo('%s: Preempted' % self._action_name) self._as.set_preempted() success = False break self._feedback.sequence.append(self._feedback.sequence[i] + self._feedback.sequence[i-1]) # publish the feedback self._as.publish_feedback(self._feedback) # this step is not necessary, the sequence is computed at 1 Hz for demonstration purposes r.sleep() if success: self._result.sequence = self._feedback.sequence rospy.loginfo('%s: Succeeded' % self._action_name) self._as.set_succeeded(self._result)
class MockActionServer(object): """ MockActionServer base class """ def __init__(self, name, topic, action_type): """ Creating a custom mock action server.""" self._topic = topic self._name = name self._action_type = action_type self.timeout = 5 self.action_result = None Subscriber('mock/' + name, String, self.receive_commands) Subscriber('mock/gui_result', Bool, self.set_gui_result) self._server = ActionServer(self._topic, self._action_type, self.success, False) self._server.start() loginfo('>>> Starting ' + self._name) def receive_commands(self, msg): """ Decides the result of the next call. """ callback, timeout = msg.data.split(':') self.timeout = float(timeout) self._server.execute_callback = getattr(self, callback) logwarn('>>> ' + self._name + ': Current callback -> ' + callback) sleep(1) def abort(self, goal): """ Aborts any incoming goal. """ logwarn('>>> ' + self._name + ': This goal will be aborted.') sleep(self.timeout) self._server.set_aborted() def success(self, goal): """ Succeeds any incoming goal. """ logwarn('>>> ' + self._name + ': This goal will succeed.') sleep(self.timeout) self._server.set_succeeded(self.action_result) def preempt(self, goal): """ Preempts any incoming goal. """ logwarn('>>> ' + self._name + ': This goal will be preempted.') sleep(self.timeout) self._server.set_preempted() def set_gui_result(self, msg): """ Sets the result of the goal. """ self.action_result = ValidateVictimGUIResult() logwarn('>>> The gui response will be: ' + str(msg.data)) self.action_result.victimValid = msg.data
class MockExplorer(): def __init__(self, exploration_topic): self.robot_pose_ = PoseStamped() self.listener = tf.TransformListener() self.navigation_succedes = True self.reply = False self.preempted = 0 self.entered_exploration = False self.do_exploration_as_ = SimpleActionServer( exploration_topic, DoExplorationAction, execute_cb=self.do_exploration_cb, auto_start=False) self.do_exploration_as_.start() def __del__(self): self.do_exploration_as_.__del__() def do_exploration_cb(self, goal): rospy.loginfo('do_exploration_cb') self.entered_exploration = True while not self.reply: rospy.sleep(0.2) (trans, rot) = self.listener.lookupTransform('/map', '/base_footprint', rospy.Time(0)) self.robot_pose_.pose.position.x = trans[0] self.robot_pose_.pose.position.y = trans[1] feedback = DoExplorationFeedback() feedback.base_position.pose.position.x = \ self.robot_pose_.pose.position.x feedback.base_position.pose.position.y = \ self.robot_pose_.pose.position.y self.do_exploration_as_.publish_feedback(feedback) if self.do_exploration_as_.is_preempt_requested(): self.preempted += 1 rospy.loginfo("Preempted!") self.entered_exploration = False self.do_exploration_as_.set_preempted(DoExplorationResult()) return None else: result = DoExplorationResult() self.reply = False self.preempted = 0 self.entered_exploration = False if self.navigation_succedes: self.do_exploration_as_.set_succeded(result) else: self.do_exploration_as_.set_aborted(result)
class LookAndMoveNode(object): cmd_vel = Twist() def __init__(self): self.tf_listener = tf.TransformListener() self.pub_cmd_vel = rospy.Publisher('cmd_vel',Twist,queue_size=1) self.sub_odom = rospy.Subscriber('odom',Odometry,callback=self.OdomCB) self.is_stop = False self.target_pose = PoseStamped() self.observe_frame = '' self.state = LookAndMoveStatus.IDLE self._as = SimpleActionServer("look_n_move_node_action", LookAndMoveAction, self.ExecuteCB, auto_start=False) self._as.start() def ExecuteCB(self,goal): print 'LOOKnMOVE GOAL RCV' ps = goal.relative_pose self.observe_frame = ps.header.frame_id if len(ps.header.frame_id)!=0 else 'base_link' ps.header.frame_id = self.observe_frame ps.header.stamp = rospy.Time(0) self.tf_listener.waitForTransform(self.observe_frame, 'odom', rospy.Time(0),rospy.Duration(1.0)) self.target_pose = self.tf_listener.transformPose('odom',ps) while not rospy.is_shutdown(): if self._as.is_preempt_requested(): print 'PREEMPT REQ' rospy.sleep(0.1) self.SendCmdVel(0.,0.,0.) self._as.set_preempted() return self.tf_listener.waitForTransform('odom','base_link',rospy.Time(0),rospy.Duration(1.0)) pose_base = self.tf_listener.transformPose('base_link',self.target_pose) pose = pose_base.pose.position q = pose_base.pose.orientation yaw = tf.transformations.euler_from_quaternion([q.x,q.y,q.z,q.w])[2] if (np.abs(pose.x) < 0.02 and np.abs(pose.y) <0.02 and np.abs(yaw)<0.01) or ((np.abs(pose.x) < 0.05 and np.abs(pose.y) <0.05 and np.abs(yaw)<0.04) and self.is_stop): print 'SUCCESS' rospy.sleep(0.1) self.SendCmdVel(0.,0.,0.) self._as.set_succeeded() return vx = pose.x * KP_X vy = pose.y * KP_Y yaw = yaw * KP_YAW self.SendCmdVel(vx,vy,yaw) def OdomCB(self,data): self.is_stop = data.twist.twist.linear.x < 0.001 and data.twist.twist.linear.y < 0.001 def SendCmdVel(self, vx, vy, vyaw): self.cmd_vel.linear.x = np.clip(vx, -MAX_LINEAR_VEL, MAX_LINEAR_VEL) self.cmd_vel.linear.y = np.clip(vy, -MAX_LINEAR_VEL, MAX_LINEAR_VEL) self.cmd_vel.angular.z = np.clip(vyaw, -MAX_ANGULAR_VEL, MAX_ANGULAR_VEL) self.pub_cmd_vel.publish(self.cmd_vel)
class MockActionServer(object): """ MockActionServer base class """ def __init__(self, name, topic, action_type): """ Creating a custom mock action server.""" self._topic = topic self._name = name self._action_type = action_type self.timeout = 1 self.action_result = None self.prefered_callback = ' ' Subscriber('mock/' + name, String, self.receive_commands) Subscriber('mock/gui_result', Bool, self.set_gui_result) self._server = ActionServer(self._topic, self._action_type, self.decide, False) self._server.start() loginfo('>>> Starting ' + self._name) def receive_commands(self, msg): """ Decides the result of the next call. """ callback, timeout = msg.data.split(':') # self.timeout = float(timeout) self.prefered_callback = callback sleep(1) def decide(self, goal): """ Deciding outcome of goal """ logwarn('Deciding callback...') sleep(self.timeout) if(self.prefered_callback == 'success'): logwarn('>>> ' + self._name + ': This goal will succeed.') self._server.set_succeeded() elif(self.prefered_callback == 'abort'): logwarn('>>> ' + self._name + ': This goal will be aborted.') self._server.set_aborted() elif(self.prefered_callback == 'preempt'): logwarn('>>> ' + self._name + ': This goal will be preempted.') self._server.set_preempted() else: logwarn('wtf?') def set_gui_result(self, msg): """ Sets the result of the goal. """ self.action_result = ValidateVictimGUIResult() logwarn('>>> The gui response will be: ' + str(msg.data)) self.action_result.victimValid = msg.data
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 FibonacciActionServer(object): # create messages that are used to publish feedback/result feedback = FibonacciFeedback() result = FibonacciResult() def __init__(self, name): self.action_name = name self.action_server = SimpleActionServer(self.action_name, FibonacciAction, execute_cb=self.execute_cb, auto_start=False) self.action_server.start() def execute_cb(self, goal): # helper variables r = rospy.Rate(1) success = True # append the seeds for the fibonacci sequence self.feedback.sequence = [] self.feedback.sequence.append(0) self.feedback.sequence.append(1) # publish info to the console for the user rospy.loginfo( '%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i' % (self.action_name, goal.order, self.feedback.sequence[0], self.feedback.sequence[1])) # start executing the action for i in range(1, goal.order): # check that preempt has not been requested by the client if self.action_server.is_preempt_requested(): rospy.loginfo('%s: Preempted' % self.action_name) self.action_server.set_preempted() success = False break self.feedback.sequence.append(self.feedback.sequence[i] + self.feedback.sequence[i - 1]) # publish the feedback rospy.loginfo('publishing feedback ...') self.action_server.publish_feedback(self.feedback) # this step is not necessary, the sequence is computed at 1 Hz for demonstration purposes r.sleep() if success: self.result.sequence = self.feedback.sequence rospy.loginfo('%s: Succeeded' % self.action_name) self.action_server.set_succeeded(self.result)
class SynchronizedSimpleActionServer(object): def __init__(self, namespace, action_spec, execute_cb): self.goal_service = rospy.Service(namespace + '/get_goal_from_id', GetTaskFromID, self.task_id_cb) self.server = SimpleActionServer(namespace, action_spec, execute_cb, auto_start=False) self.server.start() def task_id_cb(self, request): idx = request.task_id current_goal = self.server.current_goal if idx == current_goal.goal_id.id: return GetTaskFromIDResponse(current_goal.get_goal()) return GetTaskFromIDResponse() def is_preempt_requested(self): return self.server.is_preempt_requested() def set_preempted(self): return self.server.set_preempted() def set_succeeded(self, result): return self.server.set_succeeded(result) def publish_feedback(self, feedback): return self.server.publish_feedback(feedback)
class MockGui(): def __init__(self, gui_validation_topic): self.reply = False self.preempted = 0 self.victimValid = True self.victimFoundx = 0 self.victimFoundy = 0 self.probability = 0 self.sensorIDsFound = [] self.gui_validate_victim_as_ = SimpleActionServer( gui_validation_topic, ValidateVictimGUIAction, execute_cb=self.gui_validate_victim_cb, auto_start=False) self.gui_validate_victim_as_.start() def __del__(self): self.gui_validate_victim_as_.__del__() def gui_validate_victim_cb(self, goal): rospy.loginfo('gui_validate_victim_cb') self.reply = False self.victimFoundx = goal.victimFoundx self.victimFoundy = goal.victimFoundy self.probability = goal.probability self.sensorIDsFound = goal.sensorIDsFound print goal while not self.reply: rospy.sleep(0.5) if self.gui_validate_victim_as_.is_preempt_requested(): preempted += 1 self.gui_validate_victim_as_.set_preempted() break else: self.preempted = 0 result = ValidateVictimGUIResult(victimValid=self.victimValid) self.gui_validate_victim_as_.set_succeeded(result)
class MockEndEffectorPlanner(): def __init__(self, end_effector_planner_topic): self.moves_end_effector = False self.move_end_effector_succeeded = False self.reply = False self.command = 0 self.point_of_interest = "" self.center_point = "" self.end_effector_planner_as_ = SimpleActionServer( end_effector_planner_topic, MoveEndEffectorAction, execute_cb=self.end_effector_planner_cb, auto_start=False) self.end_effector_planner_as_.start() def __del__(self): self.end_effector_planner_as_.__del__() def end_effector_planner_cb(self, goal): rospy.loginfo('end_effector_planner_cb') self.moves_end_effector = True self.command = goal.command self.point_of_interest = goal.point_of_interest self.center_point = goal.center_point while not self.reply: rospy.sleep(1.) if self.end_effector_planner_as_.is_preempt_requested(): self.end_effector_planner_as_.set_preempted(MoveEndEffectorResult()) self.moves_end_effector = False return None else: self.reply = False result = MoveEndEffectorResult() if self.move_end_effector_succeeded: self.moves_end_effector = False self.end_effector_planner_as_.set_succeeded(result) else: self.moves_end_effector = False self.end_effector_planner_as_.set_aborted(result)
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 fibonacci_server: def __init__(self, name): self.name = name self.feedback = FibonacciFeedback() self.result = FibonacciResult() self.action_server = SimpleActionServer( self.name, FibonacciAction, execute_cb=self.execute_callback, auto_start=False) self.action_server.start() def execute_callback(self, goal): rate = rospy.Rate(1) success = True self.feedback.sequence[:] = [] self.feedback.sequence.append(0) self.feedback.sequence.append(1) rospy.loginfo( '[{}] Executing, creating fibonacci sequence of order {} with seeds {}, {}' .format(self.name, goal.order, self.feedback.sequence[0], self.feedback.sequence[1])) for i in range(1, goal.order): if self.action_server.is_preempt_requested() or rospy.is_shutdown( ): rospy.loginfo('[{}] Prempeted'.format(self.name)) success = False self.action_server.set_preempted() break self.feedback.sequence.append(self.feedback.sequence[i - 1] + self.feedback.sequence[i]) self.action_server.publish_feedback(self.feedback) rate.sleep() if success: self.result.sequence = self.feedback.sequence rospy.loginfo('[{}] Succeeded'.format(self.name)) self.action_server.set_succeeded(self.result)
class TrajectoryExecution: def __init__(self, side_prefix): traj_controller_name = '/' + side_prefix + '_arm_controller/joint_trajectory_action' self.traj_action_client = SimpleActionClient(traj_controller_name, JointTrajectoryAction) rospy.loginfo( 'Waiting for a response from the trajectory action server arm: ' + side_prefix) self.traj_action_client.wait_for_server() rospy.loginfo('Trajectory executor is ready for arm: ' + side_prefix) motion_request_name = '/' + side_prefix + '_arm_motion_request/joint_trajectory_action' self.action_server = SimpleActionServer(motion_request_name, JointTrajectoryAction, execute_cb=self.move_to_joints) self.action_server.start() self.has_goal = False def move_to_joints(self, traj_goal): rospy.loginfo('Receieved a trajectory execution request.') traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1)) self.traj_action_client.send_goal(traj_goal) rospy.sleep(1) is_terminated = False while (not is_terminated): action_state = self.traj_action_client.get_state() if (action_state == GoalStatus.SUCCEEDED): self.action_server.set_succeeded() is_terminated = True elif (action_state == GoalStatus.ABORTED): self.action_server.set_aborted() is_terminated = True elif (action_state == GoalStatus.PREEMPTED): self.action_server.set_preempted() is_terminated = True rospy.loginfo('Trajectory completed.')
class PipolFollower(): def __init__(self): rospy.loginfo("Creating Pipol follower AS: '" + PIPOL_FOLLOWER_AS + "'") self._as = SimpleActionServer(PIPOL_FOLLOWER_AS, PipolFollowAction, execute_cb=self.execute_cb, preempt_callback=self.preempt_cb, auto_start=False) rospy.loginfo("Starting " + PIPOL_FOLLOWER_AS) self._as.start() def execute_cb(self, goal): print "goal is: " + str(goal) # helper variables success = True # start executing the action for i in xrange(1, goal.order): # check that preempt has not been requested by the client if self._as.is_preempt_requested(): rospy.loginfo('%s: Preempted' % self._action_name) self._as.set_preempted() success = False break self._feedback.sequence.append(self._feedback.sequence[i] + self._feedback.sequence[i - 1]) # publish the feedback self._as.publish_feedback(self._feedback) # this step is not necessary, the sequence is computed at 1 Hz for demonstration purposes r.sleep() if success: self._result.sequence = self._feedback.sequence rospy.loginfo('%s: Succeeded' % self._action_name) self._as.set_succeeded(self._result)
class SmartArmGripperActionServer(): def __init__(self): # Initialize constants self.JOINTS_COUNT = 2 # Number of joints to manage self.ERROR_THRESHOLD = 0.01 # Report success if error reaches below threshold self.TIMEOUT_THRESHOLD = rospy.Duration(5.0) # Report failure if action does not succeed within timeout threshold # Initialize new node rospy.init_node(NAME, anonymous=True) # Initialize publisher & subscriber for left finger self.left_finger_frame = 'arm_left_finger_link' self.left_finger = JointControllerState(set_point=0.0, process_value=0.0, error=1.0) self.left_finger_pub = rospy.Publisher('finger_left_controller/command', Float64) rospy.Subscriber('finger_left_controller/state', JointControllerState, self.read_left_finger) rospy.wait_for_message('finger_left_controller/state', JointControllerState) # Initialize publisher & subscriber for right finger self.right_finger_frame = 'arm_right_finger_link' self.right_finger = JointControllerState(set_point=0.0, process_value=0.0, error=1.0) self.right_finger_pub = rospy.Publisher('finger_right_controller/command', Float64) rospy.Subscriber('finger_right_controller/state', JointControllerState, self.read_right_finger) rospy.wait_for_message('finger_right_controller/state', JointControllerState) # Initialize action server self.result = SmartArmGripperResult() self.feedback = SmartArmGripperFeedback() self.feedback.gripper_position = [self.left_finger.process_value, self.right_finger.process_value] self.server = SimpleActionServer(NAME, SmartArmGripperAction, self.execute_callback) # Reset gripper position rospy.sleep(1) self.reset_gripper_position() rospy.loginfo("%s: Ready to accept goals", NAME) def reset_gripper_position(self): self.left_finger_pub.publish(0.0) self.right_finger_pub.publish(0.0) rospy.sleep(5) def read_left_finger(self, data): self.left_finger = data self.has_latest_left_finger = True def read_right_finger(self, data): self.right_finger = data self.has_latest_right_finger = True def wait_for_latest_controller_states(self, timeout): self.has_latest_left_finger = False self.has_latest_right_finger = False r = rospy.Rate(100) start = rospy.Time.now() while (self.has_latest_left_finger == False or self.has_latest_right_finger == False) and \ (rospy.Time.now() - start < timeout): r.sleep() def execute_callback(self, goal): r = rospy.Rate(100) self.result.success = True self.result.gripper_position = [self.left_finger.process_value, self.right_finger.process_value] rospy.loginfo("%s: Executing move gripper", NAME) # Initialize target joints target_joints = list() for i in range(self.JOINTS_COUNT): target_joints.append(0.0) # Retrieve target joints from goal if (len(goal.target_joints) > 0): for i in range(min(len(goal.target_joints), len(target_joints))): target_joints[i] = goal.target_joints[i] else: rospy.loginfo("%s: Aborted: Invalid Goal", NAME) self.result.success = False self.server.set_aborted() return # Publish goal to controllers self.left_finger_pub.publish(target_joints[0]) self.right_finger_pub.publish(target_joints[1]) # Initialize loop variables start_time = rospy.Time.now() while (math.fabs(target_joints[0] - self.left_finger.process_value) > self.ERROR_THRESHOLD or \ math.fabs(target_joints[1] - self.right_finger.process_value) > self.ERROR_THRESHOLD): # Cancel exe if another goal was received (i.e. preempt requested) if self.server.is_preempt_requested(): rospy.loginfo("%s: Aborted: Action Preempted", NAME) self.result.success = False self.server.set_preempted() break # Publish current gripper position as feedback self.feedback.gripper_position = [self.left_finger.process_value, self.right_finger.process_value] self.server.publish_feedback(self.feedback) # Abort if timeout current_time = rospy.Time.now() if (current_time - start_time > self.TIMEOUT_THRESHOLD): rospy.loginfo("%s: Aborted: Action Timeout", NAME) self.result.success = False self.server.set_aborted() break r.sleep() if (self.result.success): rospy.loginfo("%s: Goal Completed", NAME) self.wait_for_latest_controller_states(rospy.Duration(2.0)) self.result.gripper_position = [self.left_finger.process_value, self.right_finger.process_value] self.server.set_succeeded(self.result)
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 LocalSearch(): VISUAL_FIELD_SIZE = 40 MIN_HEAD_ANGLE = -140 MAX_HEAD_ANGLE = 140 _feedback = LocalSearchFeedback() _result = LocalSearchResult() def __init__(self): self._action_name = 'local_search' self.found_marker = False self.tracking_started = False #initialize head mover name_space = '/head_traj_controller/point_head_action' self.head_client = SimpleActionClient(name_space, PointHeadAction) self.head_client.wait_for_server() rospy.loginfo('%s: Action client for PointHeadAction running' % self._action_name) #initialize tracker mark rospy.Subscriber('catch_me_destination_publisher', AlvarMarker, self.marker_tracker) rospy.loginfo('%s: subscribed to AlvarMarkers' % self._action_name) #initialize this client self._as = SimpleActionServer(self._action_name, LocalSearchAction, execute_cb=self.run, auto_start=False) self._as.start() rospy.loginfo('%s: started' % self._action_name) def marker_tracker(self, marker): if self.tracking_started: self.found_marker = True rospy.loginfo('%s: marker found' % self._action_name) def run(self, goal): success = False self.tracking_started = True self.found_marker = False rospy.loginfo('%s: Executing search for AR marker' % self._action_name) # define a set of ranges to search search_ranges = [ # first search in front of the robot (0, self.VISUAL_FIELD_SIZE), (self.VISUAL_FIELD_SIZE, -self.VISUAL_FIELD_SIZE), # then search all directions (-self.VISUAL_FIELD_SIZE, self.MAX_HEAD_ANGLE), (self.MAX_HEAD_ANGLE, self.MIN_HEAD_ANGLE), (self.MIN_HEAD_ANGLE, 0) ] range_index = 0 #success = self.search_range(*(search_ranges[range_index])) while (not success) and range_index < len(search_ranges) - 1: if self._as.is_preempt_requested(): rospy.loginfo('%s: Premepted' % self._action_name) self._as.set_preempted() break range_index = range_index + 1 success = self.search_range(*(search_ranges[range_index])) if success: rospy.loginfo('%s: Succeeded' % self._action_name) self._as.set_succeeded() else: self._as.set_aborted() self.tracking_started = False def search_range(self, start_angle, end_angle): rospy.loginfo("{}: searching range {} {}".format(self._action_name, start_angle, end_angle)) angle_tick = self.VISUAL_FIELD_SIZE if (start_angle < end_angle) else -self.VISUAL_FIELD_SIZE for cur_angle in xrange(start_angle, end_angle, angle_tick): if self._as.is_preempt_requested(): return False head_goal = self.lookat_goal(cur_angle) rospy.loginfo('%s: Head move goal for %s: %s produced' % (self._action_name, str(cur_angle), str(head_goal))) self.head_client.send_goal(head_goal) self.head_client.wait_for_result(rospy.Duration.from_sec(5.0)) if (self.head_client.get_state() != GoalStatus.SUCCEEDED): rospy.logwarn('Head could not move to specified location') break # pause at each tick rospy.sleep(0.3) if (self.found_marker): # found a marker! return True # no marker found return False def lookat_goal(self, angle): head_goal = PointHeadGoal() head_goal.target.header.frame_id = '/torso_lift_link' head_goal.max_velocity = 0.8 angle_in_radians = math.radians(angle) x = math.cos(angle_in_radians) * 5 y = math.sin(angle_in_radians) * 5 z = -0.5 head_goal.target.point = Point(x, y, z) return head_goal
class Approach(object): def __init__(self, name, takeoff_height): self.robot_name = name self.takeoff_height = takeoff_height # Mutual exclusion self.sonar_me = Condition() self.odometry_me = Condition() self.current_height = None # Create trajectory server self.trajectory_server = SimpleActionServer( 'approach_server', ExecuteDroneApproachAction, self.goCallback, False) self.server_feedback = ExecuteDroneApproachFeedback() self.server_result = ExecuteDroneApproachResult() # Get client from hector_quadrotor_actions self.move_client = SimpleActionClient("/{}/action/pose".format(name), PoseAction) self.move_client.wait_for_server() # Subscribe to ground_truth to monitor the current pose of the robot rospy.Subscriber("/{}/ground_truth/state".format(name), Odometry, self.poseCallback) # Subscribe to topic to receive the planned trajectory rospy.Subscriber("/{}/move_group/display_planned_path".format(name), DisplayTrajectory, self.planCallback) #Auxiliary variables self.trajectory = [] # Array with the trajectory to be executed self.trajectory_received = False # Flag to signal trajectory received self.odom_received = False # Flag to signal odom received self.robot = RobotCommander( robot_description="{}/robot_description".format(name), ns="/{}".format(name)) self.display_trajectory_publisher = rospy.Publisher( '/{}/move_group/display_planned_path'.format(name), DisplayTrajectory, queue_size=20) # Variables for collision callback self.validity_srv = rospy.ServiceProxy( '/{}/check_state_validity'.format(name), GetStateValidity) self.validity_srv.wait_for_service() self.collision = False # Subscribe to sonar_height rospy.Subscriber("sonar_height", Range, self.sonar_callback, queue_size=10) #Start move_group self.move_group = MoveGroup('earth', name) self.move_group.set_planner(planner_id='RRTConnectkConfigDefault', attempts=10, allowed_time=2) #RRTConnectkConfigDefault self.move_group.set_workspace([XMIN, YMIN, ZMIN, XMAX, YMAX, ZMAX]) # Set the workspace size # Get current robot position to define as start planning point self.current_pose = self.robot.get_current_state() #Start planningScenePublisher self.scene_pub = PlanningScenePublisher(name, self.current_pose) # Start trajectory server self.trajectory_server.start() def sonar_callback(self, msg): ''' Function to update drone height ''' self.sonar_me.acquire() self.current_height = msg.range self.sonar_me.notify() self.sonar_me.release() def poseCallback(self, odometry): ''' Monitor the current position of the robot ''' self.odometry_me.acquire() self.odometry = odometry.pose.pose # print(self.odometry) self.odometry_me.release() self.odom_received = True def goCallback(self, pose): ''' Require a plan to go to the desired target and try to execute it 5 time or return erro ''' self.target = pose.goal #################################################################### # First takeoff if the drone is close to ground self.sonar_me.acquire() while not (self.current_height and self.odometry): self.sonar_me.wait() if self.current_height < self.takeoff_height: self.odometry_me.acquire() takeoff_pose = PoseGoal() takeoff_pose.target_pose.header.frame_id = "{}/world".format( self.robot_name) takeoff_pose.target_pose.pose.position.x = self.odometry.position.x takeoff_pose.target_pose.pose.position.y = self.odometry.position.y takeoff_pose.target_pose.pose.position.z = self.odometry.position.z + self.takeoff_height - self.current_height #Add the heght error to the height takeoff_pose.target_pose.pose.orientation.x = self.odometry.orientation.x takeoff_pose.target_pose.pose.orientation.y = self.odometry.orientation.y takeoff_pose.target_pose.pose.orientation.z = self.odometry.orientation.z takeoff_pose.target_pose.pose.orientation.w = self.odometry.orientation.w self.odometry_me.release() self.move_client.send_goal(takeoff_pose) self.move_client.wait_for_result() result = self.move_client.get_state() if result == GoalStatus.ABORTED: rospy.logerr("Abort approach! Unable to execute takeoff!") self.trajectory_server.set_aborted() return self.sonar_me.release() #################################################################### rospy.loginfo("Try to start from [{},{},{}]".format( self.odometry.position.x, self.odometry.position.y, self.odometry.position.z)) rospy.loginfo("Try to go to [{},{},{}]".format(self.target.position.x, self.target.position.y, self.target.position.z)) self.trials = 0 while self.trials < 5: rospy.logwarn("Attempt {}".format(self.trials + 1)) if (self.trials > 1): self.target.position.z += 2 * rd() - 1 result = self.go(self.target) if (result == 'replan') or (result == 'no_plan'): self.trials += 1 else: self.trials = 10 self.collision = False if result == 'ok': self.trajectory_server.set_succeeded() elif (result == 'preempted'): self.trajectory_server.set_preempted() else: self.trajectory_server.set_aborted() def go(self, target_): ''' Function to plan and execute the trajectory one time ''' # Insert goal position on an array target = [] target.append(target_.position.x) target.append(target_.position.y) target.append(target_.position.z) target.append(target_.orientation.x) target.append(target_.orientation.y) target.append(target_.orientation.z) target.append(target_.orientation.w) #Define target for move_group # self.move_group.set_joint_value_target(target) self.move_group.set_target(target) self.odometry_me.acquire() self.current_pose.multi_dof_joint_state.transforms[ 0].translation.x = self.odometry.position.x self.current_pose.multi_dof_joint_state.transforms[ 0].translation.y = self.odometry.position.y self.current_pose.multi_dof_joint_state.transforms[ 0].translation.z = self.odometry.position.z self.current_pose.multi_dof_joint_state.transforms[ 0].rotation.x = self.odometry.orientation.x self.current_pose.multi_dof_joint_state.transforms[ 0].rotation.x = self.odometry.orientation.y self.current_pose.multi_dof_joint_state.transforms[ 0].rotation.x = self.odometry.orientation.z self.current_pose.multi_dof_joint_state.transforms[ 0].rotation.x = self.odometry.orientation.w self.odometry_me.release() #Set start state self.move_group.set_start_state(self.current_pose) # Plan a trajectory till the desired target plan = self.move_group.plan() if plan.planned_trajectory.multi_dof_joint_trajectory.points: # Execute only if has points on the trajectory while (not self.trajectory_received): rospy.loginfo("Waiting for trajectory!") rospy.sleep(0.2) # rospy.loginfo("TRAJECTORY: {}".format(self.trajectory)) #Execute trajectory with action_pose last_pose = self.trajectory[0] for pose in self.trajectory: # Verify preempt call if self.trajectory_server.is_preempt_requested(): self.move_client.send_goal(last_pose) self.move_client.wait_for_result() self.scene_pub.publishScene() self.trajectory_received = False self.odom_received = False return 'preempted' #Send next pose to move self.next_pose = pose.target_pose.pose self.move_client.send_goal(pose, feedback_cb=self.collisionCallback) self.move_client.wait_for_result() result = self.move_client.get_state() self.scene_pub.publishScene() # Abort if the drone can not reach the position if result == GoalStatus.ABORTED: self.move_client.send_goal( last_pose) #Go back to the last pose self.move_client.wait_for_result() self.trajectory_received = False self.odom_received = False return 'aborted' elif result == GoalStatus.PREEMPTED: # last_pose.target_pose.pose.position.z += rd() - 0.5 self.move_client.send_goal( last_pose) #Go back to the last pose self.move_client.wait_for_result() self.trajectory_received = False self.odom_received = False return 'replan' elif result == GoalStatus.SUCCEEDED: self.trials = 0 last_pose = pose self.server_feedback.current_pose = self.odometry self.trajectory_server.publish_feedback(self.server_feedback) # Reset control variables self.trajectory_received = False self.odom_received = False rospy.loginfo("Trajectory is traversed!") return 'ok' else: rospy.logerr("Trajectory is empty. Planning was unsuccessful.") return 'no_plan' def planCallback(self, msg): ''' Receive planned trajectories and insert it into an array of waypoints ''' if (not self.odom_received): return # Variable to calculate the distance difference between 2 consecutive points last_pose = PoseGoal() last_pose.target_pose.pose.position.x = self.odometry.position.x last_pose.target_pose.pose.position.y = self.odometry.position.y last_pose.target_pose.pose.position.z = self.odometry.position.z last_pose.target_pose.pose.orientation.x = self.odometry.orientation.x last_pose.target_pose.pose.orientation.y = self.odometry.orientation.y last_pose.target_pose.pose.orientation.z = self.odometry.orientation.z last_pose.target_pose.pose.orientation.w = self.odometry.orientation.w self.trajectory = [] last_motion_theta = 0 for t in msg.trajectory: for point in t.multi_dof_joint_trajectory.points: waypoint = PoseGoal() waypoint.target_pose.header.frame_id = "{}/world".format( self.robot_name) waypoint.target_pose.pose.position.x = point.transforms[ 0].translation.x waypoint.target_pose.pose.position.y = point.transforms[ 0].translation.y waypoint.target_pose.pose.position.z = point.transforms[ 0].translation.z # Orientate the robot always to the motion direction delta_x = point.transforms[ 0].translation.x - last_pose.target_pose.pose.position.x delta_y = point.transforms[ 0].translation.y - last_pose.target_pose.pose.position.y motion_theta = atan2(delta_y, delta_x) last_motion_theta = motion_theta # Make the robot orientation fit with the motion orientation if the movemente on xy is bigger than RESOLUTION # if (abs(delta_x) > RESOLUTION) or (abs(delta_y) > RESOLUTION): q = quaternion_from_euler(0, 0, motion_theta) waypoint.target_pose.pose.orientation.x = q[0] waypoint.target_pose.pose.orientation.y = q[1] waypoint.target_pose.pose.orientation.z = q[2] waypoint.target_pose.pose.orientation.w = q[3] # else: # waypoint.target_pose.pose.orientation.x = point.transforms[0].rotation.x # waypoint.target_pose.pose.orientation.y = point.transforms[0].rotation.y # waypoint.target_pose.pose.orientation.z = point.transforms[0].rotation.z # waypoint.target_pose.pose.orientation.w = point.transforms[0].rotation.w # Add a rotation inplace if next position has an angle difference bigger than 45 if abs(motion_theta - last_motion_theta) > 0.785: last_pose.target_pose.pose.orientation = waypoint.target_pose.pose.orientation self.trajectory.append(last_pose) last_pose = copy.copy( waypoint) # Save pose to calc the naxt delta last_motion_theta = motion_theta self.trajectory.append(waypoint) #Insert a last point to ensure that the robot end at the right position waypoint = PoseGoal() waypoint.target_pose.header.frame_id = "{}/world".format( self.robot_name) waypoint.target_pose.pose.position.x = point.transforms[ 0].translation.x waypoint.target_pose.pose.position.y = point.transforms[ 0].translation.y waypoint.target_pose.pose.position.z = point.transforms[ 0].translation.z waypoint.target_pose.pose.orientation.x = point.transforms[ 0].rotation.x waypoint.target_pose.pose.orientation.y = point.transforms[ 0].rotation.y waypoint.target_pose.pose.orientation.z = point.transforms[ 0].rotation.z waypoint.target_pose.pose.orientation.w = point.transforms[ 0].rotation.w self.trajectory.append(waypoint) self.trajectory_received = True def collisionCallback(self, feedback): ''' This callback runs on every feedback message received ''' validity_msg = GetStateValidityRequest( ) # Build message to verify collision validity_msg.group_name = PLANNING_GROUP if self.next_pose and (not self.collision): self.odometry_me.acquire() x = self.odometry.position.x y = self.odometry.position.y z = self.odometry.position.z # Distance between the robot and the next position dist = sqrt((self.next_pose.position.x - x)**2 + (self.next_pose.position.y - y)**2 + (self.next_pose.position.z - z)**2) # Pose to verify collision pose = Transform() pose.rotation.x = self.odometry.orientation.x pose.rotation.y = self.odometry.orientation.y pose.rotation.z = self.odometry.orientation.z pose.rotation.w = self.odometry.orientation.w self.odometry_me.release() #Verify possible collisions on diferent points between the robot and the goal point # rospy.logerr("\n\n\nCOLLISION CALLBACK: ") # rospy.logerr(dist) for d in arange(RESOLUTION, dist + 0.5, RESOLUTION): pose.translation.x = (self.next_pose.position.x - x) * (d / dist) + x pose.translation.y = (self.next_pose.position.y - y) * (d / dist) + y pose.translation.z = (self.next_pose.position.z - z) * (d / dist) + z self.current_pose.multi_dof_joint_state.transforms[ 0] = pose # Insert the correct odometry value validity_msg.robot_state = self.current_pose # Call service to verify collision collision_res = self.validity_srv.call(validity_msg) # print("\nCollision response:") # print(collision_res) # Check if robot is in collision if not collision_res.valid: # print(validity_msg) rospy.logerr('Collision in front [x:{} y:{} z:{}]'.format( pose.translation.x, pose.translation.y, pose.translation.z)) rospy.logerr('Current pose [x:{} y:{} z:{}]'.format( x, y, z)) # print(collision_res) self.move_client.cancel_goal() self.collision = True return
class HokuyoLaserActionServer(): def __init__(self): # Initialize constants self.error_threshold = 0.0175 # Report success if error reaches below threshold self.signal = 1 # Initialize new node rospy.init_node(NAME, anonymous=True) controller_name = rospy.get_param('~controller') # Initialize publisher & subscriber for tilt self.laser_tilt = JointControllerState() self.laser_tilt_pub = rospy.Publisher(controller_name + '/command', Float64) self.laser_signal_pub = rospy.Publisher('laser_scanner_signal', LaserScannerSignal) self.joint_speed_srv = rospy.ServiceProxy(controller_name + '/set_speed', SetSpeed, persistent=True) rospy.Subscriber(controller_name + '/state', JointControllerState, self.process_tilt_state) rospy.wait_for_message(controller_name + '/state', JointControllerState) # Initialize tilt action server self.result = HokuyoLaserTiltResult() self.feedback = HokuyoLaserTiltFeedback() self.feedback.tilt_position = self.laser_tilt.process_value self.server = SimpleActionServer('hokuyo_laser_tilt_action', HokuyoLaserTiltAction, self.execute_callback) rospy.loginfo("%s: Ready to accept goals", NAME) def process_tilt_state(self, tilt_data): self.laser_tilt = tilt_data def reset_tilt_position(self, offset=0.0): self.laser_tilt_pub.publish(offset) rospy.sleep(0.5) def execute_callback(self, goal): r = rospy.Rate(100) self.joint_speed_srv(2.0) self.reset_tilt_position(goal.offset) delta = goal.amplitude - goal.offset target_speed = delta / goal.duration timeout_threshold = rospy.Duration(5.0) + rospy.Duration.from_sec(goal.duration) self.joint_speed_srv(target_speed) print "delta = %f, target_speed = %f" % (delta, target_speed) self.result.success = True self.result.tilt_position = self.laser_tilt.process_value rospy.loginfo("%s: Executing laser tilt %s time(s)", NAME, goal.tilt_cycles) # Tilt laser goal.tilt_cycles amount of times. for i in range(1, goal.tilt_cycles + 1): self.feedback.progress = i # Issue 2 commands for each cycle for j in range(2): if j % 2 == 0: target_tilt = goal.offset + goal.amplitude # Upper tilt limit self.signal = 0 else: target_tilt = goal.offset # Lower tilt limit self.signal = 1 # Publish target command to controller self.laser_tilt_pub.publish(target_tilt) start_time = rospy.Time.now() current_time = start_time while abs(target_tilt - self.laser_tilt.process_value) > self.error_threshold: #delta = abs(target_tilt - self.laser_tilt.process_value) #time_left = goal.duration - (rospy.Time.now() - start_time).to_sec() #target_speed = delta / time_left #self.joint_speed_srv(target_speed) # Cancel exe if another goal was received (i.e. preempt requested) if self.server.is_preempt_requested(): rospy.loginfo("%s: Aborted: Action Preempted", NAME) self.result.success = False self.server.set_preempted() return # Publish current head position as feedback self.feedback.tilt_position = self.laser_tilt.process_value self.server.publish_feedback(self.feedback) # Abort if timeout current_time = rospy.Time.now() if (current_time - start_time > timeout_threshold): rospy.loginfo("%s: Aborted: Action Timeout", NAME) self.result.success = False self.server.set_aborted() return r.sleep() signal = LaserScannerSignal() signal.header.stamp = current_time signal.signal = self.signal self.laser_signal_pub.publish(signal) #rospy.sleep(0.5) if self.result.success: rospy.loginfo("%s: Goal Completed", NAME) self.result.tilt_position = self.laser_tilt.process_value self.server.set_succeeded(self.result)
class BakerArmServer(script): DOF = 5 DEFAULT_POSITION = [0.6, 1., -2., 1., 0.] TRANSPORT_POSITION = [0.9, 1., -2., 1., 0.] IS_GRIPPER_AVAILABLE = False def __init__(self, name, status=ArmStatus.NO_TRASHCAN): self.verbose_ = True self.confirm_ = False self.name_ = name self.status_ = status self.joint_values_ = [0.0] * self.DOF self.mutex_ = Lock() (self.trashcan_pose_, self.trolley_pose_) = (None, None) self.displayParameters() self.initClients() self.initServices() self.initServers() self.initSubscribers() self.initTopics() def confirm(self): if not self.confirm_: return raw_input("Please press enter to confirm") def displayParameters(self): if not self.verbose_: return print("========== baker_arm_server Parameters ==========") print("todo") @log def Initialize(self): self.sss.init('gripper') self.sss.init('arm') @log def initClients(self): # Warning: wait_for_server and wait_for_service are not the same function. # * rospy.duration expected for wait_for_server # * float expected for wait_for_service self.plan_client_ = SimpleActionClient('/arm_planning_node/PlanTo', PlanToAction) self.plan_client_.wait_for_server(timeout=rospy.Duration(5.0)) self.execution_client_ = SimpleActionClient('/traj_exec', ExecuteTrajectoryAction) self.execution_client_.wait_for_server(timeout=rospy.Duration(5.0)) self.small_gripper_finger_client_ = rospy.ServiceProxy( "/gripper/driver/set_object", SetObject) # rmb-ma. keep commented # self.small_gripper_finger_client_.wait_for_service(timeout=5.0) self.add_collision_object_client_ = rospy.ServiceProxy( "/ipa_planning_scene_creator/add_collision_objects", AddCollisionObject) self.add_collision_object_client_.wait_for_service(timeout=5.0) self.attach_object_client_ = rospy.ServiceProxy( "ipa_planning_scene_creator/attach_object", AddCollisionObject) self.attach_object_client_.wait_for_service(timeout=5.0) self.detach_object_client_ = rospy.ServiceProxy( "ipa_planning_scene_creator/detach_object", RemoveCollisionObject) self.detach_object_client_.wait_for_service(timeout=5.0) @log def initServers(self): self.to_joints_position_server_ = SimpleActionServer( self.name_ + '/set_joints_values', ExecuteTrajectoryAction, self.moveToJointsPositionCallback, False) self.to_joints_position_server_.start() self.to_transport_position_server_ = SimpleActionServer( self.name_ + '/transport_position', MoveToAction, self.moveToTransportPositionCallback, False) self.to_transport_position_server_.start() self.to_rest_position_server_ = SimpleActionServer( self.name_ + '/rest_position', MoveToAction, self.moveToRestPositionCallback, False) self.to_rest_position_server_.start() self.catch_trashcan_server_ = SimpleActionServer( self.name_ + '/catch_trashcan', MoveToAction, self.catchTrashcanCallback, False) self.catch_trashcan_server_.start() self.leave_trashcan_server_ = SimpleActionServer( self.name_ + '/leave_trashcan', MoveToAction, self.leaveTrashcanCallback, False) self.leave_trashcan_server_.start() self.empty_trashcan_server_ = SimpleActionServer( self.name_ + '/empty_trashcan', MoveToAction, self.emptyTrashcanCallback, False) self.empty_trashcan_server_.start() @log def initServices(self): pass @log def initTopics(self): Thread(target=self.statusTalker).start() @log def statusTalker(self): publisher = rospy.Publisher(self.name_ + '/status', Int32, queue_size=10) rate = rospy.Rate(5) while not rospy.is_shutdown(): publisher.publish(self.status_.value) rate.sleep() @log def initSubscribers(self): rospy.Subscriber("/arm/joint_states", JointState, self.jointStateCallback) def jointStateCallback(self, msg): ''' getting current position of the joint @param msg containts joint position, velocity, effort, names ''' for i in range(0, len(msg.position)): self.joint_values_[i] = msg.position[i] def gripperHandler(self, finger_value=0.01, finger_open=False, finger_close=True): ''' Gripper handler function use to control gripper open/close command, move parallel part of gripper with give values (in cm) @param finger_value is the big parallel part move realtive to zero position @finger_open open the small finger @finger_close close the small finger ''' if finger_open or finger_close: # set_object request, both the finger move together (at the same time) request = SetObjectRequest() response = SetObjectResponse() request.node = 'gripper_finger1_joint' request.object = '22A0' if finger_open: request.value = '5' elif finger_close: request.value = '10' request.cached = False response = self.small_gripper_finger_client_.call(request) if response.success: rospy.loginfo(response.message) else: rospy.logerr(response.message) rospy.logerr("SetObject service call Failed!!") # default blocking is true, wait until if finishes it tasks self.sss.move('gripper', [[finger_value]]) return response.success @log def handleCheckAccessibility(self, request): response = TriggerResponse() response.success = True return response @log def openGripper(self): if not self.IS_GRIPPER_AVAILABLE: return self.gripperHandler(finger_value=0.01, finger_open=True, finger_close=False) @log def closeGripper(self): if not self.IS_GRIPPER_AVAILABLE: return self.gripperHandler(finger_value=-0.01, finger_open=False, finger_close=True) @log def executeTrajectory(self, trajectory, server): goal = ExecuteTrajectoryGoal() goal.trajectory = trajectory self.execution_client_.send_goal(goal) while self.execution_client_.get_state() < 3: rospy.sleep(0.1) if not server.is_preempt_requested() and not rospy.is_shutdown(): continue self.execution_client_.cancel_goal() self.execution_client_.wait_for_result() return True result = self.execution_client_.get_result() state = self.execution_client_.get_state() if result.success and state == 3: rospy.logwarn("Execution finish with given time") return True else: rospy.logerr("Execution aborted!!") return False @log def isPoseAccessible(self, target_pose): (_, is_planned) = planTrajectoryInCartSpace(client=self.plan_client_, object_pose=target_pose, bdmp_goal=None, bdmp_action='') return is_planned @log def planAndExecuteTrajectoryInJointSpaces(self, target_joints, server): (trajectory, is_planned) = planTrajectoryInJointSpace(client=self.plan_client_, object_pose=target_joints, bdmp_goal=None, bdmp_action='') if not is_planned: rospy.logerr('Trajectory in joint spaces execution failed') raise Exception('Trajectory in joint spaces execution failed') self.confirm() is_execution_successful = self.executeTrajectory(trajectory=trajectory, server=server) if not is_execution_successful: rospy.logerr('Can not find valid trajectory at joint space') raise Exception('Trajectory Execution failed') return not server.is_preempt_requested() @log def planAndExecuteTrajectoryInCartesianSpace(self, target_pose, server): (trajectory, is_planned) = planTrajectoryInCartSpace(client=self.plan_client_, object_pose=target_pose, bdmp_goal=None, bdmp_action='') if not is_planned: rospy.logerr('Cannot find valid trajectory at cartesianSpace') raise Exception('Cannot find valid trajectory at cartesian space') self.confirm() is_execution_successful = self.executeTrajectory(trajectory=trajectory, server=server) if not is_execution_successful: rospy.logerr("Trajectory Execution Failed") raise Exception('Trajectory Execution Failed') return not server.is_preempt_requested() @staticmethod def poseToLists(pose): position = [pose.position.x, pose.position.y, pose.position.z] orientation = [ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w ] return (position, orientation) @log def catchTrashcanCallback(self, goal): rospy.sleep(3) result = MoveToResult() result.arrived = False target_pose = goal.target_pos target_pose.pose.position.z *= 1.1 #The arm canot carry a new trashcan as it already carries one if self.status_ != ArmStatus.NO_TRASHCAN: self.catch_trashcan_server_.set_aborted(result) try: self.planAndExecuteTrajectoryInCartesianSpace( target_pose, self.catch_trashcan_server_) except Exception as e: print(e) self.catch_trashcan_server_.set_aborted(result) return if self.catch_trashcan_server_.is_preempt_requested(): result.arrived = True self.catch_trashcan_server_.set_preempted(result) return self.closeGripper() #self.fixTrashCanToRobot() self.status_ = ArmStatus.FULL_TRASHCAN # todo rmb-ma setup the grapping move with a real trashcan # Help: # try: # target_pose = make_pose(position=[0.00,-0.00,-0.07], orientation=[0.0,0.0,0.0,1.0], frame_id='gripper') # self.planAndExecuteTrajectoryInCartesianSpace(target_pose, self.catch_trashcan_server_) # except: # self.catch_trashcan_server_.set_aborted(result) # return # # if self.catch_trashcan_server_.is_preempt_requested(): # result.arrived = True # self.catch_trashcan_server_.set_preempted(result) # return # # try: # target_pose = make_pose(position=[0.020,-0.00,0.00], orientation=[0.0,0.0,0.0,1.0], frame_id='gripper') # self.planAndExecuteTrajectoryInCartesianSpace(target_pose, self.catch_trashcan_server_) # except: # self.catch_trashcan_server_.set_aborted(result) # return # # if self.catch_trashcan_server_.is_preempt_requested(): # result.arrived = True # self.catch_trashcan_server_.set_preempted(result) # return # # try: # target_pose = make_pose(position=[0.000,-0.00,0.030], orientation=[0.0,0.0,0.0,1.0], frame_id='gripper') # self.planAndExecuteTrajectoryInCartesianSpace(target_pose, self.catch_trashcan_server_) # except: # self.catch_trashcan_server_.set_aborted(result) # return # result.arrived = True self.catch_trashcan_server_.set_succeeded(result) @log def emptyTrashcanCallback(self, goal): result = MoveToResult() #Trashcan emptying while the arm doesnt carry any trashcan if self.status_ != ArmStatus.FULL_TRASHCAN: result.arrived = False self.empty_trashcan_server_.set_aborted(result) return try: self.planAndExecuteTrajectoryInCartesianSpace( goal.target_pos, self.empty_trashcan_server_) except: result.arrived = False self.empty_trashcan_server_.set_aborted(result) return if self.empty_trashcan_server_.is_preempt_requested(): self.empty_trashcan_server_.set_preempted() return try: target_joints = self.joint_values_[0:-1] + [3.] self.planAndExecuteTrajectoryInJointSpaces( target_joints, self.empty_trashcan_server_) except: result.arrived = False self.empty_trashcan_server_.set_aborted(result) return if self.empty_trashcan_server_.is_preempt_requested(): self.empty_trashcan_server_.set_preempted() return rospy.sleep(5) self.status_ = ArmStatus.FULL_TRASHCAN target_joints = self.joint_values_[0:-1] + [0] self.planAndExecuteTrajectoryInJointSpaces(target_joints, self.empty_trashcan_server_) result.arrived = True self.empty_trashcan_server_.set_succeeded(result) @log def moveToRestPositionCallback(self, goal): result = MoveToResult() # Rest position is unavailable if the arm carries a trashcan if self.status_ != ArmStatus.NO_TRASHCAN: result.arrived = False self.to_rest_position_server_.set_aborted(result) return try: self.planAndExecuteTrajectoryInJointSpaces( self.DEFAULT_POSITION, self.to_rest_position_server_) except Exception as e: rospy.logerr(e) result.arrived = False self.to_rest_position_server_.set_aborted(result) return if self.to_rest_position_server_.is_preempt_requested(): self.to_rest_position_server_.set_preempted() return self.to_rest_position_server_.set_succeeded() @log def moveToTransportPositionCallback(self, goal): result = MoveToResult() # Transport position is unavailable if the arm doesnt carry a trashcan if self.status_ == ArmStatus.NO_TRASHCAN: result.arrived = False self.to_transport_position_server_.set_aborted(result) return try: result.arrived = self.planAndExecuteTrajectoryInJointSpaces( self.TRANSPORT_POSITION, self.to_transport_position_server_) except: result.arrived = False self.to_transport_position_server_.set_aborted(result) return if self.to_transport_position_server_.is_preempt_requested(): self.to_transport_position_server_.set_preempted() return self.to_transport_position_server_.set_succeeded(result) @log def leaveTrashcanCallback(self, goal): result = MoveToResult() # The arm cannot leave a trashcan if it doesnt carry one if self.status_ != ArmStatus.EMPTY_TRASHCAN: result.arrived = False self.leave_trashcan_server_.set_aborted(result) return self.planAndExecuteTrajectoryInCartesianSpace( goal.target_pos, self.leave_trashcan_server_) if self.leave_trashcan_server_.is_preempt_requested(): self.leave_trashcan_server_.set_preempted() return self.openGripper() self.status_ = ArmStatus.NO_TRASHCAN if self.leave_trashcan_server_.is_preempt_requested(): self.leave_trashcan_server_.set_preempted() return target_pose = make_pose(position=[-0.07, -0., -0.04], orientation=[0., 0., 0., 1.], frame_id='gripper') self.planAndExecuteTrajectoryInCartesianSpace( target_pose, self.leave_trashcan_server_) if self.leave_trashcan_server_.is_preempt_requested(): self.leave_trashcan_server_.set_preempted() return target_pose = make_pose(position=[-0.0, -0., 0.05], orientation=[0., 0., 0., 1.], frame_id='gripper') self.planAndExecuteTrajectoryInCartesianSpace( target_pose, self.leave_trashcan_server_) if self.leave_trashcan_server_.is_preempt_requested(): self.leave_trashcan_server_.set_preempted() return result.arrived = True self.leave_trashcan_server_.set_succeeded(result) @log def moveToJointsPositionCallback(self, goal): target_joints = goal.trajectory.joint_trajectory.points[0].positions self.planAndExecuteTrajectoryInJointSpaces( target_joints, self.to_joints_position_server_) if self.to_joints_position_server_.is_preempt_requested(): self.to_joints_position_server_.set_preempted() return self.to_joints_position_server_.set_succeeded()
class NavToNode(object): def __init__(self): # action client self._ac_gp = SimpleActionClient("global_planner_node_action", GlobalPlannerAction) self._ac_gp.wait_for_server() self._ac_lp = SimpleActionClient("local_planner_node_action", LocalPlannerAction) self._ac_lp.wait_for_server() # action servergoal self._as = SimpleActionServer("nav_to_node_action", NavToAction, self.ExecuteCB, auto_start=False) # define goals to send self.global_planner_goal_ = GlobalPlannerGoal() self.local_planner_goal_ = LocalPlannerGoal() self.new_goal_ = False self.new_path_ = False self.global_planner_error_code = -1 self.local_planner_error_code = -1 # new thread running the execution loop self.thread_ = Thread(target=self.Loop) self.thread_.start() self._as.start() def ExecuteCB(self, goal): rospy.loginfo('NAVTO: Goal received') self.global_planner_goal_.goal = goal.navgoal self.global_planner_error_code = -1 self.local_planner_error_code = -1 self.new_goal_ = True while not rospy.is_shutdown(): if self._as.is_preempt_requested(): rospy.loginfo('NAVTO: preempt requested') self._as.set_preempted() self.new_goal_ = False self.new_path_ = False self._ac_gp.cancel_all_goals() self._ac_lp.cancel_all_goals() return if self.global_planner_error_code == 0: self.new_goal_ = False self.new_path_ = False self._ac_lp.wait_for_result() self._as.set_succeeded() # rospy.loginfo('NAVTO: succeeded') break if self.global_planner_error_code >= 1: self.new_goal_ = False self.new_path_ = False self._ac_lp.wait_for_result() self._as.set_aborted() # rospy.loginfo('NAVTO: failed') break def Loop(self): rate = rospy.Rate(20) while not rospy.is_shutdown(): if self._as.is_preempt_requested(): self._ac_gp.cancel_all_goals() self._ac_lp.cancel_all_goals() if self.new_goal_: self._ac_gp.send_goal(self.global_planner_goal_, self.GlobalPlannerDoneCB, None, self.GlobalPlannerFeedbackCallback) self.new_goal_ = False if self.new_path_: self._ac_lp.send_goal(self.local_planner_goal_, self.LocalPlannerDoneCallback, None, self.LocalPlanerFeedbackCallback) self.new_path_ = False try: rate.sleep() except: rospy.loginfo('NAVTO: exiting') def GlobalPlannerDoneCB(self, state, result): rospy.loginfo("NAVTO: GP, %d, %d" % (state, result.error_code)) self.global_planner_error_code = result.error_code def GlobalPlannerFeedbackCallback(self, data): self.local_planner_goal_.route = data.path self.new_path_ = True def LocalPlannerDoneCallback(self, state, result): rospy.loginfo("NAVTO: LP, %d, %d" % (state, result.error_code)) self.local_planner_error_code = result.error_code def LocalPlanerFeedbackCallback(self, data): pass
class FakeTurtle: def __init__(self): # define the server, aut-start is set to false so we control when to # start it self.server = SimpleActionServer('move_turtle_action', MoveTurtleAction, execute_cb=self.action_cb, auto_start=False) # deine feedback msg object to be used for publishing feedback self.feedback_msg = MoveTurtleFeedback() # deine result msg object to be used for publishing result self.result_msg = MoveTurtleResult() self.server.start() print("action server started ..") self.rate = rospy.Rate(2) def turtle_is_up_side_down(self): # this turtle never goes up side down # it can always move! # (this is a dummy function, in real application you might be checking # your robot status, for example the battery level, # or motor temperature, etc..) return False def action_cb(self, goal): print("moving turtle to pose: ", goal) # Write here the logic to control turtle # this "for" loop is just a demostration, in real application, you # would be controlling the robot, publish velocity commands, read # current pose and do some computation.. for _ in range(20): print('executing goal') # during execution of the goal, publish feedback msg to the client self.server.publish_feedback(self.feedback_msg) # always check if the client has sent a cancel msg. This way the # task becomes preemtable if self.server.is_preempt_requested(): # update goal status accordingly self.server.set_preempted() print('goal preempted') # exit from the action_cb method return None # check if we need to abort for some reason. Here we abort if # the node has been killed (ctrl+C) or if the turtle is laying on # it's back and cannot move anyomre ;) if rospy.is_shutdown() or self.turtle_is_up_side_down(): # update goal status accordingly self.server.set_aborted() print('Turtle cannot move sir! goal aborted') return None self.rate.sleep() # if execution finished, set goal status to SUCCEED and publish the # result msg print('done') self.server.set_succeeded(result=self.result_msg, text="Mission completed")
class ShakePitchObjectActionServer: def __init__(self): self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus) self.start_audio_recording_srv = rospy.ServiceProxy('/audio_dump/start_audio_recording', StartAudioRecording) self.stop_audio_recording_srv = rospy.ServiceProxy('/audio_dump/stop_audio_recording', StopAudioRecording) self.wrist_pitch_velocity_srv = rospy.ServiceProxy('/wrist_pitch_controller/set_velocity', SetVelocity) self.wrist_pitch_command_pub = rospy.Publisher('wrist_pitch_controller/command', Float64) self.action_server = SimpleActionServer(ACTION_NAME, ShakePitchObjectAction, execute_cb=self.shake_pitch_object, auto_start=False) def initialize(self): rospy.loginfo('%s: waiting for wubble_grasp_status service' % ACTION_NAME) rospy.wait_for_service('/wubble_grasp_status') rospy.loginfo('%s: connected to wubble_grasp_status service' % ACTION_NAME) rospy.loginfo('%s: waiting for audio_dump/start_audio_recording service' % ACTION_NAME) rospy.wait_for_service('audio_dump/start_audio_recording') rospy.loginfo('%s: connected to audio_dump/start_audio_recording service' % ACTION_NAME) rospy.loginfo('%s: waiting for audio_dump/stop_audio_recording service' % ACTION_NAME) rospy.wait_for_service('audio_dump/stop_audio_recording') rospy.loginfo('%s: connected to audio_dump/stop_audio_recording service' % ACTION_NAME) rospy.loginfo('%s: waiting for wrist_pitch_controller service' % ACTION_NAME) rospy.wait_for_service('/wrist_pitch_controller/set_velocity') rospy.loginfo('%s: connected to wrist_pitch_controller service' % ACTION_NAME) self.action_server.start() def shake_pitch_object(self, goal): if self.action_server.is_preempt_requested(): rospy.loginfo('%s: preempted' % ACTION_NAME) self.action_server.set_preempted() wrist_pitch_state = '/wrist_pitch_controller/state' desired_velocity = 6.0 distance = 1.0 threshold = 0.1 timeout = 2.0 try: msg = rospy.wait_for_message(wrist_pitch_state, DynamixelJointState, timeout) current_pos = msg.position start_pos = current_pos # set wrist to initial position self.wrist_pitch_velocity_srv(3.0) self.wrist_pitch_command_pub.publish(distance) end_time = rospy.Time.now() + rospy.Duration(timeout) while current_pos < distance-threshold and rospy.Time.now() < end_time: msg = rospy.wait_for_message(wrist_pitch_state, DynamixelJointState, timeout) current_pos = msg.position rospy.sleep(10e-3) self.wrist_pitch_velocity_srv(desired_velocity) # start recording sound and shaking self.start_audio_recording_srv(InfomaxAction.SHAKE_PITCH, goal.category_id) rospy.sleep(0.5) for i in range(2): self.wrist_pitch_command_pub.publish(-distance) end_time = rospy.Time.now() + rospy.Duration(timeout) while current_pos > -distance+threshold and rospy.Time.now() < end_time: msg = rospy.wait_for_message(wrist_pitch_state, DynamixelJointState, timeout) current_pos = msg.position rospy.sleep(10e-3) self.wrist_pitch_command_pub.publish(distance) end_time = rospy.Time.now() + rospy.Duration(timeout) while current_pos < distance-threshold and rospy.Time.now() < end_time: msg = rospy.wait_for_message(wrist_pitch_state, DynamixelJointState, timeout) current_pos = msg.position rospy.sleep(10e-3) rospy.sleep(0.5) # check if are still holding an object after shaking sound_msg = None grasp_status = self.get_grasp_status_srv() if grasp_status.is_hand_occupied: sound_msg = self.stop_audio_recording_srv(True) else: self.stop_audio_recording_srv(False) # reset wrist to starting position self.wrist_pitch_velocity_srv(3.0) self.wrist_pitch_command_pub.publish(start_pos) end_time = rospy.Time.now() + rospy.Duration(timeout) while current_pos < distance-threshold and rospy.Time.now() < end_time: msg = rospy.wait_for_message(wrist_pitch_state, DynamixelJointState, timeout) current_pos = msg.position rospy.sleep(10e-3) rospy.sleep(0.5) if sound_msg: self.action_server.set_succeeded(ShakePitchObjectResult(sound_msg.recorded_sound)) return else: self.action_server.set_aborted() return except: rospy.logerr('%s: attempted pitch failed - %s' % ACTION_NAME) self.stop_audio_recording_srv(False) self.action_server.set_aborted()
class DropObjectActionServer: def __init__(self): self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus) self.start_audio_recording_srv = rospy.ServiceProxy('/audio_dump/start_audio_recording', StartAudioRecording) self.stop_audio_recording_srv = rospy.ServiceProxy('/audio_dump/stop_audio_recording', StopAudioRecording) self.posture_controller = SimpleActionClient('/wubble_gripper_grasp_action', GraspHandPostureExecutionAction) self.attached_object_pub = rospy.Publisher('/attached_collision_object', AttachedCollisionObject) self.action_server = SimpleActionServer(ACTION_NAME, DropObjectAction, execute_cb=self.drop_object, auto_start=False) def initialize(self): rospy.loginfo('%s: waiting for wubble_grasp_status service' % ACTION_NAME) rospy.wait_for_service('/wubble_grasp_status') rospy.loginfo('%s: connected to wubble_grasp_status service' % ACTION_NAME) rospy.loginfo('%s: waiting for wubble_gripper_grasp_action' % ACTION_NAME) self.posture_controller.wait_for_server() rospy.loginfo('%s: connected to wubble_gripper_grasp_action' % ACTION_NAME) rospy.loginfo('%s: waiting for audio_dump/start_audio_recording service' % ACTION_NAME) rospy.wait_for_service('audio_dump/start_audio_recording') rospy.loginfo('%s: connected to audio_dump/start_audio_recording service' % ACTION_NAME) rospy.loginfo('%s: waiting for audio_dump/stop_audio_recording service' % ACTION_NAME) rospy.wait_for_service('audio_dump/stop_audio_recording') rospy.loginfo('%s: connected to audio_dump/stop_audio_recording service' % ACTION_NAME) self.action_server.start() def open_gripper(self): pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.RELEASE self.posture_controller.send_goal(pg) self.posture_controller.wait_for_result() def drop_object(self, goal): if self.action_server.is_preempt_requested(): rospy.loginfo('%s: preempted' % ACTION_NAME) self.action_server.set_preempted() # check that we have something in hand before dropping it grasp_status = self.get_grasp_status_srv() # if the object is still in hand after lift is done we are good if not grasp_status.is_hand_occupied: rospy.logerr('%s: gripper empty, nothing to drop' % ACTION_NAME) self.action_server.set_aborted() return # record sound padded by 0.5 seconds from start and 1.5 seconds from the end self.start_audio_recording_srv(InfomaxAction.DROP, goal.category_id) rospy.sleep(0.5) self.open_gripper() rospy.sleep(1.5) # check if gripper actually opened first sound_msg = None grasp_status = self.get_grasp_status_srv() # if there something in the gripper - drop failed if grasp_status.is_hand_occupied: self.stop_audio_recording_srv(False) else: sound_msg = self.stop_audio_recording_srv(True) # delete the object that we just dropped, we don't really know where it will land obj = AttachedCollisionObject() obj.object.header.stamp = rospy.Time.now() obj.object.header.frame_id = GRIPPER_LINK_FRAME obj.object.operation.operation = CollisionObjectOperation.REMOVE obj.object.id = goal.collision_object_name obj.link_name = GRIPPER_LINK_FRAME obj.touch_links = GRIPPER_LINKS self.attached_object_pub.publish(obj) if sound_msg: self.action_server.set_succeeded(DropObjectResult(sound_msg.recorded_sound)) else: self.action_server.set_aborted()
class GraspObjectActionServer: def __init__(self): self.start_audio_recording_srv = rospy.ServiceProxy('/audio_dump/start_audio_recording', StartAudioRecording) self.stop_audio_recording_srv = rospy.ServiceProxy('/audio_dump/stop_audio_recording', StopAudioRecording) self.grasp_planning_srv = rospy.ServiceProxy('/GraspPlanning', GraspPlanning) self.get_solver_info_srv = rospy.ServiceProxy('/wubble2_left_arm_kinematics/get_ik_solver_info', GetKinematicSolverInfo) self.get_ik_srv = rospy.ServiceProxy('/wubble2_left_arm_kinematics/get_ik', GetPositionIK) self.set_planning_scene_diff_client = rospy.ServiceProxy('/environment_server/set_planning_scene_diff', SetPlanningSceneDiff) self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus) self.posture_controller = SimpleActionClient('/wubble_gripper_grasp_action', GraspHandPostureExecutionAction) self.move_arm_client = SimpleActionClient('/move_left_arm', MoveArmAction) self.attached_object_pub = rospy.Publisher('/attached_collision_object', AttachedCollisionObject) self.action_server = SimpleActionServer(ACTION_NAME, GraspObjectAction, execute_cb=self.grasp_object, auto_start=False) def initialize(self): rospy.loginfo('%s: waiting for audio_dump/start_audio_recording service' % ACTION_NAME) rospy.wait_for_service('audio_dump/start_audio_recording') rospy.loginfo('%s: connected to audio_dump/start_audio_recording service' % ACTION_NAME) rospy.loginfo('%s: waiting for audio_dump/stop_audio_recording service' % ACTION_NAME) rospy.wait_for_service('audio_dump/stop_audio_recording') rospy.loginfo('%s: connected to audio_dump/stop_audio_recording service' % ACTION_NAME) rospy.loginfo('%s: waiting for GraspPlanning service' % ACTION_NAME) rospy.wait_for_service('/GraspPlanning') rospy.loginfo('%s: connected to GraspPlanning service' % ACTION_NAME) rospy.loginfo('%s: waiting for wubble2_left_arm_kinematics/get_ik_solver_info service' % ACTION_NAME) rospy.wait_for_service('/wubble2_left_arm_kinematics/get_ik_solver_info') rospy.loginfo('%s: connected to wubble2_left_arm_kinematics/get_ik_solver_info service' % ACTION_NAME) rospy.loginfo('%s: waiting for wubble2_left_arm_kinematics/get_ik service' % ACTION_NAME) rospy.wait_for_service('/wubble2_left_arm_kinematics/get_ik') rospy.loginfo('%s: connected to wubble2_left_arm_kinematics/get_ik service' % ACTION_NAME) rospy.loginfo('%s: waiting for environment_server/set_planning_scene_diff service' % ACTION_NAME) rospy.wait_for_service('/environment_server/set_planning_scene_diff') rospy.loginfo('%s: connected to set_planning_scene_diff service' % ACTION_NAME) rospy.loginfo('%s: waiting for wubble_gripper_grasp_action' % ACTION_NAME) self.posture_controller.wait_for_server() rospy.loginfo('%s: connected to wubble_gripper_grasp_action' % ACTION_NAME) rospy.loginfo('%s: waiting for move_left_arm action server' % ACTION_NAME) self.move_arm_client.wait_for_server() rospy.loginfo('%s: connected to move_left_arm action server' % ACTION_NAME) rospy.loginfo('%s: waiting for wubble_grasp_status service' % ACTION_NAME) rospy.wait_for_service('/wubble_grasp_status') rospy.loginfo('%s: connected to wubble_grasp_status service' % ACTION_NAME) self.action_server.start() def find_ik_for_grasping_pose(self, pose_stamped): solver_info = self.get_solver_info_srv() arm_joints = solver_info.kinematic_solver_info.joint_names req = GetPositionIKRequest() req.timeout = rospy.Duration(5.0) req.ik_request.ik_link_name = GRIPPER_LINK_FRAME req.ik_request.pose_stamped = pose_stamped try: current_state = rospy.wait_for_message('/joint_states', JointState, 2.0) req.ik_request.ik_seed_state.joint_state.name = arm_joints req.ik_request.ik_seed_state.joint_state.position = [current_state.position[current_state.name.index(j)] for j in arm_joints] if not self.set_planning_scene_diff_client(): rospy.logerr('%s: Find IK for Grasp: failed to set planning scene diff' % ACTION_NAME) return None ik_result = self.get_ik_srv(req) if ik_result.error_code.val == ArmNavigationErrorCodes.SUCCESS: return ik_result.solution else: rospy.logerr('%s: failed to find an IK for requested grasping pose, aborting' % ACTION_NAME) return None except: rospy.logerr('%s: timed out waiting for joint state' % ACTION_NAME) return None def find_grasp_pose(self, target, collision_object_name='', collision_support_surface_name=''): """ target = GraspableObject collision_object_name = name of target in collision map collision_support_surface_name = name of surface target is touching """ req = GraspPlanningRequest() req.arm_name = ARM_GROUP_NAME req.target = target req.collision_object_name = collision_object_name req.collision_support_surface_name = collision_support_surface_name rospy.loginfo('%s: trying to find a good grasp for graspable object %s' % (ACTION_NAME, collision_object_name)) grasping_result = self.grasp_planning_srv(req) if grasping_result.error_code.value != GraspPlanningErrorCode.SUCCESS: rospy.logerr('%s: unable to find a feasable grasp, aborting' % ACTION_NAME) return None pose_stamped = PoseStamped() pose_stamped.header.frame_id = grasping_result.grasps[0].grasp_posture.header.frame_id pose_stamped.pose = grasping_result.grasps[0].grasp_pose rospy.loginfo('%s: found good grasp, looking for corresponding IK' % ACTION_NAME) return self.find_ik_for_grasping_pose(pose_stamped) def open_gripper(self): pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.RELEASE self.posture_controller.send_goal(pg) self.posture_controller.wait_for_result() def close_gripper(self): pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.GRASP self.posture_controller.send_goal(pg) self.posture_controller.wait_for_result() rospy.sleep(1) grasp_status = self.get_grasp_status_srv() return grasp_status.is_hand_occupied def grasp_object(self, goal): if self.action_server.is_preempt_requested(): rospy.loginfo('%s: preempted' % ACTION_NAME) self.action_server.set_preempted() target = goal.graspable_object collision_object_name = goal.collision_object_name collision_support_surface_name = goal.collision_support_surface_name ik_solution = self.find_grasp_pose(target, collision_object_name, collision_support_surface_name) if ik_solution: # disable collisions between gripper and target object ordered_collision_operations = OrderedCollisionOperations() collision_operation1 = CollisionOperation() collision_operation1.object1 = collision_object_name collision_operation1.object2 = GRIPPER_GROUP_NAME collision_operation1.operation = CollisionOperation.DISABLE collision_operation2 = CollisionOperation() collision_operation2.object1 = collision_support_surface_name collision_operation2.object2 = GRIPPER_GROUP_NAME collision_operation2.operation = CollisionOperation.DISABLE # collision_operation2.penetration_distance = 0.02 ordered_collision_operations.collision_operations = [collision_operation1, collision_operation2] # set gripper padding to 0 gripper_paddings = [LinkPadding(l,0.0) for l in GRIPPER_LINKS] gripper_paddings.extend([LinkPadding(l,0.0) for l in ARM_LINKS]) self.open_gripper() # move into pre-grasp pose if not move_arm_joint_goal(self.move_arm_client, ik_solution.joint_state.name, ik_solution.joint_state.position, link_padding=gripper_paddings, collision_operations=ordered_collision_operations): rospy.logerr('%s: attempted grasp failed' % ACTION_NAME) self.action_server.set_aborted() return # record grasping sound with 0.5 second padding before and after self.start_audio_recording_srv(InfomaxAction.GRASP, goal.category_id) rospy.sleep(0.5) grasp_successful = self.close_gripper() rospy.sleep(0.5) # if grasp was successful, attach it to the gripper if grasp_successful: sound_msg = self.stop_audio_recording_srv(True) obj = AttachedCollisionObject() obj.object.header.stamp = rospy.Time.now() obj.object.header.frame_id = GRIPPER_LINK_FRAME obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT obj.object.id = collision_object_name obj.link_name = GRIPPER_LINK_FRAME obj.touch_links = GRIPPER_LINKS self.attached_object_pub.publish(obj) self.action_server.set_succeeded(GraspObjectResult(sound_msg.recorded_sound)) return self.stop_audio_recording_srv(False) rospy.logerr('%s: attempted grasp failed' % ACTION_NAME) self.action_server.set_aborted()
class AggressiveGainBuffNode(object): def __init__(self, name="aggressive_gain_buff_action"): self.action_name = name self._as = SimpleActionServer(self.action_name, AggressiveGainBuffAction, execute_cb=self.ExecuteCB, auto_start=False) self.gain_buff_state = GainBuffState.ROUTE self.chassis_state_ = 0 self.sub_odom = rospy.Subscriber("aggressive_buff_info", AggressiveGainBuffInfo, callback=self.OdomCB) self.chassis_mode_client = rospy.ServiceProxy("set_chassis_mode", ChassisMode) self.chassis_arrive_state = ChassisArriveState() self.chassis_mode_ = 0 self._ac_navto = SimpleActionClient("nav_to_node_action", NavToAction) rospy.loginfo('GAIN_BUFF: Connecting NavTo action server...') ret = self._ac_navto.wait_for_server() rospy.loginfo('GAIN_BUFF: NavTo sever connected!') if ret else rospy.logerr('error: NavTo server not started!') self.nav_to_error_code = -1 self.search_start_time = rospy.Time(0) self._ac_look_n_move = SimpleActionClient("look_n_move_node_action", LookAndMoveAction) rospy.loginfo('GAIN_BUFF: Connecting Look and Move action server...') ret = self._ac_look_n_move.wait_for_server(timeout=rospy.Duration(3)) rospy.loginfo('GAIN_BUFF: Look and Move sever connected!') if ret else rospy.logerr('error: Look and Move server not started!') self.Look_n_move_error_code = -1 self.thread_ = Thread(target=self.Loop) self.thread_.start() self._as.start() def ExecuteCB(self, goal): print 'GAIN_BUFF:Aggressive gain buff goal recieved!' route = goal.route_index CHASSIS_MODE = ChassisModeForm() self.nav_to_error_code = -1 while not rospy.is_shutdown(): if self._as.is_preempt_requested(): print 'GAIN_BUFF:PREEMPT REQ' self.SetChassisMode(CHASSIS_MODE.AUTO_SEPARATE_GIMBAL) self._ac_navto.cancel_all_goals() self._ac_look_n_move.cancel_all_goals() self._as.set_preempted() return if self.gain_buff_state == GainBuffState.ROUTE: if route == 1 or route == 2: print 'GAIN_BUFF:Route %i received' % route if route == 1: self.SetChassisMode(CHASSIS_MODE.AGGRESSIVE_GAIN_BUFF_ONE) if route == 2: self.SetChassisMode(CHASSIS_MODE.AGGRESSIVE_GAIN_BUFF_TWO) print 'GAIN_BUFF:Wait for chassis response...' chassis_wait_start_time = rospy.get_time() print 'GAIN_BUFF:chassis state is %i' % (self.chassis_state_) while self.chassis_state_ == 0 and (rospy.get_time() - chassis_wait_start_time) < CHASSIS_MODE_WAIT_TIME: continue if self.chassis_state_ == 1: print 'GAIN_BUFF:Chassis has responded!' else: print 'GAIN_BUFF:Chassis does not respond for the new mode! Return.' self.SetChassisMode(CHASSIS_MODE.AUTO_SEPARATE_GIMBAL) self._as.set_aborted() return chassis_run_start_time = rospy.get_time() while not (self.chassis_state_ == self.chassis_arrive_state.SUCCEEDED or (rospy.get_time()-chassis_run_start_time > CHASSIS_RUN_WAIT_TIME) ): if self.chassis_state_ == self.chassis_arrive_state.SUCCEEDED: # self._as.set_succeeded() self.SetChassisMode(CHASSIS_MODE.AUTO_SEPARATE_GIMBAL) self.gain_buff_state = GainBuffState.SEARCH self.search_start_time = rospy.get_time() print 'GAIN_BUFF:Chassis Arrived Buff Area!' break else: print 'GAIN_BUFF:Chassis Aggressive gain buff FAILED, Time out!' self._as.set_aborted() self.SetChassisMode(CHASSIS_MODE.AUTO_SEPARATE_GIMBAL) return elif route == 3: print 'GAIN_BUFF:Route 3 received' self.NavToBuff() if self.nav_to_error_code == 0: self.SetChassisMode(CHASSIS_MODE.AUTO_SEPARATE_GIMBAL) # self._as.set_succeeded() self.gain_buff_state = GainBuffState.SEARCH self.search_start_time = rospy.get_time() print 'GAIN_BUFF:Aggressive Gain Buff Route 3 SUCCEED!' else: self._as.set_aborted() print 'No valied route' return elif self.gain_buff_state == GainBuffState.SEARCH: if (rospy.get_time() - self.search_start_time) < SEARCH_BUFF_WAIT_TIME: self.SearchBuff() else: print 'SEARCH BUFF: Chassis Aggressive search buff Time Out!' self._as.set_aborted() self.SetChassisMode(CHASSIS_MODE.AUTO_SEPARATE_GIMBAL) self.gain_buff_state = GainBuffState.IDEL return else: print 'Unvaild aggressive gain buff state!' self._as.set_aborted() self.SetChassisMode(CHASSIS_MODE.AUTO_SEPARATE_GIMBAL) return def Loop(self): rate = rospy.Rate(10) while not rospy.is_shutdown(): if self._as.is_preempt_requested(): self._ac_look_n_move.cancel_all_goals() self._ac_navto.cancel_all_goals() try: rate.sleep() except: rospy.loginfo('GAIN_BUFF: exiting') # Set chassis mode to use odom navigation def SetChassisMode(self, chassis_mode): if self.chassis_mode_ == chassis_mode: return rospy.wait_for_service("set_chassis_mode", timeout=5) print 'Set chassis mode service connected!' chassis_mode_msg = ChassisMode() call_status = self.chassis_mode_client.call(chassis_mode) chassis_mode_rec_ = ChassisModeResponse() chassis_mode_rec_ = chassis_mode_msg._response_class if(call_status and chassis_mode_rec_): self.chassis_mode_ = chassis_mode print 'Set Chassis Mode to %i' % chassis_mode else: print 'Set gimbal mode failed!' # return odom arrive result def OdomCB(self, data): self.chassis_state_ = data.state # Call NavTo Action def NavToBuff(self): print 'NavTo action is actived!' g = NavToActionGoal() for path in BuffPath: q = tf.transformations.quaternion_from_euler(0,0,path['yaw']) g.goal.navgoal.pose.position.x = path['x'] g.goal.navgoal.pose.position.y = path['y'] g.goal.navgoal.pose.orientation.z = q[2] g.goal.navgoal.pose.orientation.w = q[3] self._ac_navto.send_goal(g.goal, done_cb=self.done_cb, feedback_cb=None) print 'Waiting for %i point ...' % (path['id']) self._ac_navto.wait_for_result(timeout=rospy.Duration(10)) if self.nav_to_error_code != 0: self._ac_navto.cancel_all_goals() self._as.set_aborted() print 'AGGRESSIVA_GAIN_BUFF: NavTo Failed!' return print 'The result of %i point in BuffPath is arrived!' % path['id'] def SearchBuff(self): print 'Look and Move action is actived!' for goal in SearchBuffGoal: q = tf.transformations.quaternion_from_euler(0.,0.,goal['yaw']) g = LookAndMoveActionGoal() g.goal.relative_pose.header.frame_id = "map" g.goal.relative_pose.pose.position.x = goal['x'] g.goal.relative_pose.pose.position.y = goal['y'] g.goal.relative_pose.pose.orientation.z = q[2] g.goal.relative_pose.pose.orientation.w = q[3] self._ac_look_n_move.send_goal(g.goal) self._ac_look_n_move.wait_for_result(timeout=rospy.Duration(10)) if self.Look_n_move_error_code > 0: self._ac_look_n_move.cancel_all_goals() self._as.set_aborted() print 'AGGRESSIVA_GAIN_BUFF: Look and Move Failed!' return # Nav_to done result def done_cb(self,terminal_state,result): self.nav_to_error_code = result.error_code
class PlaceObjectActionServer: def __init__(self): self.start_audio_recording_srv = rospy.ServiceProxy('/audio_dump/start_audio_recording', StartAudioRecording) self.stop_audio_recording_srv = rospy.ServiceProxy('/audio_dump/stop_audio_recording', StopAudioRecording) self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus) self.posture_controller = SimpleActionClient('/wubble_gripper_grasp_action', GraspHandPostureExecutionAction) self.move_arm_client = SimpleActionClient('/move_left_arm', MoveArmAction) self.attached_object_pub = rospy.Publisher('/attached_collision_object', AttachedCollisionObject) self.action_server = SimpleActionServer(ACTION_NAME, PlaceObjectAction, execute_cb=self.place_object, auto_start=False) def initialize(self): rospy.loginfo('%s: waiting for audio_dump/start_audio_recording service' % ACTION_NAME) rospy.wait_for_service('audio_dump/start_audio_recording') rospy.loginfo('%s: connected to audio_dump/start_audio_recording service' % ACTION_NAME) rospy.loginfo('%s: waiting for audio_dump/stop_audio_recording service' % ACTION_NAME) rospy.wait_for_service('audio_dump/stop_audio_recording') rospy.loginfo('%s: connected to audio_dump/stop_audio_recording service' % ACTION_NAME) rospy.loginfo('%s: waiting for wubble_gripper_grasp_action' % ACTION_NAME) self.posture_controller.wait_for_server() rospy.loginfo('%s: connected to wubble_gripper_grasp_action' % ACTION_NAME) rospy.loginfo('%s: waiting for move_left_arm action server' % ACTION_NAME) self.move_arm_client.wait_for_server() rospy.loginfo('%s: connected to move_left_arm action server' % ACTION_NAME) self.action_server.start() def open_gripper(self): pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.RELEASE self.posture_controller.send_goal(pg) self.posture_controller.wait_for_result() def place_object(self, goal): if self.action_server.is_preempt_requested(): rospy.loginfo('%s: preempted' % ACTION_NAME) self.action_server.set_preempted() collision_object_name = goal.collision_object_name collision_support_surface_name = goal.collision_support_surface_name # check that we have something in hand before placing it grasp_status = self.get_grasp_status_srv() # if the object is still in hand after lift is done we are good if not grasp_status.is_hand_occupied: rospy.logerr('%s: gripper empty, nothing to place' % ACTION_NAME) self.action_server.set_aborted() return gripper_paddings = [LinkPadding(l,0.0) for l in GRIPPER_LINKS] # disable collisions between attached object and table collision_operation1 = CollisionOperation() collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS collision_operation1.object2 = collision_support_surface_name collision_operation1.operation = CollisionOperation.DISABLE # disable collisions between gripper and table collision_operation2 = CollisionOperation() collision_operation2.object1 = collision_support_surface_name collision_operation2.object2 = GRIPPER_GROUP_NAME collision_operation2.operation = CollisionOperation.DISABLE collision_operation2.penetration_distance = 0.01 # disable collisions between arm and table collision_operation3 = CollisionOperation() collision_operation3.object1 = collision_support_surface_name collision_operation3.object2 = ARM_GROUP_NAME collision_operation3.operation = CollisionOperation.DISABLE collision_operation3.penetration_distance = 0.01 ordered_collision_operations = OrderedCollisionOperations() ordered_collision_operations.collision_operations = [collision_operation1, collision_operation2, collision_operation3] self.start_audio_recording_srv(InfomaxAction.PLACE, goal.category_id) if move_arm_joint_goal(self.move_arm_client, ARM_JOINTS, PLACE_POSITION, link_padding=gripper_paddings, collision_operations=ordered_collision_operations): sound_msg = None grasp_status = self.get_grasp_status_srv() self.open_gripper() rospy.sleep(0.5) # if after lowering arm gripper is still holding an object life's good if grasp_status.is_hand_occupied: sound_msg = self.stop_audio_recording_srv(True) else: self.stop_audio_recording_srv(False) obj = AttachedCollisionObject() obj.object.header.stamp = rospy.Time.now() obj.object.header.frame_id = GRIPPER_LINK_FRAME obj.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT obj.object.id = collision_object_name obj.link_name = GRIPPER_LINK_FRAME obj.touch_links = GRIPPER_LINKS self.attached_object_pub.publish(obj) if sound_msg: self.action_server.set_succeeded(PlaceObjectResult(sound_msg.recorded_sound)) return else: self.action_server.set_aborted() return self.stop_audio_recording_srv(False) rospy.logerr('%s: attempted place failed' % ACTION_NAME) self.action_server.set_aborted()
class V_search(object): def __init__(self, height, res): self.resolution = res # Resolution of the motion self.motion_height = height # Height of the motion to the ground # Create search server self.search_server = SimpleActionServer('search_server', ExecuteSearchAction, self.searchCallback, False) self.server_feedback = ExecuteSearchFeedback() self.server_result = ExecuteSearchResult() # Get client from trajectory server self.trajectory_client = SimpleActionClient( "approach_server", ExecuteDroneApproachAction) self.trajectory_client.wait_for_server() self.next_point = ExecuteDroneApproachGoal( ) # Message to define next position to look for victims ## variables self.sonar_me = Condition() self.odometry_me = Condition() self.current_height = None self.odometry = None # Subscribe to sonar_height rospy.Subscriber("sonar_height", Range, self.sonar_callback, queue_size=10) # Subscribe to ground_truth to monitor the current pose of the robot rospy.Subscriber("ground_truth/state", Odometry, self.poseCallback) # Start trajectory server self.search_server.start() def trajectory_feed(self, msg): ''' Verifies preemption requisitions ''' if self.search_server.is_preempt_requested(): self.trajectory_client.cancel_goal() def searchCallback(self, search_area): ''' Execute a search for vitims into the defined area ''' x = search_area.x # x size of the area to explore y = search_area.y # y size of the area to explore start = search_area.origin # Point to start the search self.next_point.goal.position.x = start.x # Desired x position self.next_point.goal.position.y = start.y # Desired y position theta = 0 # Convert desired angle q = quaternion_from_euler(0, 0, theta, 'ryxz') self.next_point.goal.orientation.x = q[0] self.next_point.goal.orientation.y = q[1] self.next_point.goal.orientation.z = q[2] self.next_point.goal.orientation.w = q[3] x_positions = ceil(x / self.resolution) y_positions = ceil(y / self.resolution) x_count = 0 # Counter of steps (in meters traveled) direction = 1 # Direction of the motion (right, left or up) trials = 0 # v_search trials while not rospy.is_shutdown(): self.odometry_me.acquire() self.server_result.last_pose = self.odometry self.server_feedback.current_pose = self.odometry self.odometry_me.release() if self.search_server.is_preempt_requested(): self.search_server.set_preempted(self.server_result) return self.search_server.publish_feedback(self.server_feedback) self.sonar_me.acquire() # print("Current height from ground:\n\n{}".format(self.current_height)) # Current distance from ground h_error = self.motion_height - self.current_height self.sonar_me.release() self.odometry_me.acquire() self.next_point.goal.position.z = self.odometry.position.z + h_error # Desired z position self.odometry_me.release() self.trajectory_client.send_goal(self.next_point, feedback_cb=self.trajectory_feed) self.trajectory_client.wait_for_result() # Wait for the result result = self.trajectory_client.get_state( ) # Get the state of the action # print(result) if result == GoalStatus.SUCCEEDED: # Verifies if all the area have been searched if (self.next_point.goal.position.x == (start.x + x)) and ( (self.next_point.goal.position.y == (start.y + y))): self.odometry_me.acquire() self.server_result.last_pose = self.odometry self.odometry_me.release() self.search_server.set_succeeded(self.server_result) return last_direction = direction direction = self.square_function( x_count, 2 * x) # Get the direction of the next step if last_direction != direction: # drone moves on y axis theta = pi / 2 self.next_point.goal.position.y += y / y_positions elif direction == 1: # drone moves to the right theta = 0 self.next_point.goal.position.x += x / x_positions x_count += x / x_positions elif direction == -1: # drone moves to the left theta = pi self.next_point.goal.position.x -= x / x_positions x_count += x / x_positions # Convert desired angle q = quaternion_from_euler(0, 0, theta, 'ryxz') self.next_point.goal.orientation.x = q[0] self.next_point.goal.orientation.y = q[1] self.next_point.goal.orientation.z = q[2] self.next_point.goal.orientation.w = q[3] elif result == GoalStatus.ABORTED: trials += 1 if trials == 2: self.search_server.set_aborted(self.server_result) return def square_function(self, x, max_x): ''' Function to simulate a square wave function ''' if round(sin(2 * pi * x / max_x), 2) > 0: return 1 elif round(sin(2 * pi * x / max_x), 2) < 0: return -1 else: if round(cos(2 * pi * x / max_x), 2) > 0: return 1 else: return -1 def sonar_callback(self, msg): ''' Function to update drone height ''' self.sonar_me.acquire() self.current_height = msg.range self.sonar_me.release() def poseCallback(self, odometry): ''' Monitor the current position of the robot ''' self.odometry_me.acquire() self.odometry = odometry.pose.pose self.odometry_me.release()
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 CModelActionController(object): def __init__(self, activate=True): self._ns = rospy.get_namespace() # Read configuration parameters self._fb_rate = read_parameter( self._ns + 'gripper_action_controller/publish_rate', 60.0) self._min_gap = read_parameter( self._ns + 'gripper_action_controller/min_gap', 0.0) self._min_gap_counts = read_parameter( self._ns + 'gripper_action_controller/min_gap_counts', 230.) self._max_gap = read_parameter( self._ns + 'gripper_action_controller/max_gap', 0.085) self._min_speed = read_parameter( self._ns + 'gripper_action_controller/min_speed', 0.013) self._max_speed = read_parameter( self._ns + 'gripper_action_controller/max_speed', 0.1) self._min_force = read_parameter( self._ns + 'gripper_action_controller/min_force', 40.0) self._max_force = read_parameter( self._ns + 'gripper_action_controller/max_force', 100.0) # Configure and start the action server self._status = CModelStatus() self._name = self._ns + 'gripper_action_controller' self._server = SimpleActionServer(self._name, CModelCommandAction, execute_cb=self._execute_cb, auto_start=False) self.js_pub = rospy.Publisher('joint_states', JointState, queue_size=1) rospy.Subscriber('status', CModelStatus, self._status_cb, queue_size=1) self._cmd_pub = rospy.Publisher('command', CModelCommand, queue_size=1) working = True if activate and not self._ready(): rospy.sleep(2.0) working = self._activate() if not working: return self._server.start() rospy.logdebug('%s: Started' % self._name) def _preempt(self): self._stop() rospy.loginfo('%s: Preempted' % self._name) self._server.set_preempted() def _status_cb(self, msg): self._status = msg # Publish the joint_states for the gripper js_msg = JointState() js_msg.header.stamp = rospy.Time.now() js_msg.name.append('robotiq_85_left_knuckle_joint') js_msg.position.append(0.8 * self._status.gPO / self._min_gap_counts) self.js_pub.publish(js_msg) def _execute_cb(self, goal): success = True # Check that the gripper is active. If not, activate it. if not self._ready(): if not self._activate(): rospy.logwarn( '%s could not accept goal because the gripper is not yet active' % self._name) return # check that preempt has not been requested by the client if self._server.is_preempt_requested(): self._preempt() return # Clip the goal position = np.clip(goal.position, self._min_gap, self._max_gap) velocity = np.clip(goal.velocity, self._min_speed, self._max_speed) force = np.clip(goal.force, self._min_force, self._max_force) # Send the goal to the gripper and feedback to the action client rate = rospy.Rate(self._fb_rate) rospy.logdebug('%s: Moving gripper to position: %.3f ' % (self._name, position)) feedback = CModelCommandFeedback() while not self._reached_goal(position): self._goto_position(position, velocity, force) if rospy.is_shutdown() or self._server.is_preempt_requested(): self._preempt() return feedback.position = self._get_position() feedback.stalled = self._stalled() feedback.reached_goal = self._reached_goal(position) self._server.publish_feedback(feedback) rate.sleep() if self._stalled(): break rospy.logdebug('%s: Succeeded' % self._name) result = CModelCommandResult() result.position = self._get_position() result.stalled = self._stalled() result.reached_goal = self._reached_goal(position) self._server.set_succeeded(result) def _activate(self, timeout=5.0): command = CModelCommand() command.rACT = 1 command.rGTO = 1 command.rSP = 255 command.rFR = 150 start_time = rospy.get_time() while not self._ready(): if rospy.is_shutdown(): self._preempt() return False if rospy.get_time() - start_time > timeout: rospy.logwarn('Failed to activated gripper in ns [%s]' % (self._ns)) return False self._cmd_pub.publish(command) rospy.sleep(0.1) rospy.loginfo('Successfully activated gripper in ns [%s]' % (self._ns)) return True def _get_position(self): gPO = self._status.gPO pos = np.clip((self._max_gap - self._min_gap) / (-self._min_gap_counts) * (gPO - self._min_gap_counts), self._min_gap, self._max_gap) return pos def _goto_position(self, pos, vel, force): """ Goto position with desired force and velocity @type pos: float @param pos: Gripper width in meters @type vel: float @param vel: Gripper speed in m/s @type force: float @param force: Gripper force in N """ command = CModelCommand() command.rACT = 1 command.rGTO = 1 command.rPR = int( np.clip((-self._min_gap_counts) / (self._max_gap - self._min_gap) * (pos - self._min_gap) + self._min_gap_counts, 0, self._min_gap_counts)) command.rSP = int( np.clip((255) / (self._max_speed - self._min_speed) * (vel - self._min_speed), 0, 255)) command.rFR = int( np.clip((255) / (self._max_force - self._min_force) * (force - self._min_force), 0, 255)) self._cmd_pub.publish(command) def _moving(self): return self._status.gGTO == 1 and self._status.gOBJ == 0 def _reached_goal(self, goal, tol=0.003): return (abs(goal - self._get_position()) < tol) def _ready(self): return self._status.gSTA == 3 and self._status.gACT == 1 def _stalled(self): return self._status.gOBJ == 1 or self._status.gOBJ == 2 def _stop(self): command = CModelCommand() command.rACT = 1 command.rGTO = 0 self._cmd_pub.publish(command) rospy.logdebug('Stopping gripper in ns [%s]' % (self._ns))
class PathExecutor: _continue=True _skip_unreachable=True _path_result=ExecutePathResult() def __init__(self): self._poses = [] self._poses_idx = 0 self._client = SimpleActionClient('/bug2/move_to', MoveToAction) self._action_name = rospy.get_name() + "/execute_path" self._server = SimpleActionServer(self._action_name, ExecutePathAction, execute_cb=self.execute_cb, auto_start = False) self._server.start() def execute_cb(self, goal): """ This funciton is used to execute the move to goal """ self._path_result.visited=[] #Initialize the path result self._skip_unreachable=goal.skip_unreachable self._client.wait_for_server() self._poses = goal.path.poses self._poses_idx = 0 #Start with the first goal self._continue= True rospy.loginfo('Starting Number of Poses: %s'%len(self._poses)) move = MoveToGoal() move.target_pose.pose = self._poses[self._poses_idx].pose """ Send the goal to the _client and once done, go to callback function "move_to_done_cb" """ self._client.send_goal(move, done_cb=self.move_to_done_cb) while (self._continue==True): """ Check if current goal is preempted by other user action. If so, break out of loop """ if self._server.is_preempt_requested(): rospy.loginfo('%s: Preempted' % self._action_name) self._server.set_preempted() self._continue= False break def move_to_done_cb(self, state, result): """ This call back function is called once one goal is completed. This checks if there are any other goal remaining to be executed and recurrsively calls itself. """ feedback = ExecutePathFeedback() feedback.pose=self._poses[self._poses_idx] """ #====================================================================== #* If state is 3, i.e., goal was successful, publish feedback # and move to next goal # #* Else publish feedback and check _skip_unreachable flag, # * If True go to next pose # * Else break #====================================================================== """ if(state == 3): feedback.reached = True self._path_result.visited.append(feedback.reached) self._server.publish_feedback(feedback) self._poses_idx = self._poses_idx + 1 """ * If more pose available, move to next goal * Else break """ if(self._poses_idx < len(self._poses)): move = MoveToGoal() move.target_pose.pose = self._poses[self._poses_idx].pose self._client.send_goal(move, done_cb=self.move_to_done_cb) else: self._continue=False self._server.set_succeeded(self._path_result) else: feedback.reached = False self._path_result.visited.append(False) self._server.publish_feedback(feedback) if(self._skip_unreachable==True): self._poses_idx = self._poses_idx + 1 """ * If more pose available, move to next goal * Else break """ if(self._poses_idx < len(self._poses)): move = MoveToGoal() move.target_pose.pose = self._poses[self._poses_idx].pose self._client.send_goal(move, done_cb=self.move_to_done_cb) else: self._continue=False self._server.set_succeeded(self._path_result) else: rospy.loginfo('Unreachable') self._continue=False self._server.set_succeeded(self._path_result)
class AStarSearch(object): def __init__(self, name): rospy.loginfo("Starting {}".format(name)) self._as = SimpleActionServer(name, TopoNavAction, self.execute_cb, auto_start=False) self._as.register_preempt_callback(self.preempt_cb) self.client = SimpleActionClient("/waypoint_nav_node", WayPointNavAction) rospy.loginfo("Waiting for nav server") self.client.wait_for_server() self.nodes = set() self.edges = defaultdict(list) self.distances = {} with open(rospy.get_param("~waypoint_yaml"), 'r') as f: self.waypointInfo = yaml.load(f) for waypoint, info in self.waypointInfo.items(): self.add_node(waypoint) for edge in info["edges"]: self.add_edge(waypoint, edge, 1.0) self.waypointInfo[waypoint]["position"]["x"] *= 0.555 self.waypointInfo[waypoint]["position"]["y"] *= 0.555 self.sub = rospy.Subscriber("/orb_slam/pose", PoseStamped, self.pose_cb) self._as.start() rospy.loginfo("Started {}".format(name)) def preempt_cb(self, *args): self.client.cancel_all_goals() def add_node(self, value): self.nodes.add(value) def add_edge(self, from_node, to_node, distance): self.edges[from_node].append(to_node) # self.edges[to_node].append(from_node) self.distances[(from_node, to_node)] = distance def pose_cb(self, msg): closest = (None, None) for k, v in self.waypointInfo.items(): dist = self.get_dist(msg, v["position"]) if closest[0] is None or dist < closest[0]: closest = (dist, k) self.closest = closest[1] def get_dist(self, pose1, position): return np.sqrt( np.power(pose1.pose.position.x - position["x"], 2) + np.power(pose1.pose.position.y - position["y"], 2)) def execute_cb(self, goal): path = self.shortest_path(self.closest, goal.waypoint)[1] print path for p in path: if not self._as.is_preempt_requested(): rospy.loginfo("Going to waypoint: {}".format(p)) target = PoseStamped() target.header.stamp = rospy.Time.now() target.header.frame_id = "map" target.pose.position.x = self.waypointInfo[p]["position"]["x"] target.pose.position.y = self.waypointInfo[p]["position"]["y"] target.pose.orientation.x = self.waypointInfo[p][ "orientation"]["x"] target.pose.orientation.y = self.waypointInfo[p][ "orientation"]["y"] target.pose.orientation.z = self.waypointInfo[p][ "orientation"]["z"] target.pose.orientation.w = self.waypointInfo[p][ "orientation"]["w"] self.client.send_goal_and_wait(WayPointNavGoal(target)) if not self._as.is_preempt_requested(): self._as.set_succeeded(TopoNavResult()) else: self._as.set_preempted() def dijkstra(self, initial): visited = {initial: 0} path = {} nodes = set(self.nodes) while nodes: min_node = None for node in nodes: if node in visited: if min_node is None: min_node = node elif visited[node] < visited[min_node]: min_node = node if min_node is None: break nodes.remove(min_node) current_weight = visited[min_node] for edge in self.edges[min_node]: weight = current_weight + self.distances[(min_node, edge)] if edge not in visited or weight < visited[edge]: visited[edge] = weight path[edge] = min_node return visited, path def shortest_path(self, origin, destination): visited, paths = self.dijkstra(origin) full_path = deque() _destination = paths[destination] while _destination != origin: full_path.appendleft(_destination) _destination = paths[_destination] full_path.appendleft(origin) full_path.append(destination) return visited[destination], list(full_path)
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 PutDownAtActionServer: def __init__(self): self.start_audio_recording_srv = rospy.ServiceProxy( '/audio_dump/start_audio_recording', StartAudioRecording) self.stop_audio_recording_srv = rospy.ServiceProxy( '/audio_dump/stop_audio_recording', StopAudioRecording) self.grasp_planning_srv = rospy.ServiceProxy('/GraspPlanning', GraspPlanning) self.get_solver_info_srv = rospy.ServiceProxy( '/wubble2_left_arm_kinematics/get_ik_solver_info', GetKinematicSolverInfo) self.get_ik_srv = rospy.ServiceProxy( '/wubble2_left_arm_kinematics/get_ik', GetPositionIK) self.set_planning_scene_diff_client = rospy.ServiceProxy( '/environment_server/set_planning_scene_diff', SetPlanningSceneDiff) self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus) self.find_cluster_bbox = rospy.ServiceProxy( '/find_cluster_bounding_box', FindClusterBoundingBox) self.posture_controller = SimpleActionClient( '/wubble_gripper_grasp_action', GraspHandPostureExecutionAction) self.move_arm_client = SimpleActionClient('/move_left_arm', MoveArmAction) self.attached_object_pub = rospy.Publisher( '/attached_collision_object', AttachedCollisionObject) self.action_server = SimpleActionServer(ACTION_NAME, PutDownAtAction, execute_cb=self.put_down_at, auto_start=False) def initialize(self): rospy.loginfo( '%s: waiting for audio_dump/start_audio_recording service' % ACTION_NAME) rospy.wait_for_service('audio_dump/start_audio_recording') rospy.loginfo( '%s: connected to audio_dump/start_audio_recording service' % ACTION_NAME) rospy.loginfo( '%s: waiting for audio_dump/stop_audio_recording service' % ACTION_NAME) rospy.wait_for_service('audio_dump/stop_audio_recording') rospy.loginfo( '%s: connected to audio_dump/stop_audio_recording service' % ACTION_NAME) rospy.loginfo('%s: waiting for GraspPlanning service' % ACTION_NAME) rospy.wait_for_service('/GraspPlanning') rospy.loginfo('%s: connected to GraspPlanning service' % ACTION_NAME) rospy.loginfo( '%s: waiting for wubble2_left_arm_kinematics/get_ik_solver_info service' % ACTION_NAME) rospy.wait_for_service( '/wubble2_left_arm_kinematics/get_ik_solver_info') rospy.loginfo( '%s: connected to wubble2_left_arm_kinematics/get_ik_solver_info service' % ACTION_NAME) rospy.loginfo( '%s: waiting for wubble2_left_arm_kinematics/get_ik service' % ACTION_NAME) rospy.wait_for_service('/wubble2_left_arm_kinematics/get_ik') rospy.loginfo( '%s: connected to wubble2_left_arm_kinematics/get_ik service' % ACTION_NAME) rospy.loginfo( '%s: waiting for environment_server/set_planning_scene_diff service' % ACTION_NAME) rospy.wait_for_service('/environment_server/set_planning_scene_diff') rospy.loginfo('%s: connected to set_planning_scene_diff service' % ACTION_NAME) rospy.loginfo('%s: waiting for wubble_gripper_grasp_action' % ACTION_NAME) self.posture_controller.wait_for_server() rospy.loginfo('%s: connected to wubble_gripper_grasp_action' % ACTION_NAME) rospy.loginfo('%s: waiting for move_left_arm action server' % ACTION_NAME) self.move_arm_client.wait_for_server() rospy.loginfo('%s: connected to move_left_arm action server' % ACTION_NAME) rospy.loginfo('%s: waiting for wubble_grasp_status service' % ACTION_NAME) rospy.wait_for_service('/wubble_grasp_status') rospy.loginfo('%s: connected to wubble_grasp_status service' % ACTION_NAME) rospy.loginfo('%s: waiting for find_cluster_bounding_box service' % ACTION_NAME) rospy.wait_for_service('/find_cluster_bounding_box') rospy.loginfo('%s: connected to find_cluster_bounding_box service' % ACTION_NAME) self.action_server.start() def find_ik_for_grasping_pose(self, pose_stamped): solver_info = self.get_solver_info_srv() arm_joints = solver_info.kinematic_solver_info.joint_names req = GetPositionIKRequest() req.timeout = rospy.Duration(5.0) req.ik_request.ik_link_name = GRIPPER_LINK_FRAME req.ik_request.pose_stamped = pose_stamped try: current_state = rospy.wait_for_message('/joint_states', JointState, 2.0) req.ik_request.ik_seed_state.joint_state.name = arm_joints req.ik_request.ik_seed_state.joint_state.position = [ current_state.position[current_state.name.index(j)] for j in arm_joints ] if not self.set_planning_scene_diff_client(): rospy.logerr( '%s: Find IK for Grasp: failed to set planning scene diff' % ACTION_NAME) return None ik_result = self.get_ik_srv(req) if ik_result.error_code.val == ArmNavigationErrorCodes.SUCCESS: return ik_result.solution else: rospy.logerr( '%s: failed to find an IK for requested grasping pose, aborting' % ACTION_NAME) return None except: rospy.logerr('%s: timed out waiting for joint state' % ACTION_NAME) return None def find_grasp_pose(self, target, collision_object_name='', collision_support_surface_name=''): """ target = GraspableObject collision_object_name = name of target in collision map collision_support_surface_name = name of surface target is touching """ req = GraspPlanningRequest() req.arm_name = ARM_GROUP_NAME req.target = target req.collision_object_name = collision_object_name req.collision_support_surface_name = collision_support_surface_name rospy.loginfo( '%s: trying to find a good grasp for graspable object %s' % (ACTION_NAME, collision_object_name)) grasping_result = self.grasp_planning_srv(req) if grasping_result.error_code.value != GraspPlanningErrorCode.SUCCESS: rospy.logerr('%s: unable to find a feasable grasp, aborting' % ACTION_NAME) return None pose_stamped = PoseStamped() pose_stamped.header.frame_id = grasping_result.grasps[ 0].grasp_posture.header.frame_id pose_stamped.pose = grasping_result.grasps[0].grasp_pose rospy.loginfo('%s: found good grasp, looking for corresponding IK' % ACTION_NAME) return self.find_ik_for_grasping_pose(pose_stamped) def open_gripper(self): pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.RELEASE self.posture_controller.send_goal(pg) self.posture_controller.wait_for_result() def close_gripper(self): pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.GRASP self.posture_controller.send_goal(pg) self.posture_controller.wait_for_result() rospy.sleep(1) grasp_status = self.get_grasp_status_srv() return grasp_status.is_hand_occupied def find_cluster_bounding_box_center(self, cluster): fcbb = FindClusterBoundingBoxRequest() fcbb.cluster = cluster res = self.find_cluster_bbox(fcbb) if res.error_code == FindClusterBoundingBoxResponse.TF_ERROR: rospy.logerr( 'Unable to find bounding box for requested point cluster') return None return res.pose.pose.position def put_down_at(self, goal): if self.action_server.is_preempt_requested(): rospy.loginfo('%s: preempted' % ACTION_NAME) self.action_server.set_preempted() bbox_center = self.find_cluster_bounding_box_center( goal.graspable_object.cluster) if not bbox_center: self.action_server.set_aborted() goal.target_location.z = bbox_center.z x_dist = goal.target_location.x - bbox_center.x y_dist = goal.target_location.y - bbox_center.y target = goal.graspable_object target.cluster.points = [ Point32(p.x + x_dist, p.y + y_dist, p.z) for p in target.cluster.points ] collision_object_name = goal.collision_object_name collision_support_surface_name = goal.collision_support_surface_name ik_solution = self.find_grasp_pose(target, collision_object_name, collision_support_surface_name) if ik_solution: # disable collisions between gripper and target object ordered_collision_operations = OrderedCollisionOperations() collision_operation = CollisionOperation() collision_operation.object1 = collision_object_name collision_operation.object2 = GRIPPER_GROUP_NAME collision_operation.operation = CollisionOperation.DISABLE # disable collisions between gripper and table collision_operation2 = CollisionOperation() collision_operation2.object1 = collision_support_surface_name collision_operation2.object2 = GRIPPER_GROUP_NAME collision_operation2.operation = CollisionOperation.DISABLE collision_operation2.penetration_distance = 0.01 # disable collisions between arm and table collision_operation3 = CollisionOperation() collision_operation3.object1 = collision_support_surface_name collision_operation3.object2 = ARM_GROUP_NAME collision_operation3.operation = CollisionOperation.DISABLE collision_operation3.penetration_distance = 0.01 ordered_collision_operations.collision_operations = [ collision_operation, collision_operation2, collision_operation3 ] # set gripper padding to 0 gripper_paddings = [LinkPadding(l, 0.0) for l in GRIPPER_LINKS] gripper_paddings.extend([LinkPadding(l, 0.0) for l in ARM_LINKS]) # move into pre-grasp pose if not move_arm_joint_goal( self.move_arm_client, ik_solution.joint_state.name, ik_solution.joint_state.position, link_padding=gripper_paddings, collision_operations=ordered_collision_operations): rospy.logerr('%s: attempted grasp failed' % ACTION_NAME) self.action_server.set_aborted() return # record put down sound with 0.5 second padding before and after self.start_audio_recording_srv(InfomaxAction.GRASP, goal.category_id) rospy.sleep(0.5) self.open_gripper() rospy.sleep(0.5) sound_msg = self.stop_audio_recording_srv(True) obj = AttachedCollisionObject() obj.object.header.stamp = rospy.Time.now() obj.object.header.frame_id = GRIPPER_LINK_FRAME obj.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT obj.object.id = collision_object_name obj.link_name = GRIPPER_LINK_FRAME obj.touch_links = GRIPPER_LINKS self.attached_object_pub.publish(obj) self.action_server.set_succeeded( PutDownAtResult(sound_msg.recorded_sound)) return self.stop_audio_recording_srv(False) rospy.logerr('%s: attempted put down failed' % ACTION_NAME) self.action_server.set_aborted()
class PlaceObjectActionServer: def __init__(self): self.start_audio_recording_srv = rospy.ServiceProxy( '/audio_dump/start_audio_recording', StartAudioRecording) self.stop_audio_recording_srv = rospy.ServiceProxy( '/audio_dump/stop_audio_recording', StopAudioRecording) self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus) self.posture_controller = SimpleActionClient( '/wubble_gripper_grasp_action', GraspHandPostureExecutionAction) self.move_arm_client = SimpleActionClient('/move_left_arm', MoveArmAction) self.attached_object_pub = rospy.Publisher( '/attached_collision_object', AttachedCollisionObject) self.action_server = SimpleActionServer(ACTION_NAME, PlaceObjectAction, execute_cb=self.place_object, auto_start=False) def initialize(self): rospy.loginfo( '%s: waiting for audio_dump/start_audio_recording service' % ACTION_NAME) rospy.wait_for_service('audio_dump/start_audio_recording') rospy.loginfo( '%s: connected to audio_dump/start_audio_recording service' % ACTION_NAME) rospy.loginfo( '%s: waiting for audio_dump/stop_audio_recording service' % ACTION_NAME) rospy.wait_for_service('audio_dump/stop_audio_recording') rospy.loginfo( '%s: connected to audio_dump/stop_audio_recording service' % ACTION_NAME) rospy.loginfo('%s: waiting for wubble_gripper_grasp_action' % ACTION_NAME) self.posture_controller.wait_for_server() rospy.loginfo('%s: connected to wubble_gripper_grasp_action' % ACTION_NAME) rospy.loginfo('%s: waiting for move_left_arm action server' % ACTION_NAME) self.move_arm_client.wait_for_server() rospy.loginfo('%s: connected to move_left_arm action server' % ACTION_NAME) self.action_server.start() def open_gripper(self): pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.RELEASE self.posture_controller.send_goal(pg) self.posture_controller.wait_for_result() def place_object(self, goal): if self.action_server.is_preempt_requested(): rospy.loginfo('%s: preempted' % ACTION_NAME) self.action_server.set_preempted() collision_object_name = goal.collision_object_name collision_support_surface_name = goal.collision_support_surface_name # check that we have something in hand before placing it grasp_status = self.get_grasp_status_srv() # if the object is still in hand after lift is done we are good if not grasp_status.is_hand_occupied: rospy.logerr('%s: gripper empty, nothing to place' % ACTION_NAME) self.action_server.set_aborted() return gripper_paddings = [LinkPadding(l, 0.0) for l in GRIPPER_LINKS] # disable collisions between attached object and table collision_operation1 = CollisionOperation() collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS collision_operation1.object2 = collision_support_surface_name collision_operation1.operation = CollisionOperation.DISABLE # disable collisions between gripper and table collision_operation2 = CollisionOperation() collision_operation2.object1 = collision_support_surface_name collision_operation2.object2 = GRIPPER_GROUP_NAME collision_operation2.operation = CollisionOperation.DISABLE collision_operation2.penetration_distance = 0.01 # disable collisions between arm and table collision_operation3 = CollisionOperation() collision_operation3.object1 = collision_support_surface_name collision_operation3.object2 = ARM_GROUP_NAME collision_operation3.operation = CollisionOperation.DISABLE collision_operation3.penetration_distance = 0.01 ordered_collision_operations = OrderedCollisionOperations() ordered_collision_operations.collision_operations = [ collision_operation1, collision_operation2, collision_operation3 ] self.start_audio_recording_srv(InfomaxAction.PLACE, goal.category_id) if move_arm_joint_goal( self.move_arm_client, ARM_JOINTS, PLACE_POSITION, link_padding=gripper_paddings, collision_operations=ordered_collision_operations): sound_msg = None grasp_status = self.get_grasp_status_srv() self.open_gripper() rospy.sleep(0.5) # if after lowering arm gripper is still holding an object life's good if grasp_status.is_hand_occupied: sound_msg = self.stop_audio_recording_srv(True) else: self.stop_audio_recording_srv(False) obj = AttachedCollisionObject() obj.object.header.stamp = rospy.Time.now() obj.object.header.frame_id = GRIPPER_LINK_FRAME obj.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT obj.object.id = collision_object_name obj.link_name = GRIPPER_LINK_FRAME obj.touch_links = GRIPPER_LINKS self.attached_object_pub.publish(obj) if sound_msg: self.action_server.set_succeeded( PlaceObjectResult(sound_msg.recorded_sound)) return else: self.action_server.set_aborted() return self.stop_audio_recording_srv(False) rospy.logerr('%s: attempted place failed' % ACTION_NAME) self.action_server.set_aborted()
class PushObjectActionServer: def __init__(self): self.start_audio_recording_srv = rospy.ServiceProxy( '/audio_dump/start_audio_recording', StartAudioRecording) self.stop_audio_recording_srv = rospy.ServiceProxy( '/audio_dump/stop_audio_recording', StopAudioRecording) self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus) self.interpolated_ik_params_srv = rospy.ServiceProxy( '/l_interpolated_ik_motion_plan_set_params', SetInterpolatedIKMotionPlanParams) self.interpolated_ik_srv = rospy.ServiceProxy( '/l_interpolated_ik_motion_plan', GetMotionPlan) self.set_planning_scene_diff_client = rospy.ServiceProxy( '/environment_server/set_planning_scene_diff', SetPlanningSceneDiff) self.get_fk_srv = rospy.ServiceProxy( '/wubble2_left_arm_kinematics/get_fk', GetPositionFK) self.trajectory_filter_srv = rospy.ServiceProxy( '/trajectory_filter_unnormalizer/filter_trajectory', FilterJointTrajectory) self.trajectory_controller = SimpleActionClient( '/l_arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction) self.attached_object_pub = rospy.Publisher( '/attached_collision_object', AttachedCollisionObject) self.action_server = SimpleActionServer(ACTION_NAME, PushObjectAction, execute_cb=self.push_object, auto_start=False) def initialize(self): rospy.loginfo( '%s: waiting for audio_dump/start_audio_recording service' % ACTION_NAME) rospy.wait_for_service('audio_dump/start_audio_recording') rospy.loginfo( '%s: connected to audio_dump/start_audio_recording service' % ACTION_NAME) rospy.loginfo( '%s: waiting for audio_dump/stop_audio_recording service' % ACTION_NAME) rospy.wait_for_service('audio_dump/stop_audio_recording') rospy.loginfo( '%s: connected to audio_dump/stop_audio_recording service' % ACTION_NAME) rospy.loginfo( '%s: waiting for l_interpolated_ik_motion_plan_set_params service' % ACTION_NAME) rospy.wait_for_service('/l_interpolated_ik_motion_plan_set_params') rospy.loginfo( '%s: connected to l_interpolated_ik_motion_plan_set_params service' % ACTION_NAME) rospy.loginfo('%s: waiting for l_interpolated_ik_motion_plan service' % ACTION_NAME) rospy.wait_for_service('/l_interpolated_ik_motion_plan') rospy.loginfo( '%s: connected to l_interpolated_ik_motion_plan service' % ACTION_NAME) rospy.loginfo( '%s: waiting for environment_server/set_planning_scene_diff service' % ACTION_NAME) rospy.wait_for_service('/environment_server/set_planning_scene_diff') rospy.loginfo('%s: connected to set_planning_scene_diff service' % ACTION_NAME) rospy.loginfo( '%s: waiting for wubble2_left_arm_kinematics/get_fk service' % ACTION_NAME) rospy.wait_for_service('/wubble2_left_arm_kinematics/get_fk') rospy.loginfo( '%s: connected to wubble2_left_arm_kinematics/get_fk service' % ACTION_NAME) rospy.loginfo( '%s: waiting for trajectory_filter_unnormalizer/filter_trajectory service' % ACTION_NAME) rospy.wait_for_service( '/trajectory_filter_unnormalizer/filter_trajectory') rospy.loginfo( '%s: connected to trajectory_filter_unnormalizer/filter_trajectory service' % ACTION_NAME) rospy.loginfo( '%s: waiting for l_arm_controller/follow_joint_trajectory' % ACTION_NAME) self.trajectory_controller.wait_for_server() rospy.loginfo( '%s: connected to l_arm_controller/follow_joint_trajectory' % ACTION_NAME) self.action_server.start() def create_pose_stamped(self, pose, frame_id='base_link'): """ Creates a PoseStamped message from a list of 7 numbers (first three are position and next four are orientation: pose = [px,py,pz, ox,oy,oz,ow] """ m = PoseStamped() m.header.frame_id = frame_id m.header.stamp = rospy.Time() m.pose.position = Point(*pose[0:3]) m.pose.orientation = Quaternion(*pose[3:7]) return m #pretty-print list to string def pplist(self, list_to_print): return ' '.join(['%5.3f' % x for x in list_to_print]) def get_interpolated_ik_motion_plan(self, start_pose, goal_pose, start_angles, joint_names, pos_spacing=0.01, rot_spacing=0.1, consistent_angle=math.pi / 9, collision_aware=True, collision_check_resolution=1, steps_before_abort=-1, num_steps=0, ordered_collision_operations=None, frame='base_footprint', start_from_end=0, max_joint_vels=[1.5] * 7, max_joint_accs=[8.0] * 7): res = self.interpolated_ik_params_srv(num_steps, consistent_angle, collision_check_resolution, steps_before_abort, pos_spacing, rot_spacing, collision_aware, start_from_end, max_joint_vels, max_joint_accs) req = GetMotionPlanRequest() req.motion_plan_request.start_state.joint_state.name = joint_names req.motion_plan_request.start_state.joint_state.position = start_angles req.motion_plan_request.start_state.multi_dof_joint_state.poses = [ start_pose.pose ] req.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids = [ GRIPPER_LINK_FRAME ] req.motion_plan_request.start_state.multi_dof_joint_state.frame_ids = [ start_pose.header.frame_id ] pos_constraint = PositionConstraint() pos_constraint.position = goal_pose.pose.position pos_constraint.header.frame_id = goal_pose.header.frame_id req.motion_plan_request.goal_constraints.position_constraints = [ pos_constraint, ] orient_constraint = OrientationConstraint() orient_constraint.orientation = goal_pose.pose.orientation orient_constraint.header.frame_id = goal_pose.header.frame_id req.motion_plan_request.goal_constraints.orientation_constraints = [ orient_constraint, ] #req.motion_plan_request.link_padding = [LinkPadding(l,0.0) for l in GRIPPER_LINKS] #req.motion_plan_request.link_padding.extend([LinkPadding(l,0.0) for l in ARM_LINKS]) #if ordered_collision_operations is not None: # req.motion_plan_request.ordered_collision_operations = ordered_collision_operations res = self.interpolated_ik_srv(req) return res def check_cartesian_path_lists(self, approachpos, approachquat, grasppos, graspquat, start_angles, pos_spacing=0.03, rot_spacing=0.1, consistent_angle=math.pi / 7.0, collision_aware=True, collision_check_resolution=1, steps_before_abort=1, num_steps=0, ordered_collision_operations=None, frame='base_link'): start_pose = self.create_pose_stamped(approachpos + approachquat, frame) goal_pose = self.create_pose_stamped(grasppos + graspquat, frame) return self.get_interpolated_ik_motion_plan( start_pose, goal_pose, start_angles, ARM_JOINTS, pos_spacing, rot_spacing, consistent_angle, collision_aware, collision_check_resolution, steps_before_abort, num_steps, ordered_collision_operations, frame) def push_object(self, goal): if self.action_server.is_preempt_requested(): rospy.loginfo('%s: preempted' % ACTION_NAME) self.action_server.set_preempted() collision_object_name = goal.collision_object_name collision_support_surface_name = goal.collision_support_surface_name current_state = rospy.wait_for_message('l_arm_controller/state', FollowJointTrajectoryFeedback) start_angles = current_state.actual.positions full_state = rospy.wait_for_message('/joint_states', JointState) req = GetPositionFKRequest() req.header.frame_id = 'base_link' req.fk_link_names = [GRIPPER_LINK_FRAME] req.robot_state.joint_state = full_state if not self.set_planning_scene_diff_client(): rospy.logerr('%s: failed to set planning scene diff' % ACTION_NAME) self.action_server.set_aborted() return pose = self.get_fk_srv(req).pose_stamped[0].pose frame_id = 'base_link' approachpos = [pose.position.x, pose.position.y, pose.position.z] approachquat = [ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w ] push_distance = 0.40 grasppos = [ pose.position.x, pose.position.y - push_distance, pose.position.z ] graspquat = [0.00000, 0.00000, 0.70711, -0.70711] # attach object to gripper, they will move together obj = AttachedCollisionObject() obj.object.header.stamp = rospy.Time.now() obj.object.header.frame_id = GRIPPER_LINK_FRAME obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT obj.object.id = collision_object_name obj.link_name = GRIPPER_LINK_FRAME obj.touch_links = GRIPPER_LINKS self.attached_object_pub.publish(obj) # disable collisions between attached object and table collision_operation1 = CollisionOperation() collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS collision_operation1.object2 = collision_support_surface_name collision_operation1.operation = CollisionOperation.DISABLE collision_operation2 = CollisionOperation() collision_operation2.object1 = collision_support_surface_name collision_operation2.object2 = GRIPPER_GROUP_NAME collision_operation2.operation = CollisionOperation.DISABLE collision_operation2.penetration_distance = 0.02 ordered_collision_operations = OrderedCollisionOperations() ordered_collision_operations.collision_operations = [ collision_operation1, collision_operation2 ] res = self.check_cartesian_path_lists( approachpos, approachquat, grasppos, graspquat, start_angles, frame=frame_id, ordered_collision_operations=ordered_collision_operations) error_code_dict = { ArmNavigationErrorCodes.SUCCESS: 0, ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED: 1, ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED: 2, ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED: 3, ArmNavigationErrorCodes.PLANNING_FAILED: 4 } trajectory_len = len(res.trajectory.joint_trajectory.points) trajectory = [ res.trajectory.joint_trajectory.points[i].positions for i in range(trajectory_len) ] vels = [ res.trajectory.joint_trajectory.points[i].velocities for i in range(trajectory_len) ] times = [ res.trajectory.joint_trajectory.points[i].time_from_start for i in range(trajectory_len) ] error_codes = [ error_code_dict[error_code.val] for error_code in res.trajectory_error_codes ] # if even one code is not 0, abort if sum(error_codes) != 0: for ind in range(len(trajectory)): rospy.loginfo("error code " + str(error_codes[ind]) + " pos : " + self.pplist(trajectory[ind])) rospy.loginfo("") for ind in range(len(trajectory)): rospy.loginfo("time: " + "%5.3f " % times[ind].to_sec() + "vels: " + self.pplist(vels[ind])) rospy.logerr('%s: attempted push failed' % ACTION_NAME) self.action_server.set_aborted() return req = FilterJointTrajectoryRequest() req.trajectory = res.trajectory.joint_trajectory req.trajectory.points = req.trajectory.points[ 1:] # skip zero velocity point req.allowed_time = rospy.Duration(2.0) filt_res = self.trajectory_filter_srv(req) goal = FollowJointTrajectoryGoal() goal.trajectory = filt_res.trajectory goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.1) self.start_audio_recording_srv(InfomaxAction.PUSH, goal.category_id) rospy.sleep(0.5) self.trajectory_controller.send_goal(goal) self.trajectory_controller.wait_for_result() state = self.trajectory_controller.get_state() if state == GoalStatus.SUCCEEDED: rospy.sleep(0.5) sound_msg = self.stop_audio_recording_srv(True) self.action_server.set_succeeded( PushObjectResult(sound_msg.recorded_sound)) return rospy.logerr('%s: attempted push failed' % ACTION_NAME) self.stop_audio_recording_srv(False) self.action_server.set_aborted()
class RobbieHeadActionServer(): def __init__(self): # Initialize constants self.JOINTS_COUNT = 2 # Number of joints to manage self.ERROR_THRESHOLD = 0.02 # Report success if error reaches below threshold (0.015 also works) self.TIMEOUT_THRESHOLD = rospy.Duration(15.0) # Report failure if action does not succeed within timeout threshold # Initialize new node rospy.init_node(NAME, anonymous=True) # Initialize publisher & subscriber for pan self.head_pan_frame = 'head_pan_link' self.head_pan = JointControllerState(set_point=0.0, process_value=0.0, error=1.0) self.head_pan_pub = rospy.Publisher('head_pan_controller/command', Float64) rospy.Subscriber('head_pan_controller/state_pr2_msgs', JointControllerState, self.read_current_pan) rospy.wait_for_message('head_pan_controller/state_pr2_msgs', JointControllerState) # Initialize publisher & subscriber for tilt self.head_tilt_frame = 'head_tilt_link' self.head_tilt = JointControllerState(set_point=0.0, process_value=0.0, error=1.0) self.head_tilt_pub = rospy.Publisher('head_tilt_controller/command', Float64) rospy.Subscriber('head_tilt_controller/state_pr2_msgs', JointControllerState, self.read_current_tilt) rospy.wait_for_message('head_tilt_controller/state_pr2_msgs', JointControllerState) # Initialize tf listener self.tf = tf.TransformListener() # Initialize point action server self.result = RobbieHeadResult() self.feedback = RobbieHeadFeedback() self.feedback.head_position = [self.head_pan.process_value, self.head_tilt.process_value] self.server = SimpleActionServer(NAME, RobbieHeadAction, self.execute_callback, auto_start=False) # Reset head position rospy.sleep(1) self.reset_head_position() rospy.loginfo("%s: Ready to accept goals", NAME) def read_current_pan(self, pan_data): self.head_pan = pan_data self.has_latest_pan = True def read_current_tilt(self, tilt_data): self.head_tilt = tilt_data self.has_latest_tilt = True def reset_head_position(self): self.head_pan_pub.publish(0.0) self.head_tilt_pub.publish(0.0) rospy.sleep(5) def wait_for_latest_controller_states(self, timeout): self.has_latest_pan = False self.has_latest_tilt = False r = rospy.Rate(100) start = rospy.Time.now() while (self.has_latest_pan == False or self.has_latest_tilt == False) and (rospy.Time.now() - start < timeout): r.sleep() def transform_target_point(self, point): pan_target_frame = self.head_pan_frame tilt_target_frame = self.head_tilt_frame # Wait for tf info (time-out in 5 seconds) self.tf.waitForTransform(pan_target_frame, point.header.frame_id, rospy.Time(), rospy.Duration(5.0)) self.tf.waitForTransform(tilt_target_frame, point.header.frame_id, rospy.Time(), rospy.Duration(5.0)) # Transform target point to pan reference frame & retrieve the pan angle pan_target = self.tf.transformPoint(pan_target_frame, point) pan_angle = math.atan2(pan_target.point.y, pan_target.point.x) #rospy.loginfo("%s: Pan transformed to <%s, %s, %s> => angle %s", \ # NAME, pan_target.point.x, pan_target.point.y, pan_target.point.z, pan_angle) # Transform target point to tilt reference frame & retrieve the tilt angle tilt_target = self.tf.transformPoint(tilt_target_frame, point) tilt_angle = math.atan2(tilt_target.point.z, math.sqrt(math.pow(tilt_target.point.x, 2) + math.pow(tilt_target.point.y, 2))) #rospy.loginfo("%s: Tilt transformed to <%s, %s, %s> => angle %s", \ # NAME, tilt_target.point.x, tilt_target.point.y, tilt_target.point.z, tilt_angle) return [pan_angle, tilt_angle] def execute_callback(self, goal): r = rospy.Rate(100) self.result.success = True self.result.head_position = [self.head_pan.process_value, self.head_tilt.process_value] rospy.loginfo("%s: Executing move head", NAME) # Initialize target joints target_joints = list() for i in range(self.JOINTS_COUNT): target_joints.append(0.0) # Retrieve target joints from goal if (len(goal.target_joints) > 0): for i in range(min(len(goal.target_joints), len(target_joints))): target_joints[i] = goal.target_joints[i] else: try: # Try to convert target point to pan & tilt angles target_joints = self.transform_target_point(goal.target_point) except (tf.Exception, tf.ConnectivityException, tf.LookupException): rospy.loginfo("%s: Aborted: Transform Failure", NAME) self.result.success = False self.server.set_aborted() return # Publish goal command to controllers self.head_pan_pub.publish(target_joints[0]) self.head_tilt_pub.publish(target_joints[1]) # Initialize loop variables start_time = rospy.Time.now() while (math.fabs(target_joints[0] - self.head_pan.process_value) > self.ERROR_THRESHOLD or \ math.fabs(target_joints[1] - self.head_tilt.process_value) > self.ERROR_THRESHOLD): # Cancel exe if another goal was received (i.e. preempt requested) if self.server.is_preempt_requested(): rospy.loginfo("%s: Aborted: Action Preempted", NAME) self.result.success = False self.server.set_preempted() break # Publish current head position as feedback self.feedback.head_position = [self.head_pan.process_value, self.head_tilt.process_value] self.server.publish_feedback(self.feedback) # Abort if timeout current_time = rospy.Time.now() if (current_time - start_time > self.TIMEOUT_THRESHOLD): rospy.loginfo("%s: Aborted: Action Timeout", NAME) self.result.success = False self.server.set_aborted() break r.sleep() if (self.result.success): rospy.loginfo("%s: Goal Completed", NAME) self.wait_for_latest_controller_states(rospy.Duration(2.0)) self.result.head_position = [self.head_pan.process_value, self.head_tilt.process_value] self.server.set_succeeded(self.result)
class MockNavigation(): def __init__(self, move_base_topic): self.robot_pose_ = PoseStamped() self.listener = tf.TransformListener() self.navigation_succedes = True self.reply = False self.preempted = 0 self.moves_base = False self.target_position = Point() self.target_orientation = Quaternion() self.move_base_as_ = SimpleActionServer( move_base_topic, MoveBaseAction, execute_cb = self.move_base_cb, auto_start = False) self.move_base_as_.start() def __del__(self): self.move_base_as_.__del__() def move_base_cb(self, goal): rospy.loginfo('move_base_cb') self.target_position = goal.target_pose.pose.position self.target_orientation = goal.target_pose.pose.orientation self.moves_base = True while not self.reply: rospy.sleep(0.2) (trans, rot) = self.listener.lookupTransform('/map', '/base_footprint', rospy.Time(0)) self.robot_pose_.pose.position.x = trans[0] self.robot_pose_.pose.position.y = trans[1] feedback = MoveBaseFeedback() feedback.base_position.pose.position.x = \ self.robot_pose_.pose.position.x feedback.base_position.pose.position.y = \ self.robot_pose_.pose.position.y self.move_base_as_.publish_feedback(feedback) if self.move_base_as_.is_preempt_requested(): self.preempted += 1 rospy.loginfo("Preempted!") self.moves_base = False self.move_base_as_.set_preempted() return None if self.move_base_as_.is_new_goal_available(): self.preempted += 1 self.move_base_cb(self.move_base_as_.accept_new_goal()) return None else: result = MoveBaseResult() self.reply = False self.preempted = 0 self.moves_base = False self.target_position = Point() self.target_orientation = Quaternion() if self.navigation_succedes: self.move_base_as_.set_succeeded(result) else: self.move_base_as_.set_aborted(result)
class RobbieArmActionServer(): def __init__(self): # Initialize constants self.JOINTS_COUNT = 5 # Number of joints to manage self.ERROR_THRESHOLD = 0.15 # Report success if error reaches below threshold self.TIMEOUT_THRESHOLD = rospy.Duration(15.0) # Report failure if action does not succeed within timeout threshold # Initialize new node rospy.init_node(NAME + 'server', anonymous=True) # Initialize publisher & subscriber for shoulder pan self.shoulder_pan_frame = 'arm_shoulder_tilt_link' self.shoulder_pan = JointState(set_point=0.0, process_value=0.0, error=1.0) self.shoulder_pan_pub = rospy.Publisher('shoulder_pan_controller/command', Float64) rospy.Subscriber('shoulder_pan_controller/state', JointState, self.read_shoulder_pan) rospy.wait_for_message('shoulder_pan_controller/state', JointState) # Initialize publisher & subscriber for arm tilt self.arm_tilt_frame = 'arm_pan_tilt_bracket' self.arm_tilt = JointState(set_point=0.0, process_value=0.0, error=1.0) self.arm_tilt_pub = rospy.Publisher('arm_tilt_controller/command', Float64) rospy.Subscriber('arm_tilt_controller/state', JointState, self.read_arm_tilt) rospy.wait_for_message('arm_tilt_controller/state', JointState) # Initialize publisher & subscriber for elbow tilt self.elbow_tilt_frame = 'arm_bracket' #self.elbow_tilt = JointState(set_point=0.0, process_value=0.0, error=1.0) self.elbow_tilt_pub = rospy.Publisher('elbow_tilt_controller/command', Float64) rospy.Subscriber('elbow_tilt_controller/state', JointState, self.read_elbow_tilt) rospy.wait_for_message('elbow_tilt_controller/state', JointState) # Initialize publisher & subscriber for wrist pan self.wrist_pan_frame = 'wrist_pan_link' self.wrist_pan = JointState(set_point=0.0, process_value=0.0, error=1.0) self.wrist_pan_pub = rospy.Publisher('wrist_pan_controller/command', Float64) rospy.Subscriber('wrist_pan_controller/state', JointState, self.read_wrist_pan) rospy.wait_for_message('wrist_pan_controller/state', JointState) # Initialize publisher & subscriber for wrist tilt self.wrist_tilt_frame = 'wrist_tilt_link' self.wrist_tilt = JointState(set_point=0.0, process_value=0.0, error=1.0) self.wrist_tilt_pub = rospy.Publisher('wrist_tilt_controller/command', Float64) rospy.Subscriber('wrist_tilt_controller/state', JointState, self.read_wrist_tilt) rospy.wait_for_message('wrist_tilt_controller/state', JointState) # Initialize tf listener self.tf = tf.TransformListener() # Initialize joints action server self.result = RobbieArmResult() self.feedback = RobbieArmFeedback() self.feedback.arm_position = [self.shoulder_pan.process_value, self.arm_tilt.process_value, \ self.elbow_tilt.process_value, self.wrist_pan.process_value, self.wrist_tilt.process_value] self.server = SimpleActionServer(NAME, RobbieArmAction, self.execute_callback) # Reset arm position rospy.sleep(1) self.reset_arm_position() rospy.loginfo("%s: Ready to accept goals", NAME) def reset_arm_position(self): # reset arm to cobra position self.shoulder_pan_pub.publish(0.0) self.arm_tilt_pub.publish(1.572222) self.elbow_tilt_pub.publish(-1.572222) self.wrist_pan_pub.publish(0.0) self.wrist_tilt_pub.publish(0.0) rospy.sleep(12) def read_shoulder_pan(self, pan_data): self.shoulder_pan = pan_data self.has_latest_shoulder_pan = True def read_arm_tilt(self, tilt_data): self.arm_tilt = tilt_data self.has_latest_arm_tilt = True def read_elbow_tilt(self, tilt_data): self.elbow_tilt = tilt_data self.has_latest_elbow_tilt = True def read_wrist_pan(self, rotate_data): self.wrist_pan = rotate_data self.has_latest_wrist_pan = True def read_wrist_tilt(self, rotate_data): self.wrist_tilt = rotate_data self.has_latest_wrist_tilt = True def wait_for_latest_controller_states(self, timeout): self.has_latest_shoulder_pan = False self.has_latest_arm_tilt = False self.has_latest_elbow_tilt = False self.has_latest_wrist_pan = False self.has_latest_wrist_tilt = False r = rospy.Rate(100) start = rospy.Time.now() while (self.has_latest_shoulder_pan == False or self.has_latest_arm_tilt == False or \ self.has_latest_elbow_tilt == False or self.has_latest_wrist_tilt == False or self.has_latest_wrist_pan == False) and \ (rospy.Time.now() - start < timeout): r.sleep() def transform_target_point(self, point): rospy.loginfo("%s: Retrieving IK solutions", NAME) rospy.wait_for_service('smart_arm_ik_service', 10) ik_service = rospy.ServiceProxy('smart_arm_ik_service', SmartArmIK) resp = ik_service(point) if (resp and resp.success): return resp.solutions[0:4] else: raise Exception, "Unable to obtain IK solutions." def execute_callback(self, goal): r = rospy.Rate(100) self.result.success = True self.result.arm_position = [self.shoulder_pan.process_value, self.arm_tilt.process_value, \ self.elbow_tilt.process_value, self.wrist_pan.process_value, self.wrist_tilt.process_value] rospy.loginfo("%s: Executing move arm", NAME) # Initialize target joints target_joints = list() for i in range(self.JOINTS_COUNT): target_joints.append(0.0) # Retrieve target joints from goal if (len(goal.target_joints) > 0): for i in range(min(len(goal.target_joints), len(target_joints))): target_joints[i] = goal.target_joints[i] else: try: # Convert target point to target joints (find an IK solution) target_joints = self.transform_target_point(goal.target_point) except (Exception, tf.Exception, tf.ConnectivityException, tf.LookupException): rospy.loginfo("%s: Aborted: IK Transform Failure", NAME) self.result.success = False self.server.set_aborted() return # Publish goal to controllers self.shoulder_pan_pub.publish(target_joints[0]) self.arm_tilt_pub.publish(target_joints[1]) self.elbow_tilt_pub.publish(target_joints[2]) self.wrist_pan_pub.publish(target_joints[3]) self.wrist_tilt_pub.publish(target_joints[4]) # Initialize loop variables start_time = rospy.Time.now() while (math.fabs(target_joints[0] - self.shoulder_pan.process_value) > self.ERROR_THRESHOLD or \ math.fabs(target_joints[1] - self.arm_tilt.process_value) > self.ERROR_THRESHOLD or \ math.fabs(target_joints[2] - self.elbow_tilt.process_value) > self.ERROR_THRESHOLD or \ math.fabs(target_joints[3] - self.wrist_pan.process_value) > self.ERROR_THRESHOLD or \ math.fabs(target_joints[4] - self.wrist_tilt.process_value) > self.ERROR_THRESHOLD): # Cancel exe if another goal was received (i.e. preempt requested) if self.server.is_preempt_requested(): rospy.loginfo("%s: Aborted: Action Preempted", NAME) self.result.success = False self.server.set_preempted() break # Publish current arm position as feedback self.feedback.arm_position = [self.shoulder_pan.process_value, self.arm_tilt.process_value, \ self.elbow_tilt.process_value, self.wrist_pan.process_value, self.wrist_tilt.process_value] self.server.publish_feedback(self.feedback) # Abort if timeout current_time = rospy.Time.now() if (current_time - start_time > self.TIMEOUT_THRESHOLD): rospy.loginfo("%s: Aborted: Action Timeout", NAME) self.result.success = False self.server.set_aborted() break r.sleep() if (self.result.success): rospy.loginfo("%s: Goal Completed", NAME) self.wait_for_latest_controller_states(rospy.Duration(2.0)) self.result.arm_position = [self.shoulder_pan.process_value, self.arm_tilt.process_value, \ self.elbow_tilt.process_value, self.wrist_pan.process_value, self.wrist_tilt.process_value] self.server.set_succeeded(self.result)
class PushObjectActionServer: def __init__(self): self.start_audio_recording_srv = rospy.ServiceProxy("/audio_dump/start_audio_recording", StartAudioRecording) self.stop_audio_recording_srv = rospy.ServiceProxy("/audio_dump/stop_audio_recording", StopAudioRecording) self.get_grasp_status_srv = rospy.ServiceProxy("/wubble_grasp_status", GraspStatus) self.interpolated_ik_params_srv = rospy.ServiceProxy( "/l_interpolated_ik_motion_plan_set_params", SetInterpolatedIKMotionPlanParams ) self.interpolated_ik_srv = rospy.ServiceProxy("/l_interpolated_ik_motion_plan", GetMotionPlan) self.set_planning_scene_diff_client = rospy.ServiceProxy( "/environment_server/set_planning_scene_diff", SetPlanningSceneDiff ) self.get_fk_srv = rospy.ServiceProxy("/wubble2_left_arm_kinematics/get_fk", GetPositionFK) self.trajectory_filter_srv = rospy.ServiceProxy( "/trajectory_filter_unnormalizer/filter_trajectory", FilterJointTrajectory ) self.trajectory_controller = SimpleActionClient( "/l_arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction ) self.attached_object_pub = rospy.Publisher("/attached_collision_object", AttachedCollisionObject) self.action_server = SimpleActionServer( ACTION_NAME, PushObjectAction, execute_cb=self.push_object, auto_start=False ) def initialize(self): rospy.loginfo("%s: waiting for audio_dump/start_audio_recording service" % ACTION_NAME) rospy.wait_for_service("audio_dump/start_audio_recording") rospy.loginfo("%s: connected to audio_dump/start_audio_recording service" % ACTION_NAME) rospy.loginfo("%s: waiting for audio_dump/stop_audio_recording service" % ACTION_NAME) rospy.wait_for_service("audio_dump/stop_audio_recording") rospy.loginfo("%s: connected to audio_dump/stop_audio_recording service" % ACTION_NAME) rospy.loginfo("%s: waiting for l_interpolated_ik_motion_plan_set_params service" % ACTION_NAME) rospy.wait_for_service("/l_interpolated_ik_motion_plan_set_params") rospy.loginfo("%s: connected to l_interpolated_ik_motion_plan_set_params service" % ACTION_NAME) rospy.loginfo("%s: waiting for l_interpolated_ik_motion_plan service" % ACTION_NAME) rospy.wait_for_service("/l_interpolated_ik_motion_plan") rospy.loginfo("%s: connected to l_interpolated_ik_motion_plan service" % ACTION_NAME) rospy.loginfo("%s: waiting for environment_server/set_planning_scene_diff service" % ACTION_NAME) rospy.wait_for_service("/environment_server/set_planning_scene_diff") rospy.loginfo("%s: connected to set_planning_scene_diff service" % ACTION_NAME) rospy.loginfo("%s: waiting for wubble2_left_arm_kinematics/get_fk service" % ACTION_NAME) rospy.wait_for_service("/wubble2_left_arm_kinematics/get_fk") rospy.loginfo("%s: connected to wubble2_left_arm_kinematics/get_fk service" % ACTION_NAME) rospy.loginfo("%s: waiting for trajectory_filter_unnormalizer/filter_trajectory service" % ACTION_NAME) rospy.wait_for_service("/trajectory_filter_unnormalizer/filter_trajectory") rospy.loginfo("%s: connected to trajectory_filter_unnormalizer/filter_trajectory service" % ACTION_NAME) rospy.loginfo("%s: waiting for l_arm_controller/follow_joint_trajectory" % ACTION_NAME) self.trajectory_controller.wait_for_server() rospy.loginfo("%s: connected to l_arm_controller/follow_joint_trajectory" % ACTION_NAME) self.action_server.start() def create_pose_stamped(self, pose, frame_id="base_link"): """ Creates a PoseStamped message from a list of 7 numbers (first three are position and next four are orientation: pose = [px,py,pz, ox,oy,oz,ow] """ m = PoseStamped() m.header.frame_id = frame_id m.header.stamp = rospy.Time() m.pose.position = Point(*pose[0:3]) m.pose.orientation = Quaternion(*pose[3:7]) return m # pretty-print list to string def pplist(self, list_to_print): return " ".join(["%5.3f" % x for x in list_to_print]) def get_interpolated_ik_motion_plan( self, start_pose, goal_pose, start_angles, joint_names, pos_spacing=0.01, rot_spacing=0.1, consistent_angle=math.pi / 9, collision_aware=True, collision_check_resolution=1, steps_before_abort=-1, num_steps=0, ordered_collision_operations=None, frame="base_footprint", start_from_end=0, max_joint_vels=[1.5] * 7, max_joint_accs=[8.0] * 7, ): res = self.interpolated_ik_params_srv( num_steps, consistent_angle, collision_check_resolution, steps_before_abort, pos_spacing, rot_spacing, collision_aware, start_from_end, max_joint_vels, max_joint_accs, ) req = GetMotionPlanRequest() req.motion_plan_request.start_state.joint_state.name = joint_names req.motion_plan_request.start_state.joint_state.position = start_angles req.motion_plan_request.start_state.multi_dof_joint_state.poses = [start_pose.pose] req.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids = [GRIPPER_LINK_FRAME] req.motion_plan_request.start_state.multi_dof_joint_state.frame_ids = [start_pose.header.frame_id] pos_constraint = PositionConstraint() pos_constraint.position = goal_pose.pose.position pos_constraint.header.frame_id = goal_pose.header.frame_id req.motion_plan_request.goal_constraints.position_constraints = [pos_constraint] orient_constraint = OrientationConstraint() orient_constraint.orientation = goal_pose.pose.orientation orient_constraint.header.frame_id = goal_pose.header.frame_id req.motion_plan_request.goal_constraints.orientation_constraints = [orient_constraint] # req.motion_plan_request.link_padding = [LinkPadding(l,0.0) for l in GRIPPER_LINKS] # req.motion_plan_request.link_padding.extend([LinkPadding(l,0.0) for l in ARM_LINKS]) # if ordered_collision_operations is not None: # req.motion_plan_request.ordered_collision_operations = ordered_collision_operations res = self.interpolated_ik_srv(req) return res def check_cartesian_path_lists( self, approachpos, approachquat, grasppos, graspquat, start_angles, pos_spacing=0.03, rot_spacing=0.1, consistent_angle=math.pi / 7.0, collision_aware=True, collision_check_resolution=1, steps_before_abort=1, num_steps=0, ordered_collision_operations=None, frame="base_link", ): start_pose = self.create_pose_stamped(approachpos + approachquat, frame) goal_pose = self.create_pose_stamped(grasppos + graspquat, frame) return self.get_interpolated_ik_motion_plan( start_pose, goal_pose, start_angles, ARM_JOINTS, pos_spacing, rot_spacing, consistent_angle, collision_aware, collision_check_resolution, steps_before_abort, num_steps, ordered_collision_operations, frame, ) def push_object(self, goal): if self.action_server.is_preempt_requested(): rospy.loginfo("%s: preempted" % ACTION_NAME) self.action_server.set_preempted() collision_object_name = goal.collision_object_name collision_support_surface_name = goal.collision_support_surface_name current_state = rospy.wait_for_message("l_arm_controller/state", FollowJointTrajectoryFeedback) start_angles = current_state.actual.positions full_state = rospy.wait_for_message("/joint_states", JointState) req = GetPositionFKRequest() req.header.frame_id = "base_link" req.fk_link_names = [GRIPPER_LINK_FRAME] req.robot_state.joint_state = full_state if not self.set_planning_scene_diff_client(): rospy.logerr("%s: failed to set planning scene diff" % ACTION_NAME) self.action_server.set_aborted() return pose = self.get_fk_srv(req).pose_stamped[0].pose frame_id = "base_link" approachpos = [pose.position.x, pose.position.y, pose.position.z] approachquat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] push_distance = 0.40 grasppos = [pose.position.x, pose.position.y - push_distance, pose.position.z] graspquat = [0.00000, 0.00000, 0.70711, -0.70711] # attach object to gripper, they will move together obj = AttachedCollisionObject() obj.object.header.stamp = rospy.Time.now() obj.object.header.frame_id = GRIPPER_LINK_FRAME obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT obj.object.id = collision_object_name obj.link_name = GRIPPER_LINK_FRAME obj.touch_links = GRIPPER_LINKS self.attached_object_pub.publish(obj) # disable collisions between attached object and table collision_operation1 = CollisionOperation() collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS collision_operation1.object2 = collision_support_surface_name collision_operation1.operation = CollisionOperation.DISABLE collision_operation2 = CollisionOperation() collision_operation2.object1 = collision_support_surface_name collision_operation2.object2 = GRIPPER_GROUP_NAME collision_operation2.operation = CollisionOperation.DISABLE collision_operation2.penetration_distance = 0.02 ordered_collision_operations = OrderedCollisionOperations() ordered_collision_operations.collision_operations = [collision_operation1, collision_operation2] res = self.check_cartesian_path_lists( approachpos, approachquat, grasppos, graspquat, start_angles, frame=frame_id, ordered_collision_operations=ordered_collision_operations, ) error_code_dict = { ArmNavigationErrorCodes.SUCCESS: 0, ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED: 1, ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED: 2, ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED: 3, ArmNavigationErrorCodes.PLANNING_FAILED: 4, } trajectory_len = len(res.trajectory.joint_trajectory.points) trajectory = [res.trajectory.joint_trajectory.points[i].positions for i in range(trajectory_len)] vels = [res.trajectory.joint_trajectory.points[i].velocities for i in range(trajectory_len)] times = [res.trajectory.joint_trajectory.points[i].time_from_start for i in range(trajectory_len)] error_codes = [error_code_dict[error_code.val] for error_code in res.trajectory_error_codes] # if even one code is not 0, abort if sum(error_codes) != 0: for ind in range(len(trajectory)): rospy.loginfo("error code " + str(error_codes[ind]) + " pos : " + self.pplist(trajectory[ind])) rospy.loginfo("") for ind in range(len(trajectory)): rospy.loginfo("time: " + "%5.3f " % times[ind].to_sec() + "vels: " + self.pplist(vels[ind])) rospy.logerr("%s: attempted push failed" % ACTION_NAME) self.action_server.set_aborted() return req = FilterJointTrajectoryRequest() req.trajectory = res.trajectory.joint_trajectory req.trajectory.points = req.trajectory.points[1:] # skip zero velocity point req.allowed_time = rospy.Duration(2.0) filt_res = self.trajectory_filter_srv(req) goal = FollowJointTrajectoryGoal() goal.trajectory = filt_res.trajectory goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.1) self.start_audio_recording_srv(InfomaxAction.PUSH, goal.category_id) rospy.sleep(0.5) self.trajectory_controller.send_goal(goal) self.trajectory_controller.wait_for_result() state = self.trajectory_controller.get_state() if state == GoalStatus.SUCCEEDED: rospy.sleep(0.5) sound_msg = self.stop_audio_recording_srv(True) self.action_server.set_succeeded(PushObjectResult(sound_msg.recorded_sound)) return rospy.logerr("%s: attempted push failed" % ACTION_NAME) self.stop_audio_recording_srv(False) self.action_server.set_aborted()
class move_base_fake_node: def __init__(self): self.action_server = SimpleActionServer( 'move_base', MoveBaseAction, execute_cb=self.execute_callback, auto_start=False ) # Simple action server will pretend to be move_base # Movement goal state self.goal = None # Is a goal set self.valid_goal = False # Is this a valid goal self.current_goal_started = False # Has the goal been started (i.e. have we told our Bug algorithm to use this point and start) self.current_goal_complete = False # Has the Bug algorithm told us it completed self.position = None # move_base feedback reports the current direction ## TO DO!! # Need a service provided by this node or something for the Bug algorithm to tell us it is done # Bug service to start and stop Bug algorithm # Bug service to set a new goal in Bug algorithm # rospy.wait_for_service() # rospy.wait_for_service() self.subscriber_odometry = rospy.Subscriber( 'odom/', Odometry, self.callback_odometry ) # We need to read the robots current point for the feedback self.subscriber_simple_goal = rospy.Subscriber( '/move_base_simple/goal/', PoseStamped, self.callback_simple_goal ) # Our return goal is done with /move_base_simple/goal/ self.goal_pub = rospy.Publisher( '/move_base/goal/', MoveBaseActionGoal, queue_size=10) # /move_base_simple/goal/ gets published here self.action_server.start() def execute_callback(self, move_base_goal): self.goal = move_base_goal.target_pose.pose # Set the provided goal as the current goal rospy.logdebug('[Move Base Fake] Execute Callback: {}'.format( str(self.goal.position))) self.valid_goal = True # Assume it is valid self.current_goal_started = False # It hasnt started yet self.current_goal_complete = False # It hasnt been completed r = rospy.Rate(1) while not rospy.is_shutdown(): # Always start by checking if there is a new goal that preempts the current one if self.action_server.is_preempt_requested(): ## TO DO!! # Tell Bug algorithm to stop before changing or stopping goal if self.action_server.is_new_goal_available(): new_goal = self.action_server.accept_new_goal( ) # There is a new goal rospy.logdebug('[Move Base Fake] New Goal: {}'.format( str(self.goal.position))) self.goal = new_goal.target_pose.pose # Set the provided goal as the current goal self.valid_goal = True # Assume it is valid self.current_goal_started = False # It hasnt started yet self.current_goal_complete = False # It hasnt been completed else: self.action_server.set_preempted( ) # No new goal, we've just been told to stop self.goal = None # Stop everything self.valid_goal = False self.current_goal_started = False self.current_goal_complete = False return # Start goal if self.valid_goal and not self.current_goal_started: rospy.logdebug('[Move Base Fake] Starting Goal') ## TO DO !! # Call the Bug services/topics etc to tell Bug algorithm new target point and then to start self.current_goal_started = True # Only start once # Feedback ever loop just reports current location feedback = MoveBaseFeedback() feedback.base_position.pose.position = self.position self.action_server.publish_feedback(feedback) # Completed is set in a callback that you need to link to a service or subscriber if self.current_goal_complete: rospy.logdebug('[Move Base Fake] Finishing Goal') ## TO DO!! # Tell Bug algorithm to stop before changing or stopping goal self.goal = None # Stop everything self.valid_goal = False self.current_goal_started = False self.current_goal_complete = False self.action_server.set_succeeded( MoveBaseResult(), 'Goal reached') # Send success message return r.sleep() # Shutdown rospy.logdebug('[Move Base Fake] Shutting Down') ## TO DO!! # Tell Bug algorithm to stop before changing or stopping goal self.goal = None # Stop everything self.valid_goal = False self.current_goal_started = False self.current_goal_complete = False # you need to connect this to something being called/published from the Bug algorithm def callback_complete(self, success): # TO DO!! # Implement some kind of service or subscriber so the Bug algorithm can tell this node it is complete self.current_goal_complete = success.data def callback_odometry(self, odom): self.position = odom.pose.pose.position # Simple goals get republished to the correct topic def callback_simple_goal(self, goal): rospy.logdebug('[Move Base Fake] Simple Goal: {}'.format( str(goal.pose.position))) action_goal = MoveBaseActionGoal() action_goal.header.stamp = rospy.Time.now() action_goal.goal.target_pose = goal self.goal_pub.publish(action_goal)
class LiftObjectActionServer: def __init__(self): self.get_grasp_status_srv = rospy.ServiceProxy("/wubble_grasp_status", GraspStatus) self.start_audio_recording_srv = rospy.ServiceProxy("/audio_dump/start_audio_recording", StartAudioRecording) self.stop_audio_recording_srv = rospy.ServiceProxy("/audio_dump/stop_audio_recording", StopAudioRecording) self.posture_controller = SimpleActionClient("/wubble_gripper_grasp_action", GraspHandPostureExecutionAction) self.move_arm_client = SimpleActionClient("/move_left_arm", MoveArmAction) self.attached_object_pub = rospy.Publisher("/attached_collision_object", AttachedCollisionObject) self.action_server = SimpleActionServer( ACTION_NAME, LiftObjectAction, execute_cb=self.lift_object, auto_start=False ) def initialize(self): rospy.loginfo("%s: waiting for wubble_grasp_status service" % ACTION_NAME) rospy.wait_for_service("/wubble_grasp_status") rospy.loginfo("%s: connected to wubble_grasp_status service" % ACTION_NAME) rospy.loginfo("%s: waiting for wubble_gripper_grasp_action" % ACTION_NAME) self.posture_controller.wait_for_server() rospy.loginfo("%s: connected to wubble_gripper_grasp_action" % ACTION_NAME) rospy.loginfo("%s: waiting for audio_dump/start_audio_recording service" % ACTION_NAME) rospy.wait_for_service("audio_dump/start_audio_recording") rospy.loginfo("%s: connected to audio_dump/start_audio_recording service" % ACTION_NAME) rospy.loginfo("%s: waiting for audio_dump/stop_audio_recording service" % ACTION_NAME) rospy.wait_for_service("audio_dump/stop_audio_recording") rospy.loginfo("%s: connected to audio_dump/stop_audio_recording service" % ACTION_NAME) rospy.loginfo("%s: waiting for move_left_arm action server" % ACTION_NAME) self.move_arm_client.wait_for_server() rospy.loginfo("%s: connected to move_left_arm action server" % ACTION_NAME) self.action_server.start() def open_gripper(self): pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.RELEASE self.posture_controller.send_goal(pg) self.posture_controller.wait_for_result() def lift_object(self, goal): if self.action_server.is_preempt_requested(): rospy.loginfo("%s: preempted" % ACTION_NAME) self.action_server.set_preempted() collision_support_surface_name = goal.collision_support_surface_name # disable collisions between grasped object and table collision_operation1 = CollisionOperation() collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS collision_operation1.object2 = collision_support_surface_name collision_operation1.operation = CollisionOperation.DISABLE # disable collisions between gripper and table collision_operation2 = CollisionOperation() collision_operation2.object1 = GRIPPER_GROUP_NAME collision_operation2.object2 = collision_support_surface_name collision_operation2.operation = CollisionOperation.DISABLE ordered_collision_operations = OrderedCollisionOperations() ordered_collision_operations.collision_operations = [collision_operation1, collision_operation2] gripper_paddings = [LinkPadding(l, 0.0) for l in GRIPPER_LINKS] # this is a hack to make arm lift an object faster obj = AttachedCollisionObject() obj.object.header.stamp = rospy.Time.now() obj.object.header.frame_id = GRIPPER_LINK_FRAME obj.object.operation.operation = CollisionObjectOperation.REMOVE obj.object.id = collision_support_surface_name obj.link_name = GRIPPER_LINK_FRAME obj.touch_links = GRIPPER_LINKS self.attached_object_pub.publish(obj) current_state = rospy.wait_for_message("l_arm_controller/state", FollowJointTrajectoryFeedback) joint_names = current_state.joint_names joint_positions = current_state.actual.positions start_angles = [joint_positions[joint_names.index(jn)] for jn in ARM_JOINTS] start_angles[0] = start_angles[0] - 0.3 # move shoulder up a bit if not move_arm_joint_goal( self.move_arm_client, ARM_JOINTS, start_angles, link_padding=gripper_paddings, collision_operations=ordered_collision_operations, ): self.action_server.set_aborted() return self.start_audio_recording_srv(InfomaxAction.LIFT, goal.category_id) if move_arm_joint_goal( self.move_arm_client, ARM_JOINTS, LIFT_POSITION, link_padding=gripper_paddings, collision_operations=ordered_collision_operations, ): # check if are still holding an object after lift is done grasp_status = self.get_grasp_status_srv() # if the object is still in hand after lift is done we are good if grasp_status.is_hand_occupied: sound_msg = self.stop_audio_recording_srv(True) self.action_server.set_succeeded(LiftObjectResult(sound_msg.recorded_sound)) return self.stop_audio_recording_srv(False) rospy.logerr("%s: attempted lift failed" % ACTION_NAME) self.action_server.set_aborted() return
class MotionService(object): def __init__(self): rospy.init_node('motion_service') # Load config config_loader = RobotConfigLoader() try: robot_config_name = rospy.get_param(rospy.get_name() + '/robot_config') except KeyError: rospy.logwarn('Could not find robot config param in /' + rospy.get_name +'/robot_config. Using default config for ' 'Thor Mang.') robot_config_name = 'thor' config_loader.load_xml_by_name(robot_config_name + '_config.xml') # Create publisher for first target if len(config_loader.targets) > 0: self._motion_publisher = MotionPublisher( config_loader.robot_config, config_loader.targets[0].joint_state_topic, config_loader.targets[0].publisher_prefix) else: rospy.logerr('Robot config needs to contain at least one valid target.') self._motion_data = MotionData(config_loader.robot_config) # Subscriber to start motions via topic self.motion_command_sub = rospy.Subscriber('motion_command', ExecuteMotionCommand, self.motion_command_request_cb) # Create action server self._action_server = SimpleActionServer(rospy.get_name() + '/motion_goal', ExecuteMotionAction, None, False) self._action_server.register_goal_callback(self._execute_motion_goal) self._action_server.register_preempt_callback(self._preempt_motion_goal) self._preempted = False def _execute_motion_goal(self): goal = self._action_server.accept_new_goal() # Check if motion exists if goal.motion_name not in self._motion_data: result = ExecuteMotionResult() result.error_code = [ExecuteMotionResult.MOTION_UNKNOWN] result.error_string = "The requested motion is unknown." self._action_server.set_aborted(result) return # check if a valid time_factor is set, otherwise default to 1.0 if goal.time_factor <= 0.0: goal.time_factor = 1.0 self._preempted = False self._motion_publisher.publish_motion(self._motion_data[goal.motion_name], goal.time_factor, self._trajectory_finished) print '[MotionService] New action goal received.' def _preempt_motion_goal(self): self._motion_publisher.stop_motion() print '[MotionService] Preempting goal.' self._preempted = True def _trajectory_finished(self, error_codes, error_groups): result = ExecuteMotionResult() result.error_code = error_codes error_string = "" for error_code, error_group in zip(error_codes, error_groups): error_string += error_group + ': ' + str(error_code) + '\n' result.error_string = error_string if self._preempted: self._action_server.set_preempted(result) else: self._action_server.set_succeeded(result) print '[MotionService] Trajectory finished with error code: \n', error_string def _execute_motion(self, request): response = ExecuteMotionResponse() # check if a motion with this name exists if request.motion_name not in self._motion_data: print '[MotionService] Unknown motion name: "%s"' % request.motion_name response.ok = False response.finish_time.data = rospy.Time.now() return response # check if a valid time_factor is set, otherwise default to 1.0 if request.time_factor <= 0.0: request.time_factor = 1.0 # find the duration of the requested motion motion_duration = 0.0 for poses in self._motion_data[request.motion_name].values(): if len(poses) > 0: endtime = poses[-1]['starttime'] + poses[-1]['duration'] motion_duration = max(motion_duration, endtime) motion_duration *= request.time_factor # execute motion print '[MotionService] Executing motion: "%s" (time factor: %.3f, duration %.2fs)' % (request.motion_name, request.time_factor, motion_duration) self._motion_publisher.publish_motion(self._motion_data[request.motion_name], request.time_factor) # reply with ok and the finish_time of this motion response.ok = True response.finish_time.data = rospy.Time.now() + rospy.Duration.from_sec(motion_duration) return response def start_server(self): rospy.Service(rospy.get_name() + '/execute_motion', ExecuteMotion, self._execute_motion) self._action_server.start() print '[MotionService] Waiting for calls...' rospy.spin() def motion_command_request_cb(self, command): print "[MotionService] Initiate motion command via topic ..." request = ExecuteMotion() request.motion_name = command.motion_name request.time_factor = command.time_factor self._execute_motion(request) print "[MotionService] Motion command complete"