class depth_control(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_depth float the desired depth to reach.
<= Done Given time has passed.
<= Failed Example for a failure outcome.
'''
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 execute(self, userdata):
# This method is called periodically while the state is active.
# Main purpose is to check state conditions and trigger a corresponding outcome.
Logger.loginfo("Executing The service %s" % self._topic)
self.service_client.call(self._topic, self._depth)
return 'Done' # One of the outcomes declared above.
# If no outcome is returned, the state will stay active.
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.
# The following code is just for illustrating how the behavior logger works.
# Text logged by the behavior logger is sent to the operator and displayed in the GUI.
pass
def on_exit(self, userdata):
# This method is called when an outcome is returned and another state gets active.
# It can be used to stop possibly running processes started by on_enter.
pass # Nothing to do in this example.
def on_start(self):
# This method is called when the behavior is started.
# If possible, it is generally better to initialize used resources in the constructor
# because if anything failed, the behavior would not even be started.
pass # In this example, we use this event to set the correct start time.
def on_stop(self):
# This method is called whenever the behavior stops execution, also if it is cancelled.
# Use this event to clean up things like claimed resources.
pass # Nothing to do in this example.
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 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)
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 DetectPlaneState(EventState):
'''
The state that the camera detect the plane to place the object.
The detect part is done by Apriltag
<= continue Given time has passed.
<= failed Example for a failure outcome.
'''
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 execute(self, userdata):
# This method is called periodically while the state is active.
# Main purpose is to check state conditions and trigger a corresponding outcome.
# If no outcome is returned, the state will stay active.
if self._failed or self._srv_result.success is False:
return 'failed'
else:
return "continue"
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.
# The following code is just for illustrating how the behavior logger works.
# Text logged by the behavior logger is sent to the operator and displayed in the GUI.
try:
self._srv_result = self._srv.call(self._srv_topic,
TriggerRequest())
except Exception as e:
Logger.logwarn('Failed to send service call:\n%s' % str(e))
self._failed = True
def on_exit(self, userdata):
pass # Nothing to do in this service.
def on_start(self):
pass # Nothing to do in this service.
def on_stop(self):
pass # Nothing to do in this service.
class Wait_getCloserVictim(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_time float Time which needs to have passed since the behavior started.
<= continue Given time has passed.
<= failed Example for a failure outcome.
'''
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 execute(self, userdata):
if self.preempt_requested():
self.service_preempt()
return 'preempted'
request = GetTaskRequest()
# before userdata.task_details_task_id
request.task_id = 'victim_0_autonomous'
try:
response = self.getTask.call(self._topic_get_task, request).task
except Exception, e:
Logger.loginfo('[behavior_get_closer] Could not get task:' +
str(e))
rospy.sleep(0.5)
return 'waiting'
if not response.details.floatParams[
SarTaskTypes.
INDEX_VICTIM_DISTANCE] == userdata.params_distance:
userdata.pose.pose = response.pose
userdata.params_distance = response.details.floatParams[
SarTaskTypes.INDEX_VICTIM_DISTANCE]
return 'restart'
else:
rospy.sleep(0.5)
return 'waiting'
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 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 Wait_getCloserVictim(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_time float Time which needs to have passed since the behavior started.
<= continue Given time has passed.
<= failed Example for a failure outcome.
'''
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 execute(self, userdata):
if self.preempt_requested():
self.service_preempt()
return 'preempted'
request = GetTaskRequest()
# before userdata.task_details_task_id
request.task_id = 'victim_0_autonomous'
try:
response = self.getTask.call(self._topic_get_task, request).task
except Exception, e:
Logger.loginfo('[behavior_get_closer] Could not get task:' + str(e))
rospy.sleep(0.5)
return 'waiting'
if not response.details.floatParams[SarTaskTypes.INDEX_VICTIM_DISTANCE] == userdata.params_distance:
userdata.pose.pose = response.pose
userdata.params_distance = response.details.floatParams[SarTaskTypes.INDEX_VICTIM_DISTANCE]
return 'restart'
else:
rospy.sleep(0.5)
return 'waiting'
class DiscardVictim(EventState):
'''
Discard current victim
># victim string object_id of current victim
<= discarded Current victim was discarded.
'''
def __init__(self):
super(DiscardVictim, self).__init__(outcomes = ['discarded'], input_keys = ['victim'])
self._setVictimState = '/worldmodel/set_object_state'
self._srvSet = ProxyServiceCaller({self._setVictimState: SetObjectState})
def execute(self, userdata):
return 'discarded'
def on_enter(self, userdata):
state = ObjectState()
state.state = -2
request = SetObjectStateRequest(userdata.victim, state)
resp = self._srvSet.call(self._setVictimState, request)
Logger.loginfo('%(x)s discarded' % {
'x': userdata.victim
})
return 'discarded'
def on_exit(self, userdata):
pass
def on_start(self):
pass
def on_stop(self):
pass
class SetMappingState(EventState):
'''
Activate or deactivate mapping.
-- active bool Mapping changed to active or inactive.
<= succeeded Mapping changed.
'''
def __init__(self, active):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(SetMappingState, self).__init__(outcomes = ['succeeded'])
self._mappingTopicSet = '/mapper/set_mode'
self._srvSet = ProxyServiceCaller({self._mappingTopicSet: SetMode})
#self.set_mapper_mode = rospy.ServiceProxy('/mapper/set_mode', SetMode)
self._switch = active
def execute(self, userdata):
return 'succeeded'
def on_enter(self, userdata):
#map_state = userdata.switch
#resp = self.set_mapper_mode(True, map_state, True)
request = SetModeRequest(True,self._switch, True)
#request.map = self._switch
resp = self._srvSet.call(self._mappingTopicSet, request)
def on_exit(self, userdata):
pass
def on_start(self):
pass
def on_stop(self):
pass
class RecognizeObjectState(EventState):
'''
Recognizes an object in the given pointcloud.
-- object_name string Name of the object to look for.
># pointcloud PointCloud2 The perception data to work on.
<= detected Object is detected.
<= not_detected Object is not detected.
<= failed Something regarding the service call went wrong.
'''
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 execute(self, userdata):
if self._failed or self._srv_result is None:
return 'failed'
if self._object_name in [s.data for s in self._srv_result.ids]:
return 'detected'
else:
return 'not_detected'
def on_enter(self, userdata):
srv_request = recognizeRequest(cloud=userdata.pointcloud)
self._failed = False
try:
self._srv_result = self._srv.call(self._srv_topic, srv_request)
print self._srv_result
except Exception as e:
rospy.logwarn('Failed to call object recognizer:\n\r%s' % str(e))
self._failed = True
class CreatePath(EventState):
"""
Record a path.
#> path Path Generated path.
<= succeeded Robot is now located at the specified waypoint.
<= retry Retry to operate robot.
"""
def __init__(self):
"""
Constructor
"""
super(CreatePath, self).__init__(outcomes=["succeeded", "retry"], output_keys=["path"])
self._serv = ProxyServiceCaller({"trajectory": GetRobotTrajectory})
self._start_time = None
def execute(self, userdata):
"""
Execute this state
"""
# wait for manual exit
pass
def on_enter(self, userdata):
self._start_time = rospy.Time.now()
def on_exit(self, userdata):
path = self._serv.call("trajectory", GetRobotTrajectoryRequest())
result = path.trajectory
result.poses = filter(lambda p: p.header.stamp > self._start_time, path.trajectory.poses)
userdata.path = result
def on_stop(self):
pass
def on_pause(self):
pass
def on_resume(self, userdata):
pass
class RecognizeObjectState(EventState):
'''
Recognizes an object in the given pointcloud.
-- object_name string Name of the object to look for.
># pointcloud PointCloud2 The perception data to work on.
<= detected Object is detected.
<= not_detected Object is not detected.
<= failed Something regarding the service call went wrong.
'''
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 execute(self, userdata):
if self._failed or self._srv_result is None:
return 'failed'
if self._object_name in [s.data for s in self._srv_result.ids]:
return 'detected'
else:
return 'not_detected'
def on_enter(self, userdata):
srv_request = recognizeRequest(cloud=userdata.pointcloud)
self._failed = False
try:
self._srv_result = self._srv.call(self._srv_topic, srv_request)
print self._srv_result
except Exception as e:
rospy.logwarn('Failed to call object recognizer:\n\r%s' % str(e))
self._failed = True
class MoveitExecuteTrajectoryState(EventState):
"""
Executes a given joint trajectory message.
># joint_trajectory JointTrajectory Trajectory to be executed.
<= done Trajectory has successfully finished its execution.
<= failed Failed to execute trajectory.
"""
def __init__(self):
"""
Constructor
"""
super(MoveitExecuteTrajectoryState, self).__init__(outcomes=["done", "failed"], input_keys=["joint_trajectory"])
self._topic = "/execute_kinematic_path"
self._srv = ProxyServiceCaller({self._topic: ExecuteKnownTrajectory})
self._failed = False
self._result = None
def execute(self, userdata):
if self._failed:
return "failed"
if self._result.error_code.val == MoveItErrorCodes.SUCCESS:
return "done"
else:
Logger.logwarn("Failed to execute trajectory: %d" % self._result.error_code.val)
return "failed"
def on_enter(self, userdata):
self._failed = False
request = ExecuteKnownTrajectoryRequest()
request.trajectory.joint_trajectory = userdata.joint_trajectory
request.wait_for_execution = True
try:
self._result = self._srv.call(self._topic, request)
except Exception as e:
Logger.logwarn("Was unable to execute joint trajectory:\n%s" % str(e))
self._failed = True
class CalculateIKState(EventState):
'''
Calculates a joint target from the given endeffector pose.
-- move_group string Name of the move group to be used for planning.
-- ignore_collisions bool Ignore collision, use carefully.
># eef_pose PoseStamped Target pose of the endeffector.
#> joint_config float[] Calculated joint target.
<= planned Found a plan to the target.
<= failed Failed to find a plan to the given target.
'''
def __init__(self, move_group, ignore_collisions = False):
'''
Constructor
'''
super(CalculateIKState, self).__init__(outcomes=['planned', 'failed'],
input_keys=['eef_pose'],
output_keys=['joint_config'])
self._topic = '/compute_ik'
self._srv = ProxyServiceCaller({self._topic: GetPositionIK})
self._move_group = move_group
self._result = None
self._failed = False
self._ignore_collisions = ignore_collisions
def execute(self, userdata):
'''
Execute this state
'''
if self._failed:
return 'failed'
if self._result.error_code.val == MoveItErrorCodes.SUCCESS:
userdata.joint_config = self._result.solution.joint_state.position
return 'planned'
else:
Logger.logwarn('IK calculation resulted in error %d' % self._result.error_code.val)
self._failed = True
return 'failed'
def on_enter(self, userdata):
self._failed = False
request = GetPositionIKRequest()
request.ik_request.group_name = self._move_group
request.ik_request.avoid_collisions = not self._ignore_collisions
request.ik_request.pose_stamped = userdata.eef_pose
try:
self._result = self._srv.call(self._topic, request)
except Exception as e:
Logger.logwarn('Failed to calculate IK!\n%s' % str(e))
self._failed = True
class SweetieBotFollowHeadPoseSmart(EventState):
'''
SweetieBot follows object with head and eyes. Object is specified by PoseStamped on focus_topic.
Robot tries to keep comfort distance beetween object and head. If it is not possible, corresponding
outcome may be triggered.
If distance between head is object is smaller then `distance_uncomfortable` set joint51 to `neck_angle_uncomfortable`.
If it is greater then `distance_comfortable` set joint51 to `neck_angle_comfortable`.
-- pose_topic string geometry_msgs.msg.PoseStamped topic, where object pose is published.
-- follow_joint_state_controller string FollowJointState controller name without prefix.
-- discomfort_time boolean If distance beetween head and object is less then `distance_uncomfortable` for `discomfort_time` seconds then `too_close` outcome is triggered.
-- neck_control_parameteres float[] [ neck_angle_cofortable, distance_comfortable, neck_angle_uncofortable, distance_uncomfortable ]
-- deactivate boolean Deactivate controller on exit state.
-- controlled_chains string[] List of controlled kinematics chains, may contains 'head', 'eyes'.
<= failed Unable to activate state (controller is unavailable and etc)
<= too_close Object is too close to head.
'''
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
def on_enter(self, userdata):
self._error = False
# activate head controller
try:
res = self._set_operational_caller.call(
self._controller + '/set_operational', True)
except Exception as e:
Logger.logwarn(
'SweetieBotFollowHeadPoseSmart: Failed to activate `' +
self._controller + '` controller:\n%s' % str(e))
self._error = True
return
if not res.success:
Logger.logwarn(
'SweetieBotFollowHeadPoseSmart: Failed to activate `' +
self._controller +
'` controller (SetBoolResponse: success = false).')
self._error = True
return
# set default value
self._neck_angle = self._neck_params[0]
self._comfortable_stamp = rospy.Time.now()
Logger.loginfo(
'SweetieBotFollowHeadLeapMotion: controller `{0}` is active.'.
format(self._controller))
def execute(self, userdata):
if self._error:
return 'failed'
# check if new message is available
if self._pose_subscriber.has_msg(self._pose_topic):
# get object position
pose = self._pose_subscriber.get_last_msg(self._pose_topic)
# convert to PointStamped
focus_point = PointStamped()
focus_point.header = Header(frame_id=pose.header.frame_id)
focus_point.point = pose.pose.position
head_joints_msg = None
eyes_joints_msg = None
# CALCULATE HEAD POSITION
if self._control_head:
# calculate comfort heck_angle
try:
# convert point coordinates to bone54 frame
fp = self._ik._tf.transformPoint('bone54',
focus_point).point
# distance and direction angle
dist = math.sqrt(fp.x**2 + fp.y**2 + fp.z**2)
angle = math.acos(fp.x / dist)
# Logger.loginfo('SweetieBotFollowHeadLeapMotion: dist: %s, angle: %s' % (str(dist), str(angle)))
# check comfort distance
if angle < math.pi / 4:
if dist > self._neck_params[1]:
self._neck_angle = self._neck_params[0]
self._comfortable_stamp = rospy.Time.now()
elif dist < self._neck_params[3]:
self._neck_angle = self._neck_params[2]
# check if discomfort timer is elasped
if (rospy.Time.now() - self._comfortable_stamp
).to_sec() > self._discomfort_time:
return 'too_close'
else:
self._comfortable_stamp = rospy.Time.now()
else:
self._comfortable_stamp = rospy.Time.now()
except tf.Exception as e:
Logger.logwarn(
'SweetieBotFollowHeadPoseSmart: Cannot transform to bone54:\n%s'
% str(e))
self._neck_angle = self._neck_params[0]
# calculate head pose for given angle
head_joints_msg = self._ik.pointDirectionToHeadPose(
focus_point, self._neck_angle, 0.0)
# CALCULATE EYES POSE
if self._control_eyes:
eyes_joints_msg = self._ik.pointDirectionToEyesPose(
focus_point)
# PUBLISH POSE
if head_joints_msg:
if eyes_joints_msg:
# join head and eyes pose
head_joints_msg.name += eyes_joints_msg.name
head_joints_msg.position += eyes_joints_msg.position
# publish pose
self._joints_publisher.publish(
self._controller + '/in_joints_ref', head_joints_msg)
elif eyes_joints_msg:
# publish pose
self._joints_publisher.publish(
self._controller + '/in_joints_ref', eyes_joints_msg)
def on_exit(self, userdata):
if self._deactivate:
self.on_stop()
def on_stop(self):
try:
res = self._set_operational_caller.call(
self._controller + '/set_operational', False)
except Exception as e:
Logger.logwarn(
'SweetieBotFollowHeadPoseSmart: Failed to deactivate `' +
self._controller + '` controller:\n%s' % str(e))
Logger.loginfo(
'SweetieBotFollowHeadPoseSmart: controller `{0}` is deactivated.'.
format(self._controller))
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 RandJointsMovements(EventState):
'''
Periodically reposition joints to random point using given FollowJointState controller.
-- controller string FollowJointState controller name without `controller` prefix.
-- duration float How long this state will be executed (seconds).
-- interval float[2] Maximal and minimal interval between movements in form [min, max].
-- joints string[] Joint names.
-- minimal float[] Minimal values for joint positions.
-- maximal float[] Maximal values for joint positions.
># config dict Dictionary with keys 'duration', 'interval', 'joints', 'min' and 'max' to override default configuration. dict or key values can be set to None to use default value from parameters.
<= done Finished.
<= failed Failed to activate FollowJointState controller.
'''
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 set_movement_parameters(self, duration, interval, joints, minimal,
maximal):
if not isinstance(duration, (int, float)) or duration < 0:
raise ValueError("Duration must be nonegative number.")
if len(interval) != 2 or any([
not isinstance(t, (int, float)) for t in interval
]) or any([t < 0 for t in interval]) or interval[0] > interval[1]:
raise ValueError("Interval must represent interval of time.")
if len(joints) != len(minimal) or len(joints) != len(maximal):
raise ValueError(
"joints, minimal and maximal arrays must have the same size.")
if any([not isinstance(v, str) for v in joints]):
raise ValueError("joints: list of string is expected.")
if any([not isinstance(v, (int, float)) for v in minimal]):
raise ValueError("minimal: list of numbers is expected.")
if any([not isinstance(v, (int, float)) for v in maximal]):
raise ValueError("maximal: list of numbers is expected.")
self._duration = Duration.from_sec(duration)
self._interval = interval
self._joints = joints
self._minimal = minimal
self._maximal = maximal
def on_enter(self, userdata):
self._error = False
# override configuration if necessary
try:
if userdata.config != None:
# process dict
duration = userdata.config.get("duration",
self._duration.to_sec())
interval = userdata.config.get("interval", self._interval)
joints = userdata.config.get("joints", self._joints)
minimal = userdata.config.get("min2356", self._minimal)
maximal = userdata.config.get("max2356", self._maximal)
# check parameters
self.set_movement_parameters(duration, interval, joints,
minimal, maximal)
except Exception as e:
Logger.logwarn('Failed to process `config` input key:\n%s' %
str(e))
self._error = True
return
# activate controller
# TODO use actionlib
try:
res = self._set_operational_caller.call(
self._controller + '/set_operational', True)
except Exception as e:
Logger.logwarn('Failed to activate `' + self._controller +
'` controller:\n%s' % str(e))
self._error = True
return
if not res.success:
Logger.logwarn('Failed to activate `' + self._controller +
'` controller (SetBoolResponse: success = false).')
self._error = True
# set start timestamp
self._start_timestamp = Time.now()
self._movement_timestamp = Time.now()
Logger.loginfo('Start random pose generation, controller `%s`.' %
self._controller)
def execute(self, userdata):
if self._error:
return 'failed'
# check if time elasped
if Time.now() - self._start_timestamp > self._duration:
return 'done'
# check if we have tosend new point
if Time.now() - self._movement_timestamp > self._movement_duration:
# determine new interval
self._movement_timestamp = Time.now()
self._movement_duration = Duration.from_sec(
random.uniform(*self._interval))
# form message
msg = JointState()
msg.header = Header(stamp=Time.now())
msg.name = self._joints
for i in range(0, len(self._joints)):
x = random.uniform(0, 1)
msg.position.append(self._minimal[i] * x + self._maximal[i] *
(1 - x))
# send message
try:
self._publisher.publish(self._controller + '/in_joints_ref',
msg)
except Exception as e:
Logger.logwarn('Failed to send JointState message `' +
self._topic + '`:\n%s' % str(e))
def on_exit(self, userdata):
try:
res = self._set_operational_caller.call(
self._controller + '/set_operational', False)
except Exception as e:
Logger.logwarn('Failed to deactivate `' + self._controller +
'` controller:\n%s' % str(e))
return
Logger.loginfo('Done random pose generation.')
class ControlFeederState(EventState):
'''
State to start and stop the feeder in the conveyor belt in the factory simulation.
-- activation bool If 'true' the state instance starts the feeder, otherwise it stops it
<= succeeded The feeder was succesfully started or stopped.
<= failed There was a problem controlling the feeder.
'''
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 execute(self, userdata):
# This method is called periodically while the state is active.
# Main purpose is to check state conditions and trigger a corresponding outcome.
# If no outcome is returned, the state will stay active.
if self._failed:
return 'failed'
else:
return 'succeeded'
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.
try:
self._srv_result = self._srv.call(self._srv_name, self._srv_req)
self._failed = False
except Exception as e:
Logger.logwarn('Could not update feeder status')
rospy.logwarn(str(e))
self._failed = True
def on_exit(self, userdata):
# This method is called when an outcome is returned and another state gets active.
# It can be used to stop possibly running processes started by on_enter.
pass # Nothing to do
def on_start(self):
# This method is called when the behavior is started.
# If possible, it is generally better to initialize used resources in the constructor
# because if anything failed, the behavior would not even be started.
pass
def on_stop(self):
# This method is called whenever the behavior stops execution, also if it is cancelled.
# Use this event to clean up things like claimed resources.
pass # Nothing to do
class MoveitToJointsDynState(EventState):
'''
Uses MoveIt to plan and move the specified joints to the target configuration.
-- move_group string Name of the move group to be used for planning.
Specified joint names need to exist in the given group.
-- offset float Some offset
-- tool_link string e.g. "vacuum_gripper1_suction_cup"
-- action_topic string Topic on which MoveIt is listening for action calls.
># joint_names string[] Names of the joints to set.
Does not need to specify all joints.
># joint_values float[] Target configuration of the joints.
Same order as their corresponding names in joint_names.
<= reached Target joint configuration has been reached.
<= planning_failed Failed to find a plan to the given joint configuration.
<= control_failed Failed to move the arm along the planned trajectory.
'''
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 execute(self, userdata):
'''
Execute this state
'''
if self._planning_failed:
return 'planning_failed'
if self._control_failed:
return 'control_failed'
if self._success:
return 'reached'
if self._traj_exec_result:
# look at our cached 'execute_trajectory' result to see whether
# execution was successful
result = self._traj_exec_result
if result.error_code.val == MoveItErrorCodes.CONTROL_FAILED:
Logger.logwarn(
'Control failed for move action of group: %s (error code: %s)'
% (self._current_group_name, str(result.error_code)))
self._control_failed = True
return 'control_failed'
elif result.error_code.val != MoveItErrorCodes.SUCCESS:
Logger.logwarn(
'Move action failed with result error code: %s' %
str(result.error_code))
self._planning_failed = True
return 'planning_failed'
else:
self._success = True
return 'reached'
def on_enter(self, userdata):
self._planning_failed = False
self._control_failed = False
self._success = False
self._joint_names = userdata.joint_names
self._initial_state = RobotState()
self._initial_state.joint_state.position = copy.deepcopy(
userdata.joint_values)
self._initial_state.joint_state.name = copy.deepcopy(self._joint_names)
#print self._initial_state.joint_state.name
#print self._initial_state.joint_state.position
self._srv_req = GetPositionFKRequest()
self._srv_req.robot_state = self._initial_state
self._srv_req.header.stamp = rospy.Time.now()
self._srv_req.header.frame_id = "world"
self._srv_req.fk_link_names = [self._tool_link]
try:
srv_result = self._fk_srv.call(self._fk_srv_topic, self._srv_req)
self._failed = False
except Exception as e:
Logger.logwarn('Could not call FK: ' + str(e))
self._planning_failed = True
return
grasp_pose = srv_result.pose_stamped[0].pose
grasp_pose_stamped = srv_result.pose_stamped[0]
# Create a pre-grasp approach pose with an offset of 0.3
pre_grasp_approach_pose = copy.deepcopy(grasp_pose_stamped)
pre_grasp_approach_pose.pose.position.z += self._offset + 0.3
# Create an object to MoveGroupInterface for the current robot.
self._mgi_active_robot = MoveGroupInterface(
self._current_group_name, self._current_group_name +
'_base_link') # TODO: clean up in on_exit
self._mgi_active_robot.moveToPose(pre_grasp_approach_pose,
self._tool_link)
# Use cartesian motions to pick the object.
cartesian_service_req = GetCartesianPathRequest()
cartesian_service_req.start_state.joint_state = rospy.wait_for_message(
self._current_group_name + '/joint_states',
sensor_msgs.msg.JointState)
cartesian_service_req.header.stamp = rospy.Time.now()
cartesian_service_req.header.frame_id = "world"
cartesian_service_req.link_name = self._tool_link
cartesian_service_req.group_name = self._current_group_name
cartesian_service_req.max_step = 0.01
cartesian_service_req.jump_threshold = 0
cartesian_service_req.avoid_collisions = True
grasp_pose.position.z += self._offset + 0.16 # this is basically the toolframe (with the box as the tool)
cartesian_service_req.waypoints.append(grasp_pose)
try:
cartesian_srv_result = self._cartesian_srv.call(
self._cartesian_srv_topic, cartesian_service_req)
self._failed = False
except Exception as e:
Logger.logwarn('Could not call Cartesian: ' + str(e))
self._planning_failed = True
return
if cartesian_srv_result.fraction < 1.0:
Logger.logwarn('Cartesian failed. fraction: ' +
str(cartesian_srv_result.fraction))
self._planning_failed = True
return
traj_goal = moveit_msgs.msg.ExecuteTrajectoryGoal()
traj_goal.trajectory = cartesian_srv_result.solution
self._traj_client.send_goal(traj_goal)
self._traj_client.wait_for_result()
self._traj_exec_result = self._traj_client.get_result()
def on_exit(self, userdata):
pass
def on_stop(self):
try:
if self._client.is_available(self._action_topic) \
and not self._client.has_result(self._action_topic):
self._client.cancel(self._action_topic)
except:
# client already closed
pass
def on_pause(self):
self._client.cancel(self._action_topic)
def on_resume(self, userdata):
self.on_enter(userdata)
class CalculateCartesianPathState(EventState):
'''
Calculates the trajectory for a cartesian path to the given goal endeffector pose.
-- move_group string Name of the move group to be used for planning.
-- ignore_collisions bool Ignore collision, use carefully.
># eef_pose PoseStamped Target pose of the endeffector.
#> joint_trajectory JointTrajectory Calculated joint trajectory.
#> plan_fraction float Fraction of the planned path.
<= planned Found a plan to the target.
<= failed Failed to find a plan to the given target.
'''
def __init__(self, move_group, ignore_collisions = False):
'''
Constructor
'''
super(CalculateCartesianPathState, self).__init__(outcomes=['planned', 'failed'],
input_keys=['eef_pose'],
output_keys=['joint_trajectory', 'plan_fraction'])
self._topic = '/compute_cartesian_path'
self._srv = ProxyServiceCaller({self._topic: GetCartesianPath})
self._tf = ProxyTransformListener().listener()
self._move_group = move_group
self._result = None
self._failed = False
self._ignore_collisions = ignore_collisions
def execute(self, userdata):
'''
Execute this state
'''
if self._failed:
return 'failed'
if self._result.error_code.val == MoveItErrorCodes.SUCCESS and self._result.fraction > .2:
userdata.plan_fraction = self._result.fraction
userdata.joint_trajectory = self._result.solution.joint_trajectory
Logger.loginfo('Successfully planned %.2f of the path' % self._result.fraction)
return 'planned'
elif self._result.error_code.val == MoveItErrorCodes.SUCCESS:
Logger.logwarn('Only planned %.2f of the path' % self._result.fraction)
self._failed = True
return 'failed'
else:
Logger.logwarn('Failed to plan trajectory: %d' % self._result.error_code.val)
self._failed = True
return 'failed'
def on_enter(self, userdata):
self._failed = False
request = GetCartesianPathRequest()
request.group_name = self._move_group
request.avoid_collisions = not self._ignore_collisions
request.max_step = 0.05
request.header = userdata.eef_pose.header
request.waypoints.append(userdata.eef_pose.pose)
now = rospy.Time.now()
try:
self._tf.waitForTransform('base_link', 'gripper_cam_link', now, rospy.Duration(1))
(p,q) = self._tf.lookupTransform('gripper_cam_link', 'base_link', now)
c = OrientationConstraint()
c.header.frame_id = 'base_link'
c.header.stamp = now
c.link_name = 'gripper_cam_link'
c.orientation.x = q[0]
c.orientation.y = q[1]
c.orientation.z = q[2]
c.orientation.w = q[3]
c.weight = 1
c.absolute_x_axis_tolerance = 0.1
c.absolute_y_axis_tolerance = 0.1
c.absolute_z_axis_tolerance = 0.1
#request.path_constraints.orientation_constraints.append(c)
self._result = self._srv.call(self._topic, request)
except Exception as e:
Logger.logwarn('Exception while calculating path:\n%s' % str(e))
self._failed = True
class LocateFactoryDeviceState(EventState):
'''
State to get the exact location of the turtlebot in the factory simulation.
-- model_name string Name of the model (or link) in Gazebo
-- output_frame_id string Name of the reference frame in which the pose will be output
#> pose PoseStamped pose of the factory device in output_frame_id
<= succeeded
<= failed
'''
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 execute(self, userdata):
# This method is called periodically while the state is active.
# Main purpose is to check state conditions and trigger a corresponding outcome.
# If no outcome is returned, the state will stay active.
if self._failed:
return 'failed'
else:
# TODO: check 'success' field for actual success (service call could have
# succeeded, but looking up the pose inside gazebo could have failed).
tbp = PoseStamped()
tbp.header = self._srv_result.header
tbp.pose = self._srv_result.pose
userdata.pose = tbp
return 'succeeded'
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.
try:
self._srv_result = self._srv.call(self._srv_name, self._srv_req)
self._failed = False
except Exception as e:
Logger.logwarn('Could not get pose')
rospy.logwarn(str(e))
self._failed = True
def on_exit(self, userdata):
# This method is called when an outcome is returned and another state gets active.
# It can be used to stop possibly running processes started by on_enter.
pass # Nothing to do
def on_start(self):
# This method is called when the behavior is started.
# If possible, it is generally better to initialize used resources in the constructor
# because if anything failed, the behavior would not even be started.
pass
def on_stop(self):
# This method is called whenever the behavior stops execution, also if it is cancelled.
# Use this event to clean up things like claimed resources.
pass # Nothing to do
class SrdfStateToMoveitExecute(EventState):
'''
State to execute with MoveIt! a known trajectory defined in the "/robot_description_semantic" parameter (SRDF file) BEWARE! This state performs no self-/collison planning!
-- config_name string Name of the joint configuration of interest.
-- move_group string Name of the move group to be used for planning.
-- duration float Duration of the execution
Default to 1 second
-- action_topic string Topic on which MoveIt is listening for action calls.
Defualt to: /execute_kinematic_path
-- robot_name string Optional name of the robot to be used.
If left empty, the first one found will be used
(only required if multiple robots are specified in the same file).
># joint_config float[] Target configuration of the joints.
Same order as their corresponding names in joint_names.
<= reached Target joint configuration has been reached.
<= request_failed Failed to request the service.
<= moveit_failed Failed to execute the known trajectory.
'''
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.logerr(
'Unable to get parameter: /robot_description_semantic')
self._param_error = False
self._srdf = None
self._response = None
def execute(self, userdata):
'''
Execute this state
'''
if self._param_error:
return 'param_error'
if self._request_failed:
return 'request_failed'
if self._response.error_code.val != MoveItErrorCodes.SUCCESS:
Logger.logwarn('Move action failed with result error code: %s' %
str(self._response.error_code))
self._moveit_failed = True
return 'moveit_failed'
else:
Logger.loginfo('Move action succeeded: %s' %
str(self._response.error_code))
self._success = True
return 'reached'
def on_enter(self, userdata):
self._param_error = False
self._request_failed = False
self._moveit_failed = False
self._success = False
# Parameter check
if self._srdf_param is None:
self._param_error = True
return
try:
self._srdf = ET.fromstring(self._srdf_param)
except Exception as e:
Logger.logwarn(
'Unable to parse given SRDF parameter: /robot_description_semantic'
)
self._param_error = True
if not self._param_error:
robot = None
for r in self._srdf.iter('robot'):
if self._robot_name == '' or self._robot_name == r.attrib[
'name']:
robot = r
break
if robot is None:
Logger.logwarn('Did not find robot name in SRDF: %s' %
self._robot_name)
return 'param_error'
config = None
for c in robot.iter('group_state'):
if (self._move_group == '' or self._move_group == c.attrib['group']) \
and c.attrib['name'] == self._config_name:
config = c
self._move_group = c.attrib[
'group'] #Set move group name in case it was not defined
break
if config is None:
Logger.logwarn('Did not find config name in SRDF: %s' %
self._config_name)
return 'param_error'
try:
self._joint_config = [
float(j.attrib['value']) for j in config.iter('joint')
]
self._joint_names = [
str(j.attrib['name']) for j in config.iter('joint')
]
except Exception as e:
Logger.logwarn('Unable to parse joint values from SRDF:\n%s' %
str(e))
return 'param_error'
# Action Initialization
action_goal = ExecuteKnownTrajectoryRequest(
) # ExecuteTrajectoryGoal()
#action_goal.trajectory.joint_trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.3)
action_goal.trajectory.joint_trajectory.joint_names = self._joint_names
action_goal.trajectory.joint_trajectory.points = [
JointTrajectoryPoint()
]
action_goal.trajectory.joint_trajectory.points[
0].time_from_start = rospy.Duration(self._duration)
action_goal.wait_for_execution = self._wait_for_execution
action_goal.trajectory.joint_trajectory.points[
0].positions = self._joint_config
try:
self._response = self._client.call(self._action_topic,
action_goal)
Logger.loginfo(
"Execute Known Trajectory Service requested: \n" +
str(self._action_topic))
except rospy.ServiceException as exc:
Logger.logwarn(
"Execute Known Trajectory Service did not process request: \n"
+ str(exc))
self._request_failed = True
return
class SweetieBotRandHeadMovements(EventState):
'''
Periodically reposition eyes and head of SweetieBot to random point using given FollowJointState controller.
-- controller string FollowJointState controller name without `controller` prefix.
-- duration float How long this state will be executed (seconds).
-- interval float[2] Array of floats, maximal and minimal interval between movements.
-- max2356 float[4] Max values for joint52, joint53, eyes_pitch, eyes_yaw.
-- min2356 float[4] Min values for joint52, joint53, eyes_pitch, eyes_yaw.
># config dict Dictionary with keys 'duration', 'interval', 'max2356', 'min2356' to override default configuration. dict or key values can be set to None to use default value from parameters.
<= done Finished.
<= failed Failed to activate FollowJointState controller.
'''
def __init__(self,
controller='joint_state_head',
duration=120,
interval=[3, 5],
max2356=[0.3, 0.3, 1.5, 1.5],
min2356=[-0.3, -0.3, -1.5, -1.5]):
super(SweetieBotRandHeadMovements,
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, min2356, max2356)
# error in enter hook
self._error = False
def set_movement_parameters(self, duration, interval, min2356, max2356):
if not isinstance(duration, (int, float)) or duration < 0:
raise ValueError("Duration must be nonegative number.")
if len(interval) != 2 or any([
not isinstance(t, (int, float)) for t in interval
]) or any([t < 0 for t in interval]) or interval[0] > interval[1]:
raise ValueError("Interval must represent interval of time.")
if len(min2356) != 4 or any(
[not isinstance(v, (int, float)) for v in min2356]):
raise ValueError("min2356: list of four numbers was expected.")
if len(max2356) != 4 or any(
[not isinstance(v, (int, float)) for v in max2356]):
raise ValueError("max2356: list of four numbers was expected.")
self._duration = Duration.from_sec(duration)
self._interval = interval
self._min2356 = min2356
self._max2356 = max2356
def on_enter(self, userdata):
self._error = False
# override configuration if necessary
try:
if userdata.config != None:
# process dict
duration = userdata.config.get("duration",
self._duration.to_sec())
interval = userdata.config.get("interval", self._interval)
min2356 = userdata.config.get("min2356", self._min2356)
max2356 = userdata.config.get("max2356", self._max2356)
# check parameters
self.set_movement_parameters(duration, interval, min2356,
max2356)
except Exception as e:
Logger.logwarn('Failed to process `config` input key:\n%s' %
str(e))
self._error = True
return
# activate head controller
try:
res = self._set_operational_caller.call(
self._controller + '/set_operational', True)
except Exception as e:
Logger.logwarn('Failed to activate `' + self._controller +
'` controller:\n%s' % str(e))
self._error = True
return
if not res.success:
Logger.logwarn('Failed to activate `' + self._controller +
'` controller (SetBoolResponse: success = false).')
self._error = True
# set start timestamp
self._start_timestamp = Time.now()
self._movement_timestamp = Time.now()
Logger.loginfo(
'Start random pose generation (eyes, head), controller `%s`.' %
self._controller)
def execute(self, userdata):
if self._error:
return 'failed'
# check if time elasped
if Time.now() - self._start_timestamp > self._duration:
return 'done'
# check if we have tosend new point
if Time.now() - self._movement_timestamp > self._movement_duration:
# determine new interval
self._movement_timestamp = Time.now()
self._movement_duration = Duration.from_sec(
random.uniform(*self._interval))
# form message
msg = JointState()
msg.header = Header(stamp=Time.now())
msg.name = ['joint52', 'joint53', 'eyes_pitch', 'eyes_yaw']
# random durection
x = random.uniform(0, 1)
y = random.uniform(0, 1)
# compute head position
msg.position = [0, 0, 0, 0]
msg.position[0] = self._min2356[0] * x + self._max2356[0] * (1 - x)
msg.position[1] = self._min2356[1] * y + self._max2356[1] * (1 - y)
# compute eyes position
msg.position[2] = self._min2356[2] * y + self._max2356[2] * (1 - y)
msg.position[3] = self._min2356[3] * x + self._max2356[3] * (1 - x)
# send message
try:
self._publisher.publish(self._controller + '/in_joints_ref',
msg)
except Exception as e:
Logger.logwarn('Failed to send JointState message `' +
self._topic + '`:\n%s' % str(e))
def on_exit(self, userdata):
try:
res = self._set_operational_caller.call(
self._controller + '/set_operational', False)
except Exception as e:
Logger.logwarn('Failed to deactivate `' + self._controller +
'` controller:\n%s' % str(e))
return
Logger.loginfo('Done random pose generation for eyes and head.')
class AlignWithGate(EventState):
'''
alining with the gate in z and x direction.
basic algorithm, first start aligning in z after that align in x.
-- topic string Topic where the mesured result is published.
-- tolerance number The acceptable error for operations (absolute error).
-- keys string[] The key for the values to calculate the error.
-- referance object The referance ideal value.
#> output_value object The calculated error.
<= unacceptable Indicates an error that excedes tolerance.
<= success Indicates no need for alignment check anymore.
'''
#To be developed according to the optimization notes
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 execute(self, userdata):
# This method is called periodically while the state is active.
# Main purpose is to check state conditions and trigger a corresponding outcome.
# If no outcome is returned, the state will stay active.
Logger.loginfo("executing Align with gate")
self.update_subscriber()
if self.vision_sub.has_msg(self._v_topic):
Logger.loginfo("There is a msg coming")
if self.inrange_flag:
Logger.loginfo("start aligning with gate")
self.allign_in_z()
self.allign_in_x()
Logger.loginfo("Aligned with gate")
return "Aligned"
else:
Logger.logwarn("cant find the gate")
return "CantSee"
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.
# The following code is just for illustrating how the behavior logger works.
# Text logged by the behavior logger is sent to the operator and displayed in the GUI.
pass
def on_exit(self, userdata):
# This method is called when an outcome is returned and another state gets active.
# It can be used to stop possibly running processes started by on_enter.
pass # Nothing to do in this example.
def on_start(self):
# This method is called when the behavior is started.
# If possible, it is generally better to initialize used resources in the constructor
# because if anything failed, the behavior would not even be started.
pass# In this example, we use this event to set the correct start time.
def on_stop(self):
# This method is called whenever the behavior stops execution, also if it is cancelled.
# Use this event to clean up things like claimed resources.
pass # Nothing to do in this example.
def sway_left(self):
self.sway_left_client.call('/autonomous/sway_left',EmptyRequest())
def sway_right(self):
self.sway_right_client.call('/autonomous/sway_right',EmptyRequest())
def heave_down(self):
self.heave_down_client.call('/autonomous/heave_down',EmptyRequest())
def heave_up(self):
self.heave_up_client.call('/autonomous/heave_up',EmptyRequest())
def stop(self):
self.stop_client.call('/autonomous/stop_vehicle',EmptyRequest())
def update_subscriber (self):
if self.vision_sub.has_msg(self._v_topic):
msg = self.vision_sub.get_last_msg(self._v_topic)
self.ValZ = msg.z
self.ValX = msg.x
self.inrange_flag = msg.found
def allign_in_z(self):
self.update_subscriber()
Logger.loginfo("started aligning in Z ...")
while (self.height_l > self.ValZ or self.ValZ > self.height_h):
while (self.ValZ > self.height_h):
self.heave_up()
self.update_subscriber()
self.stop()
while (self.ValZ < self.height_l):
self.heave_down()
self.update_subscriber()
self.stop()
self.update_subscriber()
rospy.loginfo("Alligned in Z = %f...",self.ValZ)
def allign_in_x(self):
self.update_subscriber()
Logger.loginfo("started aligning in X ...")
while (self.width_l > self.ValX or self.ValX > self.width_h):
rospy.loginfo("X = %f...",self.ValX)
while (self.ValX > self.width_h):
self.sway_right()
rospy.loginfo("X = %f...",self.ValX)
self.update_subscriber()
self.stop()
while (self.ValX < self.width_l):
self.sway_left()
self.update_subscriber()
self.stop()
self.update_subscriber()
Logger.loginfo("Aligned in X")
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)
class MAVROSServiceCaller(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_time float Time which needs to have passed since the behavior started.
<= continue Given time has passed.
<= failed Example for a failure outcome.
'''
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)
# 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
#if srv_type == "SetMode":
# temp = self._target
# self._targe = SetModeRequest()
# self._target.custom_mode = temp
def execute(self, userdata):
# This method is called periodically while the state is active.
# Main purpose is to check state conditions and trigger a corresponding outcome.
# If no outcome is returned, the state will stay active.
if self._srv_type == SetMode:
Logger.loginfo("In the if statement")
Logger.loginfo("Executing The service %s and sending " %
self._topic)
self.service_client.call(self._topic,
SetModeRequest(0, self._target))
else:
Logger.loginfo("Executing The service %s and sending " %
self._topic)
self.service_client.call(self._topic, self._target)
return 'Done' # One of the outcomes declared above.
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.
# The following code is just for illustrating how the behavior logger works.
# Text logged by the behavior logger is sent to the operator and displayed in the GUI.
pass
def on_exit(self, userdata):
# This method is called when an outcome is returned and another state gets active.
# It can be used to stop possibly running processes started by on_enter.
pass # Nothing to do in this example.
def on_start(self):
# This method is called when the behavior is started.
# If possible, it is generally better to initialize used resources in the constructor
# because if anything failed, the behavior would not even be started.
pass # In this example, we use this event to set the correct start time.
def on_stop(self):
# This method is called whenever the behavior stops execution, also if it is cancelled.
# Use this event to clean up things like claimed resources.
pass # Nothing to do in this example.
class SrdfStateToMoveitExecute(EventState):
'''
State to execute with MoveIt! a known trajectory defined in the "/robot_description_semantic" parameter (SRDF file) BEWARE! This state performs no self-/collison planning!
-- config_name string Name of the joint configuration of interest.
-- move_group string Name of the move group to be used for planning.
-- duration float Duration of the execution
Default to 1 second
-- action_topic string Topic on which MoveIt is listening for action calls.
Defualt to: /execute_kinematic_path
-- robot_name string Optional name of the robot to be used.
If left empty, the first one found will be used
(only required if multiple robots are specified in the same file).
># joint_config float[] Target configuration of the joints.
Same order as their corresponding names in joint_names.
<= reached Target joint configuration has been reached.
<= request_failed Failed to request the service.
<= moveit_failed Failed to execute the known trajectory.
'''
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 execute(self, userdata):
'''
Execute this state
'''
if self._param_error:
return 'param_error'
if self._request_failed:
return 'request_failed'
if self._response.error_code.val != MoveItErrorCodes.SUCCESS:
Logger.logwarn('Move action failed with result error code: %s' % str(self._response.error_code))
self._moveit_failed = True
return 'moveit_failed'
else:
Logger.loginfo('Move action succeeded: %s' % str(self._response.error_code))
self._success = True
return 'reached'
def on_enter(self, userdata):
self._param_error = False
self._request_failed = False
self._moveit_failed = False
self._success = False
# Parameter check
if self._srdf_param is None:
self._param_error = True
return
try:
self._srdf = ET.fromstring(self._srdf_param)
except Exception as e:
Logger.logwarn('Unable to parse given SRDF parameter: /robot_description_semantic')
self._param_error = True
if not self._param_error:
robot = None
for r in self._srdf.iter('robot'):
if self._robot_name == '' or self._robot_name == r.attrib['name']:
robot = r
break
if robot is None:
Logger.logwarn('Did not find robot name in SRDF: %s' % self._robot_name)
return 'param_error'
config = None
for c in robot.iter('group_state'):
if (self._move_group == '' or self._move_group == c.attrib['group']) \
and c.attrib['name'] == self._config_name:
config = c
self._move_group = c.attrib['group'] #Set move group name in case it was not defined
break
if config is None:
Logger.logwarn('Did not find config name in SRDF: %s' % self._config_name)
return 'param_error'
try:
self._joint_config = [float(j.attrib['value']) for j in config.iter('joint')]
self._joint_names = [str(j.attrib['name']) for j in config.iter('joint')]
except Exception as e:
Logger.logwarn('Unable to parse joint values from SRDF:\n%s' % str(e))
return 'param_error'
# Action Initialization
action_goal = ExecuteKnownTrajectoryRequest() # ExecuteTrajectoryGoal()
#action_goal.trajectory.joint_trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.3)
action_goal.trajectory.joint_trajectory.joint_names = self._joint_names
action_goal.trajectory.joint_trajectory.points = [JointTrajectoryPoint()]
action_goal.trajectory.joint_trajectory.points[0].time_from_start = rospy.Duration(self._duration)
action_goal.wait_for_execution = self._wait_for_execution
action_goal.trajectory.joint_trajectory.points[0].positions = self._joint_config
try:
self._response = self._client.call(self._action_topic, action_goal)
Logger.loginfo("Execute Known Trajectory Service requested: \n" + str(self._action_topic))
except rospy.ServiceException as exc:
Logger.logwarn("Execute Known Trajectory Service did not process request: \n" + str(exc))
self._request_failed = True
return
class SetConveyorPowerState(EventState):
'''
State to update the speed of the conveyor belt through a service call (0 to stop it, 100 max), in the factory simulation.
-- stop bool If 'true' the state instance stops the conveyor belt, ignoring the speed inputkey
># speed float Value to set the speed of the conveyor belt.
<= succeeded Speed of the conveyor belt has been succesfully set.
<= failed There was a problem setting the speed.
'''
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 execute(self, userdata):
# This method is called periodically while the state is active.
# Main purpose is to check state conditions and trigger a corresponding outcome.
# If no outcome is returned, the state will stay active.
if self._failed:
return 'failed'
if self._srv_result.success is True:
return 'succeeded'
else:
return 'failed'
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.speed = float(userdata.speed)
# create service request depending on activation parameter and userdata
self._srv_req = ConveyorBeltControlRequest()
if self._stop:
self._srv_req.state.power = 0.0
else:
self._srv_req.state.power = self.speed
try:
self._srv_result = self._srv.call(self._srv_name, self._srv_req)
self._failed = False
except Exception as e:
Logger.logwarn('Could not update conveyor belt speed')
rospy.logwarn(str(e))
self._failed = True
def on_exit(self, userdata):
# This method is called when an outcome is returned and another state gets active.
# It can be used to stop possibly running processes started by on_enter.
pass # Nothing to do
def on_start(self):
# This method is called when the behavior is started.
# If possible, it is generally better to initialize used resources in the constructor
# because if anything failed, the behavior would not even be started.
pass
def on_stop(self):
# This method is called whenever the behavior stops execution, also if it is cancelled.
# Use this event to clean up things like claimed resources.
pass # Nothing to do
class ComputeGraspPartOffsetAriacState(EventState):
'''
Computes the joint configuration needed to grasp the part given its pose.
-- joint_names string[] Names of the joints
># offset float Some offset
># rotation float Rotation?
># move_group string Name of the group for which to compute the joint values for grasping.
># move_group_prefix string Name of the prefix of the move group to be used for planning.
># tool_link string e.g. "ee_link"
># part_pose Pose pose of the part on the tray
># pose PoseStamped pose of the part to pick
#> joint_values float[] joint values for grasping
#> joint_names string[] names of the joints
<= continue if a grasp configuration has been computed for the pose
<= failed otherwise.
'''
def __init__(self, joint_names):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ComputeGraspPartOffsetAriacState, self).__init__(outcomes = ['continue', 'failed'], input_keys = ['move_group', 'move_group_prefix', 'tool_link','pose', 'offset', 'rotation', 'part_pose'], output_keys = ['joint_values','joint_names'])
self._joint_names = joint_names
# 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)
def execute(self, userdata):
# This method is called periodically while the state is active.
# Main purpose is to check state conditions and trigger a corresponding outcome.
# If no outcome is returned, the state will stay active.
rospy.logwarn(userdata.pose)
if self._failed == True:
return 'failed'
if int(self._srv_result.error_code.val) == 1:
sol_js = self._srv_result.solution.joint_state
joint_names = self._joint_names
#['robot1_shoulder_pan_joint', 'robot1_shoulder_lift_joint', 'robot1_elbow_joint', 'robot1_wrist_1_joint', 'robot1_wrist_2_joint', 'robot1_wrist_3_joint'] # TODO: this needs to be a parameter
jname_idx = [sol_js.name.index(jname) for jname in joint_names]
j_angles = map(sol_js.position.__getitem__, jname_idx)
# solution_dict = dict(zip(joint_names, j_angles))
userdata.joint_values = copy.deepcopy(j_angles)
userdata.joint_names = copy.deepcopy(joint_names)
return 'continue'
else:
rospy.loginfo("ComputeGraspState::Execute state - failed. Returned: %d", int(self._srv_result.error_code.val) )
return 'failed'
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
userdata.part_pose.position.x
userdata.part_pose.position.y
userdata.part_pose.position.z
userdata.part_pose.orientation.x
userdata.part_pose.orientation.y
userdata.part_pose.orientation.z
userdata.part_pose.orientation.w
target_pose.pose.position.z += self._offset + 0.0
target_pose.pose.position.x += userdata.part_pose.position.x
target_pose.pose.position.y += userdata.part_pose.position.y
# 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)
res_q_b = quaternion_multiply(res_q, userdata.part_pose.orientation)
target_pose.pose.orientation = geometry_msgs.msg.Quaternion(*res_q_b)
# 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 on_exit(self, userdata):
# This method is called when an outcome is returned and another state gets active.
# It can be used to stop possibly running processes started by on_enter.
pass # Nothing to do
def on_start(self):
# This method is called when the behavior is started.
# If possible, it is generally better to initialize used resources in the constructor
# because if anything failed, the behavior would not even be started.
pass
def on_stop(self):
# This method is called whenever the behavior stops execution, also if it is cancelled.
# Use this event to clean up things like claimed resources.
pass # Nothing to do
class VacuumGripperControlState(EventState):
'''
State to control any suction gripper in the factory simulation of the MOOC "Hello (Real) World with ROS"
-- enable bool 'true' to activates the gripper, 'false' to deactivate it
-- service_name string topic name for the service corresponding to the gripper to control
<= continue if the gripper activation or de-activation has been succesfully achieved
<= failed otherwise
'''
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 execute(self, userdata):
# This method is called periodically while the state is active.
# Main purpose is to check state conditions and trigger a corresponding outcome.
# If no outcome is returned, the state will stay active.
if self._failed:
return 'failed'
if self._srv_result.success is True:
return 'continue'
else:
Logger.logwarn('Could not change gripper state')
return 'failed'
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.
try:
self._srv_result = self._srv.call(self._srv_name, self._srv_req)
self._failed = False
except Exception as e:
Logger.logwarn('Could not change gripper state')
rospy.logwarn(str(e))
self._failed = True
def on_exit(self, userdata):
# This method is called when an outcome is returned and another state gets active.
# It can be used to stop possibly running processes started by on_enter.
pass # Nothing to do
def on_start(self):
# This method is called when the behavior is started.
# If possible, it is generally better to initialize used resources in the constructor
# because if anything failed, the behavior would not even be started.
pass
def on_stop(self):
# This method is called whenever the behavior stops execution, also if it is cancelled.
# Use this event to clean up things like claimed resources.
pass # Nothing to do
class SetBoolState(EventState):
'''
Issue call to std_srv.SetBool service.
-- service string Service name.
-- value bool Argument for service call.
#> success bool Request result as in std_srvs.srv.SetBoolResponce message.
#> message string Request result as in std_srvs.srv.SetBoolResponce message.
<= true True has been returned.
<= false False has been returned.
<= failure Service call has failed.
'''
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 on_enter(self, userdata):
self._error = False
try:
self._response = self._service_caller.call(self._service,
self._value)
except Exception as e:
Logger.logwarn('Failed to call SetBool service `' + self._service +
'`:\n%s' % str(e))
self._error = True
Logger.loginfo(
'Call SetBool: `{0}` (value={1}) result: {2} "{3}".'.format(
self._service, self._value, self._response.success,
self._response.message))
def execute(self, userdata):
if self._error:
return 'failure'
userdata.success = self._response.success
userdata.message = self._response.message
if self._response.success:
return 'true'
else:
return 'false'
def on_exit(self, userdata):
pass
class CheckForPersonState(EventState):
"""
Checks whether or not the robot sees a Person
-- name String Name of the Person to search for
-- timeout float Timeout in seconds
<= found The Person was found
<= not_found The Person was not found
<= failed Execution failed
"""
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 people_callback(self, data):
self._mutex.acquire()
self._callback_received = True
self._people = []
for p in data.persons:
self._people.append(p)
self._transform_listener.unsubscribe_topic(self._transform_topic)
self._mutex.release()
def execute(self, userdata):
"""wait for transform callback, check all transforms for requested person"""
elapsed = rospy.get_rostime() - self._start_time
if elapsed.to_sec() > self._timeout:
return 'not_found'
self._mutex.acquire()
if self._callback_received:
known = False
for p in self._people:
request = DoIKnowThatPersonRequest()
request.transform_id = p.transformid
response = self._service_proxy.call(self._service_name,
request)
if response.name == self.name:
known = True
self._mutex.release()
if known:
return 'found'
else:
return 'not_found'
self._mutex.release()
def on_enter(self, userdata):
"""Subscribe to transforms, start timeout-timer"""
self._start_time = rospy.get_rostime()
self._transform_listener.subscribe(self._transform_topic,
ExtendedPeople,
self.people_callback)
def on_exit(self, userdata):
self._transform_listener.unsubscribe_topic(self._transform_topic)
def on_stop(self):
self._transform_listener.unsubscribe_topic(self._transform_topic)
