def execute(self, userdata):
feedback = MoveBaseFeedback()
feedback.current_state = 'SETUP_MOVE_BASE'
feedback.message = '[MOVE_BASE] Received a move base request'
userdata.move_base_feedback = feedback
rospy.loginfo('[MOVE_BASE] Moving the arm to a safe configuration...')
move_arm_goal = MoveArmGoal()
move_arm_goal.goal_type = MoveArmGoal.NAMED_TARGET
move_arm_goal.named_target = self.safe_arm_joint_config
self.move_arm_client.send_goal(move_arm_goal)
self.move_arm_client.wait_for_result()
return 'succeeded'
def running(self):
rospy.loginfo('[move_base] Moving the arm to a safe configuration...')
move_arm_goal = MoveArmGoal()
move_arm_goal.goal_type = MoveArmGoal.NAMED_TARGET
move_arm_goal.named_target = self.safe_arm_joint_config
self.move_arm_client.send_goal(move_arm_goal)
self.move_arm_client.wait_for_result()
pose = PoseStamped()
if self.goal.goal_type == MoveBaseGoal.NAMED_TARGET:
destination = self.goal.destination_location
rospy.loginfo('[move_base] Moving base to %s', destination)
self.pose = self.convert_pose_name_to_coordinates(destination)
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = self.pose_frame
pose.pose.position.x = self.pose[0]
pose.pose.position.y = self.pose[1]
quat = tf.transformations.quaternion_from_euler(0, 0, self.pose[2])
pose.pose.orientation = Quaternion(*quat)
elif self.goal.goal_type == MoveBaseGoal.POSE:
pose = self.goal.pose
rospy.loginfo('[move_base] Moving base to %s', pose)
else:
rospy.logerr(
'[move_base] Received an unknown goal type; ignoring request')
self.result = self.set_result(False)
return FTSMTransitions.DONE
goal = move_base_msgs.MoveBaseGoal()
goal.target_pose = pose
move_base_client = actionlib.SimpleActionClient(
self.move_base_server, move_base_msgs.MoveBaseAction)
move_base_client.wait_for_server()
move_base_client.send_goal(goal)
success = move_base_client.wait_for_result()
if success:
rospy.loginfo('[move_base] Pose reached successfully')
self.result = self.set_result(True)
return FTSMTransitions.DONE
rospy.logerr('[move_base] Pose could not be reached')
self.result = self.set_result(False)
return FTSMTransitions.DONE
def __move_arm(self, goal_type, goal):
move_arm_goal = MoveArmGoal()
move_arm_goal.goal_type = goal_type
if goal_type == MoveArmGoal.NAMED_TARGET:
move_arm_goal.named_target = goal
elif goal_type == MoveArmGoal.END_EFFECTOR_POSE:
move_arm_goal.end_effector_pose = goal
move_arm_goal.dmp_name = self.grasping_dmp
move_arm_goal.dmp_tau = self.dmp_tau
self.move_arm_client.send_goal(move_arm_goal)
self.move_arm_client.wait_for_result()
result = self.move_arm_client.get_result()
return result
def __move_arm(self, goal_type, goal):
'''Sends a request to the 'move_arm' action server and waits for the
results of the action execution.
Keyword arguments:
goal_type -- 'MoveArmGoal.NAMED_TARGET' or 'MoveArmGoal.END_EFFECTOR_POSE'
goal -- A string if 'goal_type' is 'MoveArmGoal.NAMED_TARGET';
a 'geometry_msgs/PoseStamped' if 'goal_type' is 'MoveArmGoal.END_EFFECTOR_POSE'
'''
move_arm_goal = MoveArmGoal()
move_arm_goal.goal_type = goal_type
if goal_type == MoveArmGoal.NAMED_TARGET:
move_arm_goal.named_target = goal
elif goal_type == MoveArmGoal.END_EFFECTOR_POSE:
move_arm_goal.end_effector_pose = goal
move_arm_goal.dmp_name = self.handle_open_dmp
move_arm_goal.dmp_tau = self.dmp_tau
self.move_arm_client.send_goal(move_arm_goal)
self.move_arm_client.wait_for_result()
result = self.move_arm_client.get_result()
return result
def running(self):
rospy.loginfo('[move_base] Moving the arm to a safe configuration...')
move_arm_goal = MoveArmGoal()
move_arm_goal.goal_type = MoveArmGoal.NAMED_TARGET
move_arm_goal.named_target = self.safe_arm_joint_config
self.move_arm_client.send_goal(move_arm_goal)
self.move_arm_client.wait_for_result()
pose = PoseStamped()
if self.goal.goal_type == MoveBaseGoal.NAMED_TARGET:
destination = self.goal.destination_location
rospy.loginfo('[move_base] Moving base to %s', destination)
self.pose = self.convert_pose_name_to_coordinates(destination)
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = self.pose_frame
pose.pose.position.x = self.pose[0]
pose.pose.position.y = self.pose[1]
quat = tf.transformations.quaternion_from_euler(0, 0, self.pose[2])
pose.pose.orientation = Quaternion(*quat)
elif self.goal.goal_type == MoveBaseGoal.POSE:
pose = self.goal.pose
rospy.loginfo('[move_base] Moving base to %s', pose)
else:
rospy.logerr(
'[move_base] Received an unknown goal type; ignoring request')
self.result = self.set_result(False)
return FTSMTransitions.DONE
# we attempt a recovery in which we send the robot to a slightly
# different pose from the one originally requested
if self.is_recovering:
pose.pose.position.x += np.random.normal(
0., self.recovery_position_m_std)
pose.pose.position.y += np.random.normal(
0., self.recovery_position_m_std)
goal = move_base_msgs.MoveBaseGoal()
goal.target_pose = pose
self.move_base_goal_pub.publish(pose)
move_base_client = actionlib.SimpleActionClient(
self.move_base_server, move_base_msgs.MoveBaseAction)
move_base_client.wait_for_server()
move_base_client.send_goal(goal)
if move_base_client.wait_for_result(
rospy.Duration.from_sec(self.timeout)):
result = move_base_client.get_result()
resulting_state = move_base_client.get_state()
if result and resulting_state == GoalStatus.SUCCEEDED:
rospy.loginfo('[move_base] Pose reached successfully')
self.reset_state()
self.result = self.set_result(True)
return FTSMTransitions.DONE
else:
rospy.logerr('[move_base] Pose could not be reached')
if self.recovery_count < self.max_recovery_attempts:
self.recovery_count += 1
rospy.logwarn('[move_base] Attempting recovery')
return FTSMTransitions.RECOVER
else:
rospy.logerr(
'[move_base] Pose could not be reached within %f seconds',
self.timeout)
if self.recovery_count < self.max_recovery_attempts:
self.recovery_count += 1
rospy.logerr('[move_base] Attempting recovery')
return FTSMTransitions.RECOVER
rospy.logerr('[move_base] Pose could not be reached; giving up')
self.reset_state()
self.result = self.set_result(False)
return FTSMTransitions.DONE