def __init__(self,
move_group,
offset,
tool_link,
action_topic='/move_group'):
'''
Constructor
'''
super(MoveitToJointsDynState, self).__init__(
outcomes=['reached', 'planning_failed', 'control_failed'],
input_keys=['joint_values', 'joint_names'])
self._offset = offset
self._tool_link = tool_link
self._action_topic = action_topic
self._fk_srv_topic = '/compute_fk'
self._cartesian_srv_topic = '/compute_cartesian_path'
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
self._fk_srv = ProxyServiceCaller({self._fk_srv_topic: GetPositionFK})
self._cartesian_srv = ProxyServiceCaller(
{self._cartesian_srv_topic: GetCartesianPath})
self._current_group_name = move_group
self._joint_names = None
self._planning_failed = False
self._control_failed = False
self._success = False
self._traj_client = actionlib.SimpleActionClient(
'execute_trajectory', moveit_msgs.msg.ExecuteTrajectoryAction)
self._traj_client.wait_for_server()
self._traj_exec_result = None
def __init__(self, robot_name):
"""Constructor"""
super(CaptureCharUcoState, self).__init__(outcomes=['done', 'fail'])
self.robot_name = robot_name
self.move_mode = 'p2p'
self.pose_indx = 0
self.hand_eye_service = '/camera/hand_eye_calibration'
self.hand_eye_client = ProxyServiceCaller({self.hand_eye_service: hand_eye_calibration})
self.get_feedback_service = self.robot_name +'/get_kinematics_pose'
self.get_feedback_client = ProxyServiceCaller({self.get_feedback_service: GetKinematicsPose})
def test_service_caller(self):
t1 = '/service_1'
rospy.Service(t1, Trigger, lambda r: (True, 'ok'))
srv = ProxyServiceCaller({t1: Trigger})
result = srv.call(t1, TriggerRequest())
self.assertIsNotNone(result)
self.assertTrue(result.success)
self.assertEqual(result.message, 'ok')
self.assertFalse(srv.is_available('/not_there'))
srv = ProxyServiceCaller({'/invalid': Trigger}, wait_duration=.1)
self.assertFalse(srv.is_available('/invalid'))
def __init__(self,
controller='joint_state_head',
duration=120,
interval=[2.0, 4.0],
joints=[],
minimal=[],
maximal=[]):
super(RandJointsMovements, self).__init__(outcomes=['done', 'failed'],
input_keys=['config'])
# Store topic parameter for later use.
self._controller = 'motion/controller/' + controller
# create proxies
self._set_operational_caller = ProxyServiceCaller(
{self._controller + '/set_operational': SetBool})
self._publisher = ProxyPublisher(
{self._controller + '/in_joints_ref': JointState})
# state
self._start_timestamp = None
self._movement_timestamp = None
self._movement_duration = Duration()
# user input
self.set_movement_parameters(duration, interval, joints, minimal,
maximal)
# error in enter hook
self._error = False
def __init__(self, sentence, input_keys=[], emotion=0, block=True):
"""Constructor"""
super(SaraSay, self).__init__(outcomes=['done'], input_keys=input_keys)
if not (isinstance(sentence, types.LambdaType) and sentence.__name__
== "<lambda>") and len(input_keys) is not 0:
raise ValueError(
"In state " + type(self).__name__ + " saying \"" + sentence +
"\", you need to define a lambda function for sentence.")
# Get parameters
self.msg = say()
self.sentence = sentence
self.emotion = UInt8()
self.emotion.data = emotion
self.block = block
# Set topics
self.emotion_topic = "sara_face/Emotion"
self.say_topic = "/say"
self.sayServiceTopic = "/wm_say"
self.emotionPub = ProxyPublisher({self.emotion_topic: UInt8})
# Prepare wm_tts
if self.block:
self.sayClient = ProxyServiceCaller(
{self.sayServiceTopic: say_service})
else:
self.sayPub = ProxyPublisher({self.say_topic: say})
# Initialise the face sub
self.emotionSub = ProxySubscriberCached({self.emotion_topic: UInt8})
self.lastEmotion = None
def __init__(self, group, offset, joint_names, tool_link, rotation):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ComputeGraspState,
self).__init__(outcomes=['continue', 'failed'],
input_keys=['pose'],
output_keys=['joint_values', 'joint_names'])
self._group = group
self._offset = offset
self._joint_names = joint_names
self._tool_link = tool_link
self._rotation = rotation
self._srv_name = '/compute_ik'
self._ik_srv = ProxyServiceCaller({self._srv_name: GetPositionIK})
# tf to transfor the object pose
self._tf_buffer = tf2_ros.Buffer(
rospy.Duration(10.0)) #tf buffer length
self._tf_listener = tf2_ros.TransformListener(self._tf_buffer)
self._robot1_client = actionlib.SimpleActionClient(
'execute_trajectory', moveit_msgs.msg.ExecuteTrajectoryAction)
self._robot1_client.wait_for_server()
rospy.loginfo('Execute Trajectory server is available for robot1')
def __init__(self, config_name, move_group="", duration=1.0, wait_for_execution=True, action_topic='/execute_kinematic_path', robot_name=""):
'''
Constructor
'''
super(SrdfStateToMoveitExecute, self).__init__(
outcomes=['reached', 'request_failed', 'moveit_failed', 'param_error'])
self._config_name = config_name
self._robot_name = robot_name
self._move_group = move_group
self._duration = duration
self._wait_for_execution = wait_for_execution
self._action_topic = action_topic
self._client = ProxyServiceCaller({self._action_topic: ExecuteKnownTrajectory})
# self._action_topic = action_topic
# self._client = ProxyActionServer({self._action_topic: ExecuteTrajectoryAction})
self._request_failed = False
self._moveit_failed = False
self._success = False
self._srdf_param = None
if rospy.has_param("/robot_description_semantic"):
self._srdf_param = rospy.get_param("/robot_description_semantic")
else:
Logger.logerror('Unable to get parameter: /robot_description_semantic')
self._param_error = False
self._srdf = None
self._response = None
def on_enter(self, userdata):
# This method is called when the state becomes active, i.e. a transition from another state to this one is taken.
# It is primarily used to start actions which are associated with this state.
self._move_group = userdata.move_group
self._move_group_prefix = userdata.move_group_prefix
self._tool_link = userdata.tool_link
self._offset = userdata.offset
self._rotation = userdata.rotation
self._srv_name = userdata.move_group_prefix + '/compute_ik'
self._ik_srv = ProxyServiceCaller({self._srv_name: GetPositionIK})
self._robot1_client = actionlib.SimpleActionClient(userdata.move_group_prefix + '/execute_trajectory', moveit_msgs.msg.ExecuteTrajectoryAction)
self._robot1_client.wait_for_server()
rospy.loginfo('Execute Trajectory server is available for robot')
# Get transform between camera and robot1_base
while True:
rospy.sleep(0.1)
try:
target_pose = self._tf_buffer.transform(userdata.pose, "world")
break
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
rospy.logerr("ComputeGraspState::on_enter - Failed to transform to world")
continue
# the grasp pose is defined as being located on top of the item
target_pose.pose.position.z += self._offset + 0.0
# rotate the object pose 180 degrees around - now works with -90???
#q_orig = [target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]
q_orig = [0, 0, 0, 1]
#q_rot = quaternion_from_euler(self._rotation, 0, 0)
q_rot = quaternion_from_euler(self._rotation, math.pi/2.0, 0) # math.pi/2.0 added by gerard!!
#q_rot = quaternion_from_euler(math.pi/-2.0, 0, 0)
res_q = quaternion_multiply(q_rot, q_orig)
target_pose.pose.orientation = geometry_msgs.msg.Quaternion(*res_q)
# use ik service to compute joint_values
self._srv_req = GetPositionIKRequest()
self._srv_req.ik_request.group_name = self._move_group
self._srv_req.ik_request.robot_state.joint_state = rospy.wait_for_message(self._move_group_prefix + '/joint_states', sensor_msgs.msg.JointState)
self._srv_req.ik_request.ik_link_name = self._tool_link # TODO: this needs to be a parameter
self._srv_req.ik_request.pose_stamped = target_pose
self._srv_req.ik_request.avoid_collisions = True
self._srv_req.ik_request.attempts = 500
try:
self._srv_result = self._ik_srv.call(self._srv_name, self._srv_req)
self._failed = False
except Exception as e:
Logger.logwarn('Could not call IK: ' + str(e))
self._failed = True
def __init__(self, tolerance):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(AlignWithGate, self).__init__(outcomes = ['Aligned', 'CantSee'])
#To catch the sent srv_type in the state parameter
self._v_topic = "/vision/gate"
self.sway_left_client = ProxyServiceCaller({'/autonomous/sway_left': Empty}) # pass required clients as dict (topic: type)
self.sway_right_client = ProxyServiceCaller({'/autonomous/sway_right': Empty}) # pass required clients as dict (topic: type)
self.heave_down_client = ProxyServiceCaller({'/autonomous/heave_down': Empty}) # pass required clients as dict (topic: type)
self.heave_up_client = ProxyServiceCaller({'/autonomous/heave_up': Empty}) # pass required clients as dict (topic: type)
self.stop_client = ProxyServiceCaller({'/autonomous/stop_vehicle': Empty}) # pass required clients as dict (topic: type)
self.vision_sub = ProxySubscriberCached({self._v_topic: vision_target})
self.width = rospy.get_param("frame_width")
self.height = rospy.get_param("frame_height")
self.width_h = (self.width/2) + tolerance
self.width_l = (self.width/2) - tolerance
self.height_h = (self.height/2) + tolerance
self.height_l = (self.height/2) - tolerance
self.inrange_flag = True
Logger.loginfo("initiated allign gate state")
def __init__(self):
# See example_state.py for basic explanations.
super(SaraNLUrestaurant, self).__init__(outcomes=['understood', 'fail'], input_keys=['sentence'],
output_keys=['orderList'])
self.serviceName = "/Restaurant_NLU_Service"
Logger.loginfo("waiting forservice: " + self.serviceName)
self.serviceNLU = ProxyServiceCaller({self.serviceName: RestaurantNLUService})
def __init__(self, stop):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(SetConveyorPowerState, self).__init__(outcomes = ['succeeded', 'failed'], input_keys = ['speed'])
# Store state parameter for later use.
self._stop = stop
# initialize service proxy
self._srv_name = '/omtp/conveyor/control'
self._srv = ProxyServiceCaller({self._srv_name: ConveyorBeltControl})
def __init__(self, object_name):
super(RecognizeObjectState,
self).__init__(outcomes=['detected', 'not_detected', 'failed'],
input_keys=['pointcloud'])
self._srv_topic = '/recognition_service/sv_recognition'
self._srv = ProxyServiceCaller({self._srv_topic: recognize})
self._object_name = object_name
self._srv_result = None
self._failed = False
def __init__(self):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(Wait_getCloserVictim, self).__init__(
outcomes=['waiting', 'restart', 'preempted'],
input_keys=['task_details_task_id', 'params_distance'],
output_keys=['pose', 'params_distance'])
self._allocated_task = '/taskallocation/allocatedTask'
self._sub = ProxySubscriberCached({self._allocated_task: Task})
self._topic_get_task = 'taskallocation/get_task'
self.getTask = ProxyServiceCaller({self._topic_get_task: GetTask})
def __init__(self, depth):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(MovementServiceCaller,
self).__init__(outcomes=['Done', 'Failed'])
#To catch the sent srv_type in the state parameter
self._topic = "/autonomous/depth_control"
self._srv_type = depth_adv
self._depth = depth
self.service_client = ProxyServiceCaller(
{self._topic:
self._srv_type}) # pass required clients as dict (topic: type)
Logger.loginfo("initiated %s service caller" % self._topic)
def __init__(self):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(DetectPlaneState, self).__init__(outcomes=['continue', 'failed'])
# The constructor is called when building the state machine, not when actually starting the behavior.
# Thus, we cannot save the starting time now and will do so later.
self._srv_topic = "/detect_plane_srv"
self._srv = ProxyServiceCaller({self._srv_topic: Trigger})
self._srv_result = None
self._failed = False
def __init__(self, robot_name):
'''
Constructor
'''
super(Pos2Robot, self).__init__(outcomes=['done', 'failed'],
input_keys=['c_pos'],
output_keys=['pos'])
self.robot_name = robot_name
self.pos2robot_service = self.robot_name + '/eye2base'
self.pos2robot_client = ProxyServiceCaller(
{self.pos2robot_service: eye2base})
def __init__(self, model_name, output_frame_id):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(LocateFactoryDeviceState, self).__init__(outcomes = ['succeeded', 'failed'], output_keys = ['pose'])
# Store state parameter for later use.
self._failed = True
# initialize service proxy
self._srv_name = '/gazebo/get_model_state'
self._srv = ProxyServiceCaller({self._srv_name: GetModelState})
self._srv_req = GetModelStateRequest()
self._srv_req.model_name = model_name # TODO: change parameter name
self._srv_req.relative_entity_name = output_frame_id # TODO: change parameter name
def __init__(self, target, service_name, srv_type):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(MAVROSServiceCaller, self).__init__(outcomes=['Done', 'Failed'])
#To catch the sent srv_type in the state parameter
types = {"CommandBool": CommandBool, "SetMode": SetMode}
self._topic = service_name
self._target = target
self._srv_type = types[srv_type]
self.service_client = ProxyServiceCaller(
{self._topic:
self._srv_type}) # pass required clients as dict (topic: type)
#self._proxy = rospy.ServiceProxy(self._topic,dist)
# Store state parameter for later use.
Logger.loginfo("initiated %s service caller" % self._topic)
def __init__(self, enable, service_name):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(VacuumGripperControlState,
self).__init__(outcomes=['continue', 'failed'])
# initialize service proxy
self._srv_name = service_name
self._srv = ProxyServiceCaller({self._srv_name: VacuumGripperControl})
self._srv_req = VacuumGripperControlRequest()
if enable:
self._srv_req.enable = True
else:
self._srv_req.enable = False
def __init__(self):
super(StopHandTracking, self).__init__(outcomes=['done'])
self.TRACKING_TOPIC = '/handtracking_server/stop'
self._srv = ProxyServiceCaller({
self.TRACKING_TOPIC: Handtracking
})
Logger.loginfo('waiting for handtracking service server')
while not self._srv.is_available(self.TRACKING_TOPIC):
rospy.Rate(10).sleep()
Logger.loginfo('service found.')
def __init__(self, activation):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ControlFeederState, self).__init__(outcomes = ['succeeded', 'failed'])
# Store state parameter for later use.
self._activation = activation
# initialize service proxy
if self._activation:
self._srv_name = '/start_spawn'
else:
self._srv_name = '/stop_spawn'
self._srv = ProxyServiceCaller({self._srv_name: Empty})
self._srv_req = EmptyRequest()
def __init__(self, service, value):
super(SetBoolState,
self).__init__(outcomes=['true', 'false', 'failure'],
output_keys=['success', 'message'])
# Store state parameter for later use.
self._value = value
self._service = service
# get service caller
self._service_caller = ProxyServiceCaller({service: SetBool})
self._response = None
# It may happen that the service caller fails to send the action goal.
self._error = False
def __init__(self, name, timeout=2.0):
"""Constructor"""
super(CheckForPersonState,
self).__init__(outcomes=['found', 'not_found', 'failed'])
self._name = name
self._mutex = Lock()
self._people = []
self._timeout = timeout
self._start_time = None
self._callback_received = False
self._transform_topic = '/clf_perception_vision/people/raw/transform'
self._service_name = 'pepper_face_identification'
self._service_proxy = ProxyServiceCaller(
{self._service_name: DoIKnowThatPerson})
self._transform_listener = ProxySubscriberCached(
{self._transform_topic: ExtendedPeople})
def __init__(self,
pose_topic,
follow_joint_state_controller='joint_state_head',
discomfort_time=1000.0,
neck_control_parameteres=[-0.13, 0.3, 0.20, 0.2],
deactivate=True,
controlled_chains=['head', 'eyes']):
super(SweetieBotFollowHeadPoseSmart,
self).__init__(outcomes=['failed', 'too_close'])
# store state parameter for later use.
self._pose_topic = pose_topic
if len(neck_control_parameteres) != 4:
raise TypeError(
'SweetieBotFollowHeadPoseSmart: neck_control_parameteres must be float[4]'
)
self._neck_params = neck_control_parameteres
self._discomfort_time = discomfort_time
self._controller = 'motion/controller/' + follow_joint_state_controller
self._deactivate = deactivate
self._control_head = 'head' in controlled_chains
self._control_eyes = 'eyes' in controlled_chains
# setup proxies
self._set_operational_caller = ProxyServiceCaller(
{self._controller + '/set_operational': SetBool})
self._pose_subscriber = ProxySubscriberCached(
{self._pose_topic: PoseStamped})
self._joints_publisher = ProxyPublisher(
{self._controller + '/in_joints_ref': JointState})
# head inverse kinematics
self._ik = HeadIK()
# state
self._neck_angle = None
self._comfortable_stamp = None
# error in enter hook
self._error = False
class precise_movement(EventState):
'''
Example for a state to demonstrate which functionality is available for state implementation.
This example lets the behavior wait until the given target_time has passed since the behavior has been started.
-- target_heading float Time which needs to have passed since the behavior started.
<= Done Given time has passed.
<= Failed Example for a failure outcome.
'''
def __init__(self, mode, target_distance):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(MovementServiceCaller, self).__init__(outcomes = ['Done', 'Failed'])
#To catch the sent srv_type in the state parameter
self._topic = "/autonomous/precise_movement"
self._srv_type = dist
self._mode = mode
self._target = target_distance
self.service_client = ProxyServiceCaller({self._topic: self._srv_type}) # pass required clients as dict (topic: type)
Logger.loginfo("initiated %s service caller" %self._topic)