class CaptureCharUcoState(EventState):
"""
Publishes a pose from userdata so that it can be displayed in rviz.
-- robot_name string Robots name to move
<= done Calib done.
<= fail Calib fail.
"""
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 execute(self, _):
if not self.get_feedback_client.is_available(self.get_feedback_service):
rospy.logerr("get_feedback_service is not available!")
return 'fail'
try:
arm_feedback = self.get_feedback_client.call(self.get_feedback_service, 'arm')
except rospy.ServiceException as e:
rospy.logerr("Service call failed: %s" % e)
return 'fail'
req = hand_eye_calibrationRequest()
req.end_trans.translation.x = arm_feedback.group_pose.position.x
req.end_trans.translation.y = arm_feedback.group_pose.position.y
req.end_trans.translation.z = arm_feedback.group_pose.position.z
req.end_trans.rotation.x = arm_feedback.group_pose.orientation.x
req.end_trans.rotation.y = arm_feedback.group_pose.orientation.y
req.end_trans.rotation.z = arm_feedback.group_pose.orientation.z
req.end_trans.rotation.w = arm_feedback.group_pose.orientation.w
if not self.hand_eye_client.is_available(self.hand_eye_service):
rospy.logerr("get_feedback_service is not available!")
return 'fail'
try:
res = self.hand_eye_client.call(self.hand_eye_service, req)
except rospy.ServiceException as e:
rospy.logerr("Service call failed: %s" % e)
return 'fail'
return 'done'
def on_enter(self, _):
time.sleep(0.2)
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'))
class StopHandTracking(EventState):
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 execute(self, d):
Logger.loginfo('stopping handtracking ...')
req = HandtrackingRequest()
#req.carrying = True
self._srv.call(self.TRACKING_TOPIC, req)
Logger.loginfo('done')
return 'done'
class Trans2Robot(EventState):
'''
Get the pose in robot coordinate from pose in camera coordinate
-- robot_name string Robots name to move
># c_trans float[16] transformation matrix in camera coordinate with shape -1
#> pos float[3] pos with meter in robot coordinate
#> euler float[3] euler with degrees in robot coordinate
#> trans float[16] transformation matrix in robot coordinate
<= done Robot move done.
<= failed Robot move failed.
'''
def __init__(self, robot_name):
'''
Constructor
'''
super(Trans2Robot,
self).__init__(outcomes=['done', 'failed'],
input_keys=['c_trans'],
output_keys=['trans', 'pos', 'euler'])
self.robot_name = robot_name
self.trans2robot_service = self.robot_name + '/eye_trans2base'
self.trans2robot_client = ProxyServiceCaller(
{self.trans2robot_service: eye2base})
def execute(self, userdata):
'''
Execute this state
'''
if len(userdata.pix_and_depth) != 16:
rospy.logerr("data illegal")
return 'failed'
req = eye2baseRequest()
req.ini_pose = userdata.c_trans
try:
res = self.trans2robot_client.call(self.trans2robot_service, req)
except rospy.ServiceException as e:
rospy.logerr("Service call failed: %s" % e)
return 'failed'
userdata.trans = res.trans
userdata.pos = res.pos
userdata.euler = res.euler
return 'done'
def on_enter(self, userdata):
if not self.trans2robot_client.is_available(self.trans2robot_service):
rospy.logerr("trans2robot_service is not available!")
return 'failed'
def stop(self):
pass
class SaraNLUreceptionist(EventState):
'''
Use wm_nlu to parse a sentence and return the answer.
># sentence string sentence to parse
#> answer string answer
<= understood Finished job.
<= not_understood Finished job but no commands detected.
<= fail Finished job but sentence not successfully parsed or service unavailable.
'''
def __init__(self):
# See example_state.py for basic explanations.
super(SaraNLUreceptionist,
self).__init__(outcomes=['understood', 'fail'],
input_keys=['sentence'],
output_keys=['answer'])
self.serviceName = "/Receptionist_NLU_Service"
Logger.loginfo("waiting forservice: " + self.serviceName)
self.serviceNLU = ProxyServiceCaller(
{self.serviceName: ReceptionistNLUService})
# self.service = rospy.ServiceProxy(serviceName, ReceptionistNLUService)
def execute(self, userdata):
if self.serviceNLU.is_available(self.serviceName):
try:
# Call the say service
srvRequest = ReceptionistNLUServiceRequest()
srvRequest.str = userdata.sentence
response = self.serviceNLU.call(self.serviceName, srvRequest)
if not response:
userdata.answer = "unknown"
return "fail"
# Checking the validity of the response
if response.response is "unknown":
userdata.answer = response.response
return "fail"
if response.response is "":
userdata.answer = "unknown"
return "fail"
userdata.answer = response.response
return "understood"
except rospy.ServiceException as exc:
Logger.logwarn("Receptionist NLU did not work: \n" + str(exc))
class Pix2Robot(EventState):
'''
Get the position in robot coordinate from pixel and depth in image
-- robot_name string Robots name to move
># pix_and_depth float[3] pixel x, y, and the depth with meter
#> pos float[3] pos with meter in robot coordinate
<= done Robot move done.
<= failed Robot move failed.
'''
def __init__(self, robot_name):
'''
Constructor
'''
super(Pix2Robot, self).__init__(outcomes=['done', 'failed'],
input_keys=['pix_and_depth'],
output_keys=['pos'])
self.robot_name = robot_name
self.pix2robot_service = self.robot_name + '/pix2base'
self.pix2robot_client = ProxyServiceCaller(
{self.pix2robot_service: eye2base})
def execute(self, userdata):
'''
Execute this state
'''
if len(userdata.pix_and_depth) != 3:
rospy.logerr("data illegal")
return 'failed'
req = eye2baseRequest()
req.ini_pose = userdata.pix_and_depth
try:
res = self.pix2robot_client.call(self.pix2robot_service, req)
except rospy.ServiceException as e:
rospy.logerr("Service call failed: %s" % e)
return 'failed'
userdata.pos = res.pos
return 'done'
def on_enter(self, userdata):
if not self.pix2robot_client.is_available(self.pix2robot_service):
rospy.logerr("pix2robot_service is not available!")
return 'failed'
def stop(self):
pass
class SaraSay(EventState):
"""
Make sara say something
-- sentence string What to say.
Can be a simple String or a lanmda function using input_keys.
(e.g., lambda x: "I found the "x[0] + " on the " + x[1] + "!").
-- input_keys string[] List of available input keys.
-- emotion int Set the facial expression.
(0=no changes, 1=happy, 2=sad, 3=straight mouth, 4=angry, 5=surprise, 6=blink, 7=party)
-- block Bool Should the robot finish it's sentence before continuing?
># input_keys object[] Input(s) to the sentence as a list of userdata.
The individual inputs can be accessed as list elements (see lambda expression example).
<= done what's said is said
"""
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 execute(self, userdata):
if self.block:
if self.sayClient.is_available(self.sayServiceTopic):
try:
# Call the say service
srvSay = say_serviceRequest()
srvSay.say.sentence = self.msg.sentence
srvSay.say.emotion = self.msg.emotion
self._response = self.sayClient.call(
self.sayServiceTopic, srvSay)
except rospy.ServiceException as exc:
Logger.logwarn("Sara say did not work: \n" + str(exc))
return 'done'
else:
Logger.logwarn(
"wm_tts service is not available. Remaining trials: " +
str(self.maxBlockCount))
# Count the number of trials before continuing on life
self.maxBlockCount -= 1
if self.maxBlockCount <= 0:
return 'done'
else:
return 'done'
def on_enter(self, userdata):
if self.emotion.data is not 0:
# Save the previous emotion so we can place it back later
if self.block and self.emotionSub.has_msg(self.emotion_topic):
self.lastEmotion = self.emotionSub.get_last_msg(
self.emotion_topic)
self.emotionSub.remove_last_msg(self.emotion_topic)
# Set the emotion
self.emotionPub.publish(self.emotion_topic, self.emotion)
# Set the message according to the lambda function if needed.
if isinstance(
self.sentence,
types.LambdaType) and self.sentence.__name__ == "<lambda>":
self.msg.sentence = self.sentence(
map(lambda key: userdata[key],
list(reversed(list(self._input_keys)))))
else:
self.msg.sentence = self.sentence
Logger.loginfo('Sara is saying: "' + str(self.msg.sentence) + '".')
# Say the thing if not blocking
if not self.block:
# publish
self.sayPub.publish(self.say_topic, self.msg)
return "done"
# Set the maximum tries before continuing
self.maxBlockCount = 30
def on_exit(self, userdata):
if self.block and self.emotion.data is not 0:
# Put the original emotion back
if self.lastEmotion:
self.emotionPub.publish(self.emotion_topic, self.lastEmotion)
class ComputeCalibrationState(EventState):
"""
Publishes a pose from userdata so that it can be displayed in rviz.
-- robot_name string Robots name to move
<= done Calib done.
<= fail Calib fail.
"""
def __init__(self, robot_name):
"""Constructor"""
super(ComputeCalibrationState, self).__init__(outcomes=['done', 'fail'])
self.robot_name = robot_name
self.move_mode = 'p2p'
self.pose_indx = 0
self.compute_calibration_service = '/camera/compute_calibration'
self.hand_eye_client = ProxyServiceCaller({self.compute_calibration_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 execute(self, _):
req = hand_eye_calibrationRequest()
req.is_done = True
if not self.hand_eye_client.is_available(self.compute_calibration_service):
rospy.logerr('compute_calibration_service is not available!')
return 'fail'
try:
res = self.hand_eye_client.call(self.compute_calibration_service, req)
except rospy.ServiceException as e:
rospy.logerr("Service call failed: %s" % e)
if len(res.end2cam_trans) == 16:
trans_mat = np.array(res.end2cam_trans).reshape(4,4)
# camera_mat = np.array(res.camera_mat).reshape(4, 4)
print('##################################################################')
print(trans_mat)
print('##################################################################')
config = ConfigParser.ConfigParser()
config.optionxform = str
rospack = rospkg.RosPack()
hand_eye_path = rospack.get_path('hand_eye')
config.read(hand_eye_path + '/config/' + self.robot_name + '_img_trans.ini')
config.set("External", "Key_1_1", str(trans_mat[0][0]))
config.set("External", "Key_1_2", str(trans_mat[0][1]))
config.set("External", "Key_1_3", str(trans_mat[0][2]))
config.set("External", "Key_1_4", str(trans_mat[0][3]))
config.set("External", "Key_2_1", str(trans_mat[1][0]))
config.set("External", "Key_2_2", str(trans_mat[1][1]))
config.set("External", "Key_2_3", str(trans_mat[1][2]))
config.set("External", "Key_2_4", str(trans_mat[1][3]))
config.set("External", "Key_3_1", str(trans_mat[2][0]))
config.set("External", "Key_3_2", str(trans_mat[2][1]))
config.set("External", "Key_3_3", str(trans_mat[2][2]))
config.set("External", "Key_3_4", str(trans_mat[2][3]))
config.write(open(hand_eye_path + '/config/' + self.robot_name + '_img_trans.ini', 'wb'))
rospy.loginfo('f**k')
else:
rospy.logerr('calibration compute fail')
return 'fail'
return 'done'
def on_enter(self, _):
time.sleep(0.2)
