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')
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.
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})
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))
def __init__(self, depth):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(MovementServiceCaller,
self).__init__(outcomes=['Done', 'Failed'])
#To catch the sent srv_type in the state parameter
self._topic = "/autonomous/depth_control"
self._srv_type = depth_adv
self._depth = depth
self.service_client = ProxyServiceCaller(
{self._topic:
self._srv_type}) # pass required clients as dict (topic: type)
Logger.loginfo("initiated %s service caller" % self._topic)
def __init__(self):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(DetectPlaneState, self).__init__(outcomes=['continue', 'failed'])
# The constructor is called when building the state machine, not when actually starting the behavior.
# Thus, we cannot save the starting time now and will do so later.
self._srv_topic = "/detect_plane_srv"
self._srv = ProxyServiceCaller({self._srv_topic: Trigger})
self._srv_result = None
self._failed = False
def __init__(self, robot_name):
'''
Constructor
'''
super(Pos2Robot, self).__init__(outcomes=['done', 'failed'],
input_keys=['c_pos'],
output_keys=['pos'])
self.robot_name = robot_name
self.pos2robot_service = self.robot_name + '/eye2base'
self.pos2robot_client = ProxyServiceCaller(
{self.pos2robot_service: eye2base})
class CaptureCharUcoState(EventState):
"""
Publishes a pose from userdata so that it can be displayed in rviz.
-- robot_name string Robots name to move
<= done Calib done.
<= fail Calib fail.
"""
def __init__(self, robot_name):
"""Constructor"""
super(CaptureCharUcoState, self).__init__(outcomes=['done', 'fail'])
self.robot_name = robot_name
self.move_mode = 'p2p'
self.pose_indx = 0
self.hand_eye_service = '/camera/hand_eye_calibration'
self.hand_eye_client = ProxyServiceCaller({self.hand_eye_service: hand_eye_calibration})
self.get_feedback_service = self.robot_name +'/get_kinematics_pose'
self.get_feedback_client = ProxyServiceCaller({self.get_feedback_service: GetKinematicsPose})
def execute(self, _):
if not self.get_feedback_client.is_available(self.get_feedback_service):
rospy.logerr("get_feedback_service is not available!")
return 'fail'
try:
arm_feedback = self.get_feedback_client.call(self.get_feedback_service, 'arm')
except rospy.ServiceException as e:
rospy.logerr("Service call failed: %s" % e)
return 'fail'
req = hand_eye_calibrationRequest()
req.end_trans.translation.x = arm_feedback.group_pose.position.x
req.end_trans.translation.y = arm_feedback.group_pose.position.y
req.end_trans.translation.z = arm_feedback.group_pose.position.z
req.end_trans.rotation.x = arm_feedback.group_pose.orientation.x
req.end_trans.rotation.y = arm_feedback.group_pose.orientation.y
req.end_trans.rotation.z = arm_feedback.group_pose.orientation.z
req.end_trans.rotation.w = arm_feedback.group_pose.orientation.w
if not self.hand_eye_client.is_available(self.hand_eye_service):
rospy.logerr("get_feedback_service is not available!")
return 'fail'
try:
res = self.hand_eye_client.call(self.hand_eye_service, req)
except rospy.ServiceException as e:
rospy.logerr("Service call failed: %s" % e)
return 'fail'
return 'done'
def on_enter(self, _):
time.sleep(0.2)
def __init__(self, model_name, output_frame_id):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(LocateFactoryDeviceState, self).__init__(outcomes = ['succeeded', 'failed'], output_keys = ['pose'])
# Store state parameter for later use.
self._failed = True
# initialize service proxy
self._srv_name = '/gazebo/get_model_state'
self._srv = ProxyServiceCaller({self._srv_name: GetModelState})
self._srv_req = GetModelStateRequest()
self._srv_req.model_name = model_name # TODO: change parameter name
self._srv_req.relative_entity_name = output_frame_id # TODO: change parameter name
def __init__(self, target, service_name, srv_type):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(MAVROSServiceCaller, self).__init__(outcomes=['Done', 'Failed'])
#To catch the sent srv_type in the state parameter
types = {"CommandBool": CommandBool, "SetMode": SetMode}
self._topic = service_name
self._target = target
self._srv_type = types[srv_type]
self.service_client = ProxyServiceCaller(
{self._topic:
self._srv_type}) # pass required clients as dict (topic: type)
#self._proxy = rospy.ServiceProxy(self._topic,dist)
# Store state parameter for later use.
Logger.loginfo("initiated %s service caller" % self._topic)
def __init__(self, enable, service_name):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(VacuumGripperControlState,
self).__init__(outcomes=['continue', 'failed'])
# initialize service proxy
self._srv_name = service_name
self._srv = ProxyServiceCaller({self._srv_name: VacuumGripperControl})
self._srv_req = VacuumGripperControlRequest()
if enable:
self._srv_req.enable = True
else:
self._srv_req.enable = False
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.')
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.
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
