def __init__(self, text=None, text_cb=None, wait_before_speaking=0, **kwargs):
"""
@param text: the text to speak out aloud (if `text_cb' is None)
@param text_cb: a callback returning the text to speak out
(if `text' is None)
@param wait_before_speaking: how long to wait before speaking;
it may be a number (in seconds) or a rospy.Duration
@param **kwargs: input_keys, output_keys, etc.
"""
if not text and not text_cb:
raise ValueError("Neither `text' nor `text_cb' where set!")
elif text and text_cb:
raise ValueError("You've set both `text' and `text_cb'!")
if not isinstance(wait_before_speaking, rospy.Duration):
wait_before_speaking = rospy.Duration(wait_before_speaking)
def generic_goal_cb(userdata, old_goal):
tts_goal = SoundGoal()
tts_goal.wait_before_speaking = wait_before_speaking
tts_goal.text = text if text else text_cb(userdata)
print "Speaking:", tts_goal.text
return tts_goal
SimpleActionState.__init__(self,
TTS_ACTION_NAME, SoundAction,
goal_cb=generic_goal_cb, **kwargs)
def __init__(self):
SimpleActionState.__init__(self, topics["plan_execution"],
ExecutePlanAction,
goal_cb=self.goal_cb,
result_cb=self.result_cb,
input_keys=['goal'],
output_keys=['result'])
def __init__(self, frame='/map'):
SimpleActionState.__init__(self,
'move_base',
MoveBaseAction,
input_keys=['x', 'y', 'yaw'],
goal_cb=self.__goal_cb)
self.frame = frame
def __init__(
self,
goal_positon,
travel_speed=0.5,
sphere_of_acceptance=0.5,
action_server="/guidance_interface/los_server",
):
"""A SimpleActionState for traveling to a goal position using LOS guidance.
Args:
goal_positon (geometry_msgs/Point): position drone should travel to.
start_position (geometry_msgs/Point): position drone starts in.
travel_speed (float, optional):
forward velocity drone shoudl travel at. Defaults to 0.5.
sphere_of_acceptance (float, optional):
action returns success when drone is inside this sphere. Defaults to 0.5.
action_server (str, optional):
name of los action server. Defaults to "/guidance_interface/los_server".
"""
goal = LosPathFollowingGoal()
goal.next_waypoint = goal_positon
goal.forward_speed = travel_speed
goal.desired_depth = goal_positon.z
goal.sphereOfAcceptance = sphere_of_acceptance
SimpleActionState.__init__(self, action_server, LosPathFollowingAction,
goal)
def __init__(self, input_keys=['tuck_left', 'tuck_right']):
tuck_arms_uri = '/tuck_arms'
SimpleActionState.__init__(self,
tuck_arms_uri,
TuckArmsAction,
goal_cb=self.goal_cb,
input_keys=input_keys)
def __init__(self):
SimpleActionState.__init__(
self,
move_kinect_topic,
MoveSensorAction,
goal_cb=self.goal_cb,
outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['move_end_effector_msg'],
output_keys=['move_end_effector_msg'])
def __init__(self, target_type):
target_selection_goal = self.targetSelectionGoal(target_type)
SimpleActionState.__init__(self,
select_target_topic,
SelectTargetAction,
goal=target_selection_goal,
result_key='target_pose')
def __init__(self, vocabulary_list):
if not isinstance(vocabulary_list, list):
rospy.logerr('SetSpeechVocabularyState received something which is not a list')
sys.exit(-1)
elif vocabulary_list == []:
rospy.logwarn('SetSpeechVocabularyState received an empty vocabulary_list')
voc_goal = SetSpeechVocabularyGoal()
voc_goal.words = vocabulary_list
SimpleActionState.__init__(self, '/speech_vocabulary_action', SetSpeechVocabularyAction, goal=voc_goal)
def __init__(self, posture_name, speed):
# Assert posture is known
if not posture_name in self.POSTURES:
rospy.logerr('Posture "%s" is an unknown posture! Known postures are: %s. Shutting down node...' % (posture_name, str(self.POSTURES)))
sys.exit(-1)
def body_pose_goal_cb(userdata, goal):
goal = BodyPoseWithSpeedGoal()
goal.posture_name = posture_name
goal.speed = speed
return goal
SimpleActionState.__init__(self, '/body_pose_naoqi', BodyPoseWithSpeedAction, goal_cb=body_pose_goal_cb)
def __init__(self, behavior_name=None):
# Assert behavior is installed
input_keys = []
if not behavior_name:
input_keys = ['behavior_name']
else:
_checkInstalledBehavior(behavior_name)
def behavior_goal_cb(userdata, goal):
goal = RunBehaviorGoal()
goal.behavior = userdata.behavior_name if not behavior_name else behavior_name
return goal
SimpleActionState.__init__(self, '/run_behavior', RunBehaviorAction, goal_cb=behavior_goal_cb, input_keys=input_keys)
def __init__(self, pose, action_server="/guidance_interface/dp_server"):
"""A SimpleActionState that travels to a goal pose using our DP guidance.
Only use when in close proximity of goal pose.
Args:
pose (geometry_msgs/Pose): Goal pose.
action_server (str, optional):
name of dp action server. Defaults to "/guidance_interface/dp_server".
"""
goal = MoveBaseGoal()
goal.target_pose.pose = pose
SimpleActionState.__init__(self, action_server, MoveBaseAction, goal)
def __init__(self, vocabulary_list):
if not isinstance(vocabulary_list, list):
rospy.logerr(
'SetSpeechVocabularyState received something which is not a list'
)
sys.exit(-1)
elif vocabulary_list == []:
rospy.logwarn(
'SetSpeechVocabularyState received an empty vocabulary_list')
voc_goal = SetSpeechVocabularyGoal()
voc_goal.words = vocabulary_list
SimpleActionState.__init__(self,
'/speech_vocabulary_action',
SetSpeechVocabularyAction,
goal=voc_goal)
def __init__(
self,
twist,
dp_control_mode=ControlModeEnum.OPEN_LOOP,
action_server="/guidance_interface/vel_server",
):
"""A SimpleActionState that sets drone velocity to a given twist.
Args:
twist (geometry_msgs/Twist): desired velocity
action_server (str, optional):
name of vel action server. Defaults to "/guidance_interface/vel_server".
"""
goal = SetVelocityGoal()
goal.desired_velocity = twist
goal.control_mode = dp_control_mode.value
SimpleActionState.__init__(self, action_server, SetVelocityAction,
goal)
def __init__(self, text=None, wait_before_speak=None):
''' If text is used, it will be that text which the robot will say. '''
self._text = text
input_keys = []
if not text:
input_keys = ['text']
self._wait_before_speak = wait_before_speak
# Method to define the goal
def tts_request_cb(ud, goal):
tts_goal = SpeechWithFeedbackGoal()
if (not self._text):
tts_goal.say = ud.text
else:
tts_goal.say = self._text
return tts_goal
SimpleActionState.__init__(self, '/speech_action', SpeechWithFeedbackAction, input_keys=input_keys, goal_cb=tts_request_cb)
def __init__(self, text=None, wait_before_speak=None):
''' If text is used, it will be that text which the robot will say. '''
self._text = text
input_keys = []
if not text:
input_keys = ['text']
self._wait_before_speak = wait_before_speak
# Method to define the goal
def tts_request_cb(ud, goal):
tts_goal = SpeechWithFeedbackGoal()
if (not self._text):
tts_goal.say = ud.text
else:
tts_goal.say = self._text
return tts_goal
SimpleActionState.__init__(self,
'/speech_action',
SpeechWithFeedbackAction,
input_keys=input_keys,
goal_cb=tts_request_cb)
def __init__(self, path=None, frame_id='base_footprint'):
# Private attributes
self._path = path
self._frame_id = frame_id
# Method to define the goal
def walk_path_goal_cb(userdata, goal):
walk_path_goal = FollowPathGoal()
path = Path()
path.header.frame_id = self._frame_id
path.header.stamp = rospy.Time.now()
for sub in self._path:
subgoal = PoseStamped()
subgoal.header.frame_id = self._frame_id
subgoal.header.stamp = rospy.Time.now()
subgoal.pose.position = Point(sub[0], sub[1], 0.0)
q = tf.transformations.quaternion_from_euler(0, 0, sub[2])
subgoal.pose.orientation = Quaternion(*q)
path.poses.append(subgoal)
walk_path_goal.path = path
return walk_path_goal
# Class constructor
SimpleActionState.__init__(self, '/walk_path', FollowPathAction, goal_cb=walk_path_goal_cb)
def __init__(self, joint_names=None, joint_angles=None):
self.joint_names = joint_names
self.joint_angles = joint_angles
input_keys = []
if joint_angles is None:
input_keys = ['joint_angles']
# Method to define the goal
def joint_request_cb(ud, goal):
joint_goal = JointAnglesWithSpeedGoal()
joint_goal.joint_angles.joint_names = self.joint_names
if self.joint_angles is not None:
joint_goal.joint_angles.joint_angles = self.joint_angles
else:
joint_goal.joint_angles.joint_angles = ud.joint_angles
joint_goal.joint_angles.speed = 0.10
joint_goal.joint_angles.relative = 0
return joint_goal
SimpleActionState.__init__(self,
'/joint_angles_action',
JointAnglesWithSpeedAction,
input_keys=input_keys,
goal_cb=joint_request_cb)
def __init__(self, input_keys=[]):
SimpleActionState.__init__(self,
'recharge',
RechargeAction,
goal_cb=self.goal_cb,
input_keys=input_keys)
def __init__(self, frame_id="/base_link", move_base_action_name=MOVE_BASE_ACTION_NAME, pose=None, goal_cb=None,
goal_key=None, **kwargs):
"""Constructor for MoveActionState
@type frame_id: string
@param frame_id: The fame_id of the pose object. By default is "/base_link"
@type move_base_action_name: string
@param move_base_action_name: The action name. By default is the action name defined on the MOVE_BASE_ACTION_NAME variable.
@type pose: geometry_msgs.Pose
@param pose: The pose where the robot should move to on frame_id link.
@type goal_cb: callable
@param goal_cb: The function that will be called to get the pose. You can set only one of 'pose, goal_cb and goal_key' variables.
@type goal_key: input_key on userdata.
@param goal_key: The key on userdata variable that contains the pose.
"""
if (pose and goal_cb) or (pose and goal_key) or (goal_cb and goal_key):
raise ValueError("You've set more than one of `pose', " \
"`goal_cb' and `goal_key'!")
if not goal_cb and not goal_key and not pose:
raise ValueError("Neither `pose' nor `goal_cb' nor `goal_key' were set!")
if goal_key:
if not 'input_keys' in kwargs:
kwargs['input_keys'] = []
kwargs['input_keys'].append(goal_key)
def generic_goal_cb(userdata, old_goal):
""" Send a goal to the action """
self.nav_goal = MoveBaseGoal()
self.nav_goal.target_pose.header.stamp = rospy.Time.now()
self.nav_goal.target_pose.header.frame_id = frame_id
if pose:
self.nav_goal.target_pose.pose = pose
return self.nav_goal
elif goal_key:
self.nav_goal.target_pose.pose = getattr(userdata, goal_key)
return self.nav_goal
else:
self.nav_goal.target_pose.pose = Pose()
return goal_cb(userdata, self.nav_goal)
def _result_cb(userdata, status, result):
""" This function will analize the status of the response
If the status is aborted, will call the action again with the same
orientation, but with the distance -0,5. If the status now is succeeded,
then call the move_by with x=0.5 in front.
:type status: actionlib.GoalStatus
:type result: MoveBaseResult
"""
if status != GoalStatus.SUCCEEDED:
rospy.loginfo(C.BACKGROUND_YELLOW + "Retrying go to the target goal" + C.NATIVE_COLOR)
new_pose = pose_at_distance(self.nav_goal.target_pose.pose, DISTANCE_TO_RETRY )
self.nav_goal.target_pose.pose = new_pose
self.nav_goal.target_pose.header.stamp = rospy.Time.now()
move_action = SimpleActionState(move_base_action_name, MoveBaseAction, goal=self.nav_goal)
result_status = move_action.execute(userdata)
if result_status == succeeded:
new_goal = MoveBaseGoal()
new_goal.target_pose.header.frame_id = FRAME_BASE_LINK
new_goal.target_pose.pose.position.x = DISTANCE_TO_RETRY
new_goal.target_pose.header.stamp = rospy.Time.now()
move_action = SimpleActionState(MOVE_BY_ACTION_NAME , MoveBaseAction, goal=new_goal)
return move_action.execute(userdata)
# THE PART BELOW WAS 'REMOVED' BECAUSE FINAL_APPROACH IS NOT RUNNING BY DEFAULT.
# final_approach_goal = FinalApproachPoseRequest()
# final_approach_goal.pose.position.x = DISTANCE_TO_RETRY - 0.1 # -0.1 Just because final_approach don't check if is secure to move
# final_approach_goal.pose.orientation.w = 0.0
# rospy.loginfo("Calling /approachToGoal with message:\n" + str(final_approach_goal))
# final_approach = ServiceState('/approachToGoal', FinalApproachPose, request=final_approach_goal)
# return final_approach.execute(userdata)
SimpleActionState.__init__(self,
move_base_action_name, MoveBaseAction,
goal_cb=generic_goal_cb, result_cb=_result_cb, **kwargs)
def __init__(self, frame='/map'):
SimpleActionState.__init__(self, 'move_base', MoveBaseAction, input_keys=['x', 'y', 'yaw'], goal_cb=self.__goal_cb)
self.frame = frame
def __init__(self, follow_joint_trajectory_topic = '/right_hand_controller/follow_joint_trajectory'):
SimpleActionState.__init__(self, follow_joint_trajectory_topic,
FollowJointTrajectoryAction,
goal_cb=grasp_open_hand_goal_cb2,
result_cb=grasp_hand_result_cb)
def __init__(self):
SimpleActionState.__init__(self, initial_turn_topic, InitialTurnAction)
def __init__(self):
SimpleActionState.__init__(self,
move_base_topic,
MoveBaseAction,
goal_key='target_pose')
def __init__(self):
SimpleActionState.__init__(self, 'move_base', MoveBaseAction, input_keys=['pose'], goal_cb=self.__goal_cb)
def __init__(self, input_keys=['tuck_left', 'tuck_right']):
tuck_arms_uri = '/tuck_arms'
SimpleActionState.__init__(self, tuck_arms_uri, TuckArmsAction, goal_cb=self.goal_cb, input_keys=input_keys)
def __init__(self, frame='/map'):
SimpleActionState.__init__(self, 'move_base', MoveBaseAction, input_keys=['xyt','sResult'], goal_cb=self.__goal_cb,result_cb=self.movebase_result_cb,output_keys=['sResult','sRecCounter_out'])
self.frame = frame
def __init__(self, frame="/map"):
SimpleActionState.__init__(
self, "move_base", MoveBaseAction, input_keys=["x", "y", "yaw"], goal_cb=self.__goal_cb
)
self.frame = frame
def __init__(self, input_keys=[]):
SimpleActionState.__init__(self, 'recharge', RechargeAction, goal_cb=self.goal_cb, input_keys=input_keys)
def __init__(self):
SimpleActionState.__init__(
self,
'move_SchunkArm',
MoveArmAction,
input_keys=['x', 'y', 'z', 'phi', 'parent_frame'])
def __init__(self):
SimpleActionState.__init__(self, 'move_SchunkArm', MoveArmAction,
input_keys=['x', 'y', 'z', 'phi', 'parent_frame'])