def execute(self, userdata): if self.save_sm_state: self.save_current_state() goal = MoveBaseGoal() for destination_location in self.destination_locations: goal.destination_location = destination_location # calling the actionlib server and waiting for the execution to end rospy.loginfo( '[move_base] Sending action lib goal to move_base_server,' + ' destination: ' + goal.destination_location) self.say('Going to ' + goal.destination_location) self.client.send_goal(goal) self.client.wait_for_result( rospy.Duration.from_sec(int(self.timeout))) success = self.client.get_result() if success: rospy.loginfo('Successfully reached %s' % destination_location) else: rospy.logerr('Could not reach %s' % destination_location) self.say('Could not reach ' + goal.destination_location) if self.retry_count == self.number_of_retries: self.say('Aborting operation') return 'failed_after_retrying' self.retry_count += 1 return 'failed' return 'succeeded'
def __align_base_with_pose(self, pose_base_link): '''Moves the base so that the elbow is aligned with the goal pose. Keyword arguments: pose_base_link -- a 'geometry_msgs/PoseStamped' message representing the goal pose in the base link frame ''' aligned_base_pose = PoseStamped() aligned_base_pose.header.frame_id = 'base_link' aligned_base_pose.header.stamp = rospy.Time.now() aligned_base_pose.pose.position.x = 0. aligned_base_pose.pose.position.y = pose_base_link.pose.position.y - self.base_elbow_offset aligned_base_pose.pose.position.z = 0. aligned_base_pose.pose.orientation.x = 0. aligned_base_pose.pose.orientation.y = 0. aligned_base_pose.pose.orientation.z = 0. aligned_base_pose.pose.orientation.w = 1. move_base_goal = MoveBaseGoal() move_base_goal.goal_type = MoveBaseGoal.POSE move_base_goal.pose = aligned_base_pose self.move_base_client.send_goal(move_base_goal) self.move_base_client.wait_for_result() self.move_base_client.get_result()
def execute(self, userdata): goal = MoveBaseGoal() goal.goal_type = MoveBaseGoal.POSE goal.pose.header.frame_id = self.rotation_frame q = quaternion_from_euler(0, 0, userdata.turn_base_to_goal.desired_yaw) goal.pose.pose.orientation.x = q[0] goal.pose.pose.orientation.y = q[1] goal.pose.pose.orientation.z = q[2] goal.pose.pose.orientation.w = q[3] rospy.loginfo("[mdr_turn_base_to] Goal %s", goal) self.move_base_client.send_goal(goal) success = self.move_base_client.wait_for_result() if success: return 'succeeded' return 'failed'
def execute(self, userdata): unknown_people = self.kb_interface.get_all_attributes('unknown') people_identifiers = [] for item in unknown_people: if not item.is_negative: for param in item.values: if param.key == 'person': people_identifiers.append(param.value) print(people_identifiers) if not people_identifiers: print('[move_to_person] No people found; aborting operation') return 'failed_after_retrying' person_to_interview = people_identifiers[0] person_info = self.kb_interface.get_obj_instance( person_to_interview, Person._type) person_pose = MoveToPerson.subtract_distance_from_pose( person_info.pose, 1) move_base_goal = MoveBaseGoal() move_base_goal.goal_type = MoveBaseGoal.POSE move_base_goal.pose = person_pose rospy.loginfo('[move_to_person] Going to person ' + person_to_interview + ' at pose ' + str(person_pose.pose.position.x) + ' ' + str(person_pose.pose.position.y)) self.move_base_client.send_goal(move_base_goal) self.move_base_client.wait_for_result( rospy.Duration.from_sec(int(self.move_base_timeout))) result = self.move_base_client.get_result() if result and result.success: return 'succeeded' if self.retry_count == self.number_of_retries: rospy.loginfo('[move_to_person] Could not go to %s', person_to_interview) self.say('[move_to_person] Could not go to %s; aborting operation', person_to_interview) return 'failed_after_retrying' rospy.logerr('[move_to_person] Could not go to %s; retrying', person_to_interview) self.retry_count += 1 return 'failed'
def execute(self, userdata): destination_locations = None if self.destination_locations is None: destination_locations = list(userdata.destination_locations) rospy.loginfo("Using userdata's destination_locations %s", destination_locations) else: destination_locations = list(self.destination_locations) rospy.loginfo("Using predefined destination_locations %s", destination_locations) client = actionlib.SimpleActionClient(self.action_server_name, MoveBaseAction) client.wait_for_server() for destination_location in destination_locations: goal = MoveBaseGoal() goal.goal_type = MoveBaseGoal.NAMED_TARGET goal.destination_location = destination_location client.send_goal(goal) client.wait_for_result(rospy.Duration.from_sec(int(self.timeout))) result = client.get_result() if result: if result.success: rospy.loginfo('[%s] Successfully moved to %s', self.state_name, destination_location) else: rospy.logerr('[%s] Could not move to %s', self.state_name, destination_location) if self.retry_count == self.number_of_retries: rospy.logerr( '[%s] Could not move to %s even after retrying; giving up', self.state_name, destination_location) self.retry_count = 0 return 'failed_after_retrying' self.retry_count += 1 return 'failed' else: rospy.logerr( '[%s] Could not move to %s within %f seconds; giving up', self.state_name, destination_location, self.timeout) client.cancel_all_goals() return 'succeeded'
def running(self): goal = MoveBaseGoal() goal.goal_type = MoveBaseGoal.POSE goal.pose.header.frame_id = self.rotation_frame q = quaternion_from_euler(0, 0, self.goal.desired_yaw) goal.pose.pose.orientation.x = q[0] goal.pose.pose.orientation.y = q[1] goal.pose.pose.orientation.z = q[2] goal.pose.pose.orientation.w = q[3] rospy.loginfo('[turn_base_to] Rotating the base to %s', str(self.goal.desired_yaw)) self.move_base_client.send_goal(goal) success = self.move_base_client.wait_for_result() if success: self.result = self.set_result(True) self.result = self.set_result(False) return FTSMTransitions.DONE
def execute(self, userdata): if self.save_sm_state: self.save_current_state() # if the current location of the robot was not specified # in the request, we query this information from the knowledge base original_location = '' request = rosplan_srvs.GetAttributeServiceRequest() request.predicate_name = 'robot_at' result = self.attribute_fetching_client(request) for item in result.attributes: if not item.is_negative: for param in item.values: if param.key == 'bot' and param.value != self.robot_name: break if param.key == 'wp': original_location = param.value break break for destination_location in self.destination_locations: if destination_location.lower().find('table') != -1: self.say('Moving to table') table_pose = self.get_table_pose() if table_pose is None: rospy.loginfo('Could not get table pose') self.say('Could not move to table') return 'failed' move_base_goal = MoveBaseGoal() move_base_goal.goal_type = MoveBaseGoal.POSE move_base_goal.pose = table_pose self.move_base_client.send_goal(move_base_goal) self.move_base_client.wait_for_result() result = self.move_base_client.get_result() if result: return 'succeeded' else: dispatch_msg = self.get_dispatch_msg(original_location, destination_location) rospy.loginfo('Sending the base to %s' % destination_location) self.say('Going to ' + destination_location) self.action_dispatch_pub.publish(dispatch_msg) self.executing = True self.succeeded = False start_time = time.time() duration = 0. while self.executing and duration < self.timeout: rospy.sleep(0.1) duration = time.time() - start_time if self.succeeded: rospy.loginfo('Successfully reached %s' % destination_location) original_location = destination_location else: rospy.logerr('Could not reach %s' % destination_location) self.say('Could not reach ' + destination_location) if self.retry_count == self.number_of_retries: self.say('Aborting operation') return 'failed_after_retrying' self.retry_count += 1 return 'failed' return 'succeeded'
def running(self): hand_over_pose = PoseStamped() hand_over_pose.header.frame_id = 'base_link' if self.goal.context_aware: # > Pick context-dependent hand-over position: policy_parameter_a = self.hand_over_position_policy_parameters[0][ 0] policy_parameter_A = self.hand_over_position_policy_parameters[0][ 1] if self.goal.posture_type == 'standing': context_vector = np.array([0.0, 0.0, 0.4]) elif self.goal.posture_type == 'seated': context_vector = np.array([0.5, 0.0, 0.0]) elif self.goal.posture_type == 'lying': context_vector = np.array([0.0, 0.7, 0.0]) # Sample upper-level policy (with no exploration) for hand-over position; # by calculating the mean of the linear-Gaussian model, given context vector s: hand_over_position = np.squeeze( policy_parameter_a + context_vector.dot(policy_parameter_A)) hand_over_pose.pose.position.x = hand_over_position[0] hand_over_pose.pose.position.y = hand_over_position[1] hand_over_pose.pose.position.z = hand_over_position[2] else: # Pick context-independent hand-over position: hand_over_pose.pose.position.x = 0.5 hand_over_pose.pose.position.y = 0.078 hand_over_pose.pose.position.z = 0.8 hand_over_pose.pose.orientation.x = 0.000 hand_over_pose.pose.orientation.y = 0.000 hand_over_pose.pose.orientation.z = 0.000 hand_over_pose.pose.orientation.w = 1.000 # > Pick context_dependent hand-over trajectory shape: if not self.goal.obstacle: trajectory_weights_filename = 'grasp.yaml' else: ## Uncomment one of the following if testing other learned parameters is desired: ## Initial learned trajectory: # trajectory_weights_filename = 'learned_obstacle_avoiding_weights.yaml' ## Smoothed learned trajectory: # trajectory_weights_filename = 'learned_smoothed_obstacle_avoiding_weights.yaml' ## Learned underhand trajectory: # trajectory_weights_filename = 'learned_smoothed_corrected_underhand_weights.yaml' ## Smoothed learned trajectory, with corrected end_points: trajectory_weights_filename = 'learned_smoothed_corrected_obstacle_avoiding_weights.yaml' self.hand_over_dmp = join(self.hand_over_dmp_weights_dir, trajectory_weights_filename) self.goal.person_pose = self.tf_listener.transformPose( "base_link", self.goal.person_pose) distance_to_person = np.sqrt(self.goal.person_pose.pose.position.x**2 + self.goal.person_pose.pose.position.y**2) if distance_to_person > self.person_dist_threshold: move_to_person_goal = MoveBaseGoal() move_to_person_goal.goal_type = MoveBaseGoal.POSE move_to_person_goal.pose.header.frame_id = self.goal.person_pose.header.frame_id move_to_person_goal.pose.pose.position.x = self.goal.person_pose.pose.position.x - 0.3 move_to_person_goal.pose.pose.position.y = self.goal.person_pose.pose.position.y - 0.3 rospy.loginfo('[hand_over] Moving to person...') self.move_base_client.send_goal(move_to_person_goal) # we try moving towards the person before handing the object over; # the person should be close by, so we move for a short time # since the robot will either reach the person in that time, # or will stop as close to the person as possible self.move_base_client.wait_for_result(rospy.Duration(10.)) self.move_base_client.cancel_goal() # Start with arm in neutral position: rospy.loginfo('[hand_over] Moving arm to neutral position...') self.__move_arm(MoveArmGoal.NAMED_TARGET, self.init_config_name) # Move to chosen hand_over position, along appropriate trajectory: rospy.loginfo('[hand_over] Handing object over...') self.__move_arm(MoveArmGoal.END_EFFECTOR_POSE, hand_over_pose) if not self.goal.release_detection: # Naive object release strategy: rospy.loginfo('[hand_over] Waiting before releasing object...') rospy.sleep(5.) # Release the object: rospy.loginfo('[hand_over] Opening the gripper...') self.gripper.open() # Since no detection is used here, the object is released and we always set the reception to True. self.object_reception_detected = True else: # Force sensing object release strategy: rospy.sleep(1.) rospy.loginfo('[hand_over] Waiting for object to be received...') rospy.Subscriber(self.force_sensor_topic, WrenchStamped, self.force_sensor_cb) self.object_reception_detected = False self.cumsum_z = 0. start_time = rospy.get_time() while (rospy.get_time() - start_time) < 50 and not self.object_reception_detected: self.detect_object_reception() rospy.sleep(0.1) # If a pull is detected, release the object: if self.object_reception_detected: rospy.loginfo( '[hand_over] Opening the gripper to release object...') self.gripper.open() else: rospy.loginfo( '[hand_over] Keeping object since no reception was detected' ) # Return to a neutral arm position: rospy.loginfo('[hand_over] Moving back to neutral position...') self.__move_arm(MoveArmGoal.NAMED_TARGET, self.init_config_name) if self.object_reception_detected: self.result = self.set_result(True) else: self.result = self.set_result(False) return FTSMTransitions.DONE
def running(self): receive_object_pose = PoseStamped() receive_object_pose.header.frame_id = 'base_link' if self.goal.context_aware: # > Pick context-dependent receive object position: policy_parameter_a = self.receive_object_position_policy_parameters[ 0][0] policy_parameter_A = self.receive_object_position_policy_parameters[ 0][1] if self.goal.posture_type == 'standing': context_vector = np.array([0.0, 0.0, 0.4]) elif self.goal.posture_type == 'seated': context_vector = np.array([0.5, 0.0, 0.0]) elif self.goal.posture_type == 'lying': context_vector = np.array([0.0, 0.7, 0.0]) # Sample upper-level policy (with no exploration) for object reception position; # by calculating the mean of the linear-Gaussian model, given context vector s receive_object_position = np.squeeze( policy_parameter_a + context_vector.dot(policy_parameter_A)) receive_object_pose.pose.position.x = receive_object_position[0] receive_object_pose.pose.position.y = receive_object_position[1] receive_object_pose.pose.position.z = receive_object_position[2] else: # Pick context-independent object reception position receive_object_pose.pose.position.x = 0.5 receive_object_pose.pose.position.y = 0.078 receive_object_pose.pose.position.z = 0.8 receive_object_pose.pose.orientation.x = 0.000 receive_object_pose.pose.orientation.y = 0.000 receive_object_pose.pose.orientation.z = 0.000 receive_object_pose.pose.orientation.w = 1.000 self.goal.person_pose = self.tf_listener.transformPose( "base_link", self.goal.person_pose) distance_to_person = np.sqrt(self.goal.person_pose.pose.position.x**2 + self.goal.person_pose.pose.position.y**2) if distance_to_person > self.person_dist_threshold: move_to_person_goal = MoveBaseGoal() move_to_person_goal.goal_type = MoveBaseGoal.POSE move_to_person_goal.pose.header.frame_id = self.goal.person_pose.header.frame_id move_to_person_goal.pose.pose.position.x = self.goal.person_pose.pose.position.x - 0.3 move_to_person_goal.pose.pose.position.y = self.goal.person_pose.pose.position.y - 0.3 rospy.loginfo('[receive_object] Moving to person...') self.move_base_client.send_goal(move_to_person_goal) # we try moving towards the person before receiving the object; # the person should be close by, so we move for a short time # since the robot will either reach the person in that time, # or will stop as close to the person as possible self.move_base_client.wait_for_result(rospy.Duration(10.)) self.move_base_client.cancel_goal() # Start with arm in neutral position: rospy.loginfo('[receive_object] Moving arm to neutral position...') self.__move_arm(MoveArmGoal.NAMED_TARGET, self.init_config_name) # Move to chosen receive_object position, along appropriate trajectory: rospy.loginfo('[receive_object] Receiving object...') self.__move_arm(MoveArmGoal.END_EFFECTOR_POSE, receive_object_pose) if not self.goal.reception_detection: # Naive object release strategy: rospy.loginfo( '[receive_object] Waiting before releasing object...') rospy.sleep(5.) # Release the object: rospy.loginfo('[receive_object] Opening the gripper...') self.gripper.open() # Since no detection is used here, the object is released and we always set the reception to True. self.object_reception_detected = True else: # Force sensing object release strategy: rospy.sleep(1.) rospy.loginfo( '[receive_object] Waiting for object to be received...') rospy.Subscriber(self.force_sensor_topic, WrenchStamped, self.force_sensor_cb) self.object_reception_detected = False self.cumsum_z = 0. start_time = rospy.get_time() while (rospy.get_time() - start_time) < 50 and not self.object_reception_detected: self.detect_object_reception() rospy.sleep(0.1) # If a pull is detected, release the object: if self.object_reception_detected: rospy.loginfo( '[receive_object] Closing the gripper to receive the object...' ) self.gripper.close() else: rospy.loginfo( '[receive_object] Not waiting anymore since no reception was detected' ) # Return to a neutral arm position: rospy.loginfo('[receive_object] Moving back to neutral position...') self.__move_arm(MoveArmGoal.NAMED_TARGET, self.init_config_name) if self.object_reception_detected: self.result = self.set_result(True) else: self.result = self.set_result(False) return FTSMTransitions.DONE