def __init__(self,
                 controller='joint_state_head',
                 duration=120,
                 interval=[2.0, 4.0],
                 joints=[],
                 minimal=[],
                 maximal=[]):
        super(RandJointsMovements, self).__init__(outcomes=['done', 'failed'],
                                                  input_keys=['config'])

        # Store topic parameter for later use.
        self._controller = 'motion/controller/' + controller

        # create proxies
        self._set_operational_caller = ProxyServiceCaller(
            {self._controller + '/set_operational': SetBool})
        self._publisher = ProxyPublisher(
            {self._controller + '/in_joints_ref': JointState})

        # state
        self._start_timestamp = None
        self._movement_timestamp = None
        self._movement_duration = Duration()

        # user input
        self.set_movement_parameters(duration, interval, joints, minimal,
                                     maximal)

        # error in enter hook
        self._error = False
    def __init__(self, group, offset, joint_names, tool_link, rotation):
        # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
        super(ComputeGraspState,
              self).__init__(outcomes=['continue', 'failed'],
                             input_keys=['pose'],
                             output_keys=['joint_values', 'joint_names'])

        self._group = group
        self._offset = offset
        self._joint_names = joint_names
        self._tool_link = tool_link
        self._rotation = rotation

        self._srv_name = '/compute_ik'
        self._ik_srv = ProxyServiceCaller({self._srv_name: GetPositionIK})

        # tf to transfor the object pose
        self._tf_buffer = tf2_ros.Buffer(
            rospy.Duration(10.0))  #tf buffer length
        self._tf_listener = tf2_ros.TransformListener(self._tf_buffer)

        self._robot1_client = actionlib.SimpleActionClient(
            'execute_trajectory', moveit_msgs.msg.ExecuteTrajectoryAction)
        self._robot1_client.wait_for_server()
        rospy.loginfo('Execute Trajectory server is available for robot1')
Example #3
0
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.
Example #4
0
    def __init__(self,
                 move_group,
                 offset,
                 tool_link,
                 action_topic='/move_group'):
        '''
		Constructor
		'''
        super(MoveitToJointsDynState, self).__init__(
            outcomes=['reached', 'planning_failed', 'control_failed'],
            input_keys=['joint_values', 'joint_names'])

        self._offset = offset
        self._tool_link = tool_link

        self._action_topic = action_topic
        self._fk_srv_topic = '/compute_fk'
        self._cartesian_srv_topic = '/compute_cartesian_path'
        self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
        self._fk_srv = ProxyServiceCaller({self._fk_srv_topic: GetPositionFK})
        self._cartesian_srv = ProxyServiceCaller(
            {self._cartesian_srv_topic: GetCartesianPath})

        self._current_group_name = move_group
        self._joint_names = None

        self._planning_failed = False
        self._control_failed = False
        self._success = False

        self._traj_client = actionlib.SimpleActionClient(
            'execute_trajectory', moveit_msgs.msg.ExecuteTrajectoryAction)
        self._traj_client.wait_for_server()
        self._traj_exec_result = None
    def __init__(self, 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 __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
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'
	def on_enter(self, userdata):
		# This method is called when the state becomes active, i.e. a transition from another state to this one is taken.
		# It is primarily used to start actions which are associated with this state.

		self._move_group = userdata.move_group
		self._move_group_prefix = userdata.move_group_prefix
		self._tool_link = userdata.tool_link

		self._offset = userdata.offset
		self._rotation = userdata.rotation

		self._srv_name = userdata.move_group_prefix + '/compute_ik'
		self._ik_srv = ProxyServiceCaller({self._srv_name: GetPositionIK})

		self._robot1_client = actionlib.SimpleActionClient(userdata.move_group_prefix + '/execute_trajectory', moveit_msgs.msg.ExecuteTrajectoryAction)
		self._robot1_client.wait_for_server()
		rospy.loginfo('Execute Trajectory server is available for robot')


		# Get transform between camera and robot1_base
		while True:
			rospy.sleep(0.1)
			try:
				target_pose = self._tf_buffer.transform(userdata.pose, "world")
				break
			except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
				rospy.logerr("ComputeGraspState::on_enter - Failed to transform to world")
				continue

		# the grasp pose is defined as being located on top of the item
		target_pose.pose.position.z += self._offset + 0.0


		# rotate the object pose 180 degrees around - now works with -90???

		#q_orig = [target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]
		q_orig = [0, 0, 0, 1]
		#q_rot = quaternion_from_euler(self._rotation, 0, 0)
		q_rot = quaternion_from_euler(self._rotation, math.pi/2.0, 0) # math.pi/2.0 added by gerard!!
		#q_rot = quaternion_from_euler(math.pi/-2.0, 0, 0)
		res_q = quaternion_multiply(q_rot, q_orig)
		target_pose.pose.orientation = geometry_msgs.msg.Quaternion(*res_q)

		# use ik service to compute joint_values
		self._srv_req = GetPositionIKRequest()
		self._srv_req.ik_request.group_name = self._move_group 
		self._srv_req.ik_request.robot_state.joint_state = rospy.wait_for_message(self._move_group_prefix + '/joint_states', sensor_msgs.msg.JointState)
		self._srv_req.ik_request.ik_link_name = self._tool_link  # TODO: this needs to be a parameter
		self._srv_req.ik_request.pose_stamped = target_pose
		self._srv_req.ik_request.avoid_collisions = True
		self._srv_req.ik_request.attempts = 500
		try:
			self._srv_result = self._ik_srv.call(self._srv_name, self._srv_req)
			self._failed = False

		except Exception as e:
			Logger.logwarn('Could not call IK: ' + str(e))
			self._failed = True
    def __init__(self):
        # See example_state.py for basic explanations.
        super(SaraNLUrestaurant, self).__init__(outcomes=['understood', 'fail'], input_keys=['sentence'],
                                         output_keys=['orderList'])

        self.serviceName = "/Restaurant_NLU_Service"

        Logger.loginfo("waiting forservice: " + self.serviceName)

        self.serviceNLU = ProxyServiceCaller({self.serviceName: RestaurantNLUService})
	def __init__(self, stop):
		# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
		super(SetConveyorPowerState, self).__init__(outcomes = ['succeeded', 'failed'], input_keys = ['speed'])

		# Store state parameter for later use.
		self._stop = stop

		# initialize service proxy
		self._srv_name = '/omtp/conveyor/control'
		self._srv = ProxyServiceCaller({self._srv_name: ConveyorBeltControl})
 def __init__(self, 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})
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
    def __init__(self, object_name):
        super(RecognizeObjectState,
              self).__init__(outcomes=['detected', 'not_detected', 'failed'],
                             input_keys=['pointcloud'])

        self._srv_topic = '/recognition_service/sv_recognition'
        self._srv = ProxyServiceCaller({self._srv_topic: recognize})

        self._object_name = object_name
        self._srv_result = None
        self._failed = False
    def __init__(self):
        # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
        super(Wait_getCloserVictim, self).__init__(
            outcomes=['waiting', 'restart', 'preempted'],
            input_keys=['task_details_task_id', 'params_distance'],
            output_keys=['pose', 'params_distance'])

        self._allocated_task = '/taskallocation/allocatedTask'
        self._sub = ProxySubscriberCached({self._allocated_task: Task})

        self._topic_get_task = 'taskallocation/get_task'
        self.getTask = ProxyServiceCaller({self._topic_get_task: GetTask})
Example #15
0
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))
Example #16
0
 def __init__(self, depth):
     # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
     super(MovementServiceCaller,
           self).__init__(outcomes=['Done', 'Failed'])
     #To catch the sent srv_type in the state parameter
     self._topic = "/autonomous/depth_control"
     self._srv_type = depth_adv
     self._depth = depth
     self.service_client = ProxyServiceCaller(
         {self._topic:
          self._srv_type})  # pass required clients as dict (topic: type)
     Logger.loginfo("initiated %s service caller" % self._topic)
    def __init__(self):
        # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
        super(DetectPlaneState, self).__init__(outcomes=['continue', 'failed'])

        # The constructor is called when building the state machine, not when actually starting the behavior.
        # Thus, we cannot save the starting time now and will do so later.

        self._srv_topic = "/detect_plane_srv"
        self._srv = ProxyServiceCaller({self._srv_topic: Trigger})

        self._srv_result = None
        self._failed = False
Example #18
0
    def __init__(self, robot_name):
        '''
		Constructor
		'''
        super(Pos2Robot, self).__init__(outcomes=['done', 'failed'],
                                        input_keys=['c_pos'],
                                        output_keys=['pos'])

        self.robot_name = robot_name
        self.pos2robot_service = self.robot_name + '/eye2base'
        self.pos2robot_client = ProxyServiceCaller(
            {self.pos2robot_service: eye2base})
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)
        
Example #20
0
	def __init__(self, model_name, output_frame_id):
		# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
		super(LocateFactoryDeviceState, self).__init__(outcomes = ['succeeded', 'failed'], output_keys = ['pose'])

		# Store state parameter for later use.
		self._failed = True

		# initialize service proxy
		self._srv_name = '/gazebo/get_model_state'
		self._srv = ProxyServiceCaller({self._srv_name: GetModelState})
		self._srv_req = GetModelStateRequest()
		self._srv_req.model_name = model_name  # TODO: change parameter name
		self._srv_req.relative_entity_name = output_frame_id  # TODO: change parameter name
 def __init__(self, target, service_name, srv_type):
     # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
     super(MAVROSServiceCaller, self).__init__(outcomes=['Done', 'Failed'])
     #To catch the sent srv_type in the state parameter
     types = {"CommandBool": CommandBool, "SetMode": SetMode}
     self._topic = service_name
     self._target = target
     self._srv_type = types[srv_type]
     self.service_client = ProxyServiceCaller(
         {self._topic:
          self._srv_type})  # pass required clients as dict (topic: type)
     #self._proxy = rospy.ServiceProxy(self._topic,dist)
     # Store state parameter for later use.
     Logger.loginfo("initiated %s service caller" % self._topic)
Example #22
0
    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
Example #23
0
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
    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.')
Example #25
0
    def __init__(self, service, value):
        super(SetBoolState,
              self).__init__(outcomes=['true', 'false', 'failure'],
                             output_keys=['success', 'message'])

        # Store state parameter for later use.
        self._value = value
        self._service = service

        # get service caller
        self._service_caller = ProxyServiceCaller({service: SetBool})
        self._response = None

        # It may happen that the service caller fails to send the action goal.
        self._error = False
	def __init__(self, activation):
		# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
		super(ControlFeederState, self).__init__(outcomes = ['succeeded', 'failed'])

		# Store state parameter for later use.
		self._activation = activation

		# initialize service proxy
		if self._activation:
			self._srv_name = '/start_spawn'
		else:
			self._srv_name = '/stop_spawn'

		self._srv = ProxyServiceCaller({self._srv_name: Empty})
		self._srv_req = EmptyRequest()
	def __init__(self):
		
		super(DiscardVictim, self).__init__(outcomes = ['discarded'], input_keys = ['victim'])


		self._setVictimState = '/worldmodel/set_object_state'
		self._srvSet = ProxyServiceCaller({self._setVictimState: SetObjectState})
    def __init__(self):
        """
		Constructor
		"""
        super(CreatePath, self).__init__(outcomes=["succeeded", "retry"], output_keys=["path"])

        self._serv = ProxyServiceCaller({"trajectory": GetRobotTrajectory})
        self._start_time = None
	def __init__(self):
		# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
		super(Wait_getCloserVictim, self).__init__(outcomes = ['waiting','restart','preempted'], input_keys = ['task_details_task_id', 'params_distance'], output_keys = ['pose', 'params_distance'])
		
		self._allocated_task = '/taskallocation/allocatedTask'
		self._sub = ProxySubscriberCached({self._allocated_task: Task})
		
		
		self._topic_get_task = 'taskallocation/get_task'
		self.getTask = ProxyServiceCaller({self._topic_get_task: GetTask})
    def __init__(self, name, timeout=2.0):
        """Constructor"""

        super(CheckForPersonState,
              self).__init__(outcomes=['found', 'not_found', 'failed'])

        self._name = name
        self._mutex = Lock()
        self._people = []
        self._timeout = timeout
        self._start_time = None
        self._callback_received = False
        self._transform_topic = '/clf_perception_vision/people/raw/transform'
        self._service_name = 'pepper_face_identification'

        self._service_proxy = ProxyServiceCaller(
            {self._service_name: DoIKnowThatPerson})
        self._transform_listener = ProxySubscriberCached(
            {self._transform_topic: ExtendedPeople})
	def __init__(self, object_name):
		super(RecognizeObjectState, self).__init__(outcomes = ['detected', 'not_detected', 'failed'],
													input_keys = ['pointcloud'])

		self._srv_topic = '/recognition_service/sv_recognition'
		self._srv = ProxyServiceCaller({self._srv_topic: recognize})

		self._object_name = object_name
		self._srv_result = None
		self._failed = False
	def __init__(self, tolerance):
		# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
		super(AlignWithGate, self).__init__(outcomes = ['Aligned', 'CantSee'])
		#To catch the sent srv_type in the state parameter
		self._v_topic = "/vision/gate"
		self.sway_left_client = ProxyServiceCaller({'/autonomous/sway_left': Empty}) # pass required clients as dict (topic: type)
		self.sway_right_client = ProxyServiceCaller({'/autonomous/sway_right': Empty}) # pass required clients as dict (topic: type)
		self.heave_down_client = ProxyServiceCaller({'/autonomous/heave_down': Empty}) # pass required clients as dict (topic: type)
		self.heave_up_client = ProxyServiceCaller({'/autonomous/heave_up': Empty}) # pass required clients as dict (topic: type)
		self.stop_client = ProxyServiceCaller({'/autonomous/stop_vehicle': Empty}) # pass required clients as dict (topic: type)
		self.vision_sub = ProxySubscriberCached({self._v_topic: vision_target})
		self.width = rospy.get_param("frame_width")
		self.height = rospy.get_param("frame_height")
		self.width_h = (self.width/2) + tolerance
		self.width_l = (self.width/2) - tolerance
		self.height_h = (self.height/2) + tolerance
		self.height_l = (self.height/2) - tolerance
		self.inrange_flag = True
		Logger.loginfo("initiated allign gate state")
	def __init__(self, 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 __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
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.
Example #36
0
    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 __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
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
	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
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 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 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 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