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)