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