class PublishPoseState(EventState):
"""
Publishes a pose from userdata so that it can be displayed in rviz.
-- topic string Topic to which the pose will be published.
># pose PoseStamped Pose to be published.
<= done Pose has been published.
"""
def __init__(self, topic):
"""Constructor"""
super(PublishPoseState, self).__init__(outcomes=['done'],
input_keys=['pose'])
self._topic = topic
self._pub = ProxyPublisher({self._topic: PoseStamped})
def execute(self, userdata):
return 'done'
def on_enter(self, userdata):
self._pub.publish(self._topic, userdata.pose)
class complete_task_StartHack(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(complete_task_StartHack, self).__init__(outcomes = ['error','done'], input_keys = ['task_details_task_id'])
self._topic_set_task_state = 'taskallocation/set_task_state'
self._pub = ProxyPublisher({self._topic_set_task_state: SetTaskState})
self._task_state_completed = TaskState()
self._task_state_completed.state = TaskState.COMPLETED
def execute(self, userdata):
request = SetTaskStateRequest()
request.task_id = userdata.task_details_task_id
request.new_state = self._task_state_completed
try:
self._pub.publish(self._topic_set_task_state, request)
return 'done'
except Exception, e:
Logger.logwarn('Could not set task state:' + str(e))
return 'error'
class ShowPictureWebinterfaceState(EventState):
'''
Displays the picture in a web interface
># Image Image The received Image
<= done Displaying the Picture
'''
def __init__(self):
super(ShowPictureWebinterfaceState, self).__init__(outcomes = ['tweet', 'forget'],
input_keys = ['image_name'])
self._pub_topic = '/webinterface/display_picture'
self._pub = ProxyPublisher({self._pub_topic: String})
self._sub_topic = '/webinterface/dialog_feedback'
self._sub = ProxySubscriberCached({self._sub_topic: String})
def execute(self, userdata):
if self._sub.has_msg(self._sub_topic):
msg = self._sub.get_last_msg(self._sub_topic)
if msg.data == 'yes':
print 'show_picture_webinterface_state, returning tweet'
return 'tweet'
else:
print 'show_picture_webinterface_state, returning forget'
return 'forget'
def on_enter(self,userdata):
self._sub.remove_last_msg(self._sub_topic)
self._pub.publish(self._pub_topic, String(userdata.image_name))
class PreemptableState(LoopbackState):
"""
A state that can be preempted.
If preempted, the state will not be executed anymore and return the outcome preempted.
"""
_preempted_name = "preempted"
preempt = False
switching = False
def __init__(self, *args, **kwargs):
# add preempted outcome
if len(args) > 0 and type(args[0]) is list:
# need this ugly check for list type, because first argument in CBState is the callback
args[0].append(self._preempted_name)
else:
outcomes = kwargs.get("outcomes", [])
outcomes.append(self._preempted_name)
kwargs["outcomes"] = outcomes
super(PreemptableState, self).__init__(*args, **kwargs)
self.__execute = self.execute
self.execute = self._preemptable_execute
self._feedback_topic = "/flexbe/command_feedback"
self._preempt_topic = "/flexbe/command/preempt"
self._pub = ProxyPublisher()
self._sub = ProxySubscriberCached()
def _preemptable_execute(self, *args, **kwargs):
preempting = False
if self._is_controlled and self._sub.has_msg(self._preempt_topic):
self._sub.remove_last_msg(self._preempt_topic)
self._pub.publish(self._feedback_topic, CommandFeedback(command="preempt"))
preempting = True
rospy.loginfo("--> Behavior will be preempted")
if PreemptableState.preempt:
PreemptableState.preempt = False
preempting = True
rospy.loginfo("Behavior will be preempted")
if preempting:
self.service_preempt()
self._force_transition = True
return self._preempted_name
return self.__execute(*args, **kwargs)
def _enable_ros_control(self):
super(PreemptableState, self)._enable_ros_control()
self._pub.createPublisher(self._feedback_topic, CommandFeedback)
self._sub.subscribe(self._preempt_topic, Empty)
PreemptableState.preempt = False
def _disable_ros_control(self):
super(PreemptableState, self)._disable_ros_control()
self._sub.unsubscribe_topic(self._preempt_topic)
class PauseExploration(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(PauseExploration, self).__init__(outcomes = ['continue', 'pause'])
self._topic_cancel = 'move_base/cancel'
self._pub = ProxyPublisher({self._topic_cancel: Empty})
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.
return 'pause' # One of the outcomes declared above.
def on_enter(self, userdata):
# This method is called when the state becomes active, i.e. a transition from another state to this one is taken.
# It is primarily used to start actions which are associated with this state.
self._pub.publish(self._topic_cancel, Empty())
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
def on_start(self):
# This method is called when the behavior is started.
# If possible, it is generally better to initialize used resources in the constructor
# because if anything failed, the behavior would not even be started.
pass
def on_stop(self):
# This method is called whenever the behavior stops execution, also if it is cancelled.
# Use this event to clean up things like claimed resources.
pass
def __set_pubSub(self):
print("[Arm] name space : " + str(self.robot_name))
self.p2p_topic = str(self.robot_name) + '/p2p_pose_msg'
self.__p2p_pub = ProxyPublisher({self.p2p_topic: P2PPose})
self.line_topic = str(self.robot_name) + '/kinematics_pose_msg'
self.__line_pub = ProxyPublisher({self.line_topic: KinematicsPose})
self.arm_status_topic = str(self.robot_name) + '/status'
self.__status_sub = ProxySubscriberCached(
{self.arm_status_topic: StatusMsg})
def __init__(self,
topic='motion/controller/look_at/in_pose_ref',
duration=120,
interval=[3, 5],
maxXYZ=[1, 0.3, 0.5],
minXYZ=[1.0, -0.3, 0.0],
frame_xyz='base_link',
frame_out='odom_combined'):
super(RandPoseGenerator, self).__init__(outcomes=['done', 'failure'])
# Store topic parameter for later use.
if not isinstance(topic, str):
raise ValueError("Topic name must be string.")
if not isinstance(duration, (int, float)) or duration <= 0:
raise ValueError("Duration must be positive number.")
if len(interval) != 2 or any([
not isinstance(t, (int, float)) for t in interval
]) or any([t < 0 for t in interval]) or interval[0] > interval[1]:
raise ValueError("Interval must represent interval of time.")
if len(minXYZ) != 3 or any(
[not isinstance(v, (int, float)) for v in minXYZ]):
raise ValueError("minXYZ: list of three numbers was expected.")
if len(maxXYZ) != 3 or any(
[not isinstance(v, (int, float)) for v in maxXYZ]):
raise ValueError("maxXYZ: list of three numbers was expected.")
if not isinstance(frame_xyz, str) or not isinstance(frame_out, str):
raise ValueError("Frame names must be string.")
self._topic = topic
self._duration = Duration.from_sec(duration)
self._interval = interval
self._minXYZ = minXYZ
self._maxXYZ = maxXYZ
self._frame_in = frame_xyz
self._frame_out = frame_out
# create proxies
self._publisher = ProxyPublisher({self._topic: PoseStamped})
self._tf_listener = ProxyTransformListener().listener()
self._tf_listener.waitForTransform(self._frame_out, self._frame_in,
rospy.Time(), rospy.Duration(1))
if not self._tf_listener.canTransform(self._frame_out, self._frame_in,
rospy.Time(0)):
raise ValueError(
"Unable to perform transform between frames %s and %s." %
(self._frame_out, self._frame_in))
# state
self._start_timestamp = None
self._movement_timestamp = None
self._movement_duration = Duration()
# error in enter hook
self._error = False
def __init__(self, command):
'''Constructor'''
super(RobotStateCommandState,
self).__init__(outcomes=['done', 'failed'])
self._topic = '/flor/controller/robot_state_command'
self._pub = ProxyPublisher({self._topic: FlorRobotStateCommand})
self._command = command
self._failed = False
def __init__(self):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(complete_task_StartHack,
self).__init__(outcomes=['error', 'done'],
input_keys=['task_details_task_id'])
self._topic_set_task_state = 'taskallocation/set_task_state'
self._pub = ProxyPublisher({self._topic_set_task_state: SetTaskState})
self._task_state_completed = TaskState()
self._task_state_completed.state = TaskState.COMPLETED
def __set_pubSub(self):
print("[Arm] name space : " + str(self.robot_name))
self.set_mode_topic = str(self.robot_name) + '/set_mode_msg'
self.__set_mode_pub = ProxyPublisher({self.set_mode_topic: String})
self.joint_topic = str(self.robot_name) + '/joint_pose_msg'
self.__joint_pub = ProxyPublisher({self.joint_topic: JointPose})
self.arm_status_topic = str(self.robot_name) + '/status'
self.__status_sub = ProxySubscriberCached(
{self.arm_status_topic: StatusMsg})
def __init__(self, mechanisms):
super(mechanisms, self).__init__(outcomes = ['dropped'])
topics = {'gripper hold': "arduino/close_gripper" , 'gripper release': "arduino/open_gripper",
'fire torpedo': "arduino/launch_torpedo", 'drop marker': "/arduino/open_dropper"}
self._topic = topics[mechanism]
(msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic)
if msg_topic == self._topic:
msg_type = self._get_msg_from_path(msg_path)
self._publisher = ProxyPublisher({self._topic: msg_type})
def __init__(self, target_name, target_path, given_outcomes,
outcome_autonomy):
super(MirrorState, self).__init__(outcomes=given_outcomes)
self.set_rate(100)
self._target_name = target_name
self._target_path = target_path
self._outcome_topic = 'flexbe/mirror/outcome'
self._pub = ProxyPublisher()
self._sub = ProxySubscriberCached({self._outcome_topic: UInt8})
def __init__(self, command, no_video=False, no_bags=True):
"""Constructor"""
super(VideoLoggingState, self).__init__(outcomes=['done'],
input_keys=['experiment_name', 'description'])
self._topic = "/vigir_logging"
self._pub = ProxyPublisher({self._topic: OCSLogging})
self._command = command
self._no_video = no_video
self._no_bags = no_bags
def __init__(self):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(Send_Request, self).__init__(outcomes = ['succeeded'], input_keys = ['pose','params','frameId'])
#self.userdata.goalId = 'none'
#self.userdata.frameId = frameId
useMoveBase=True
topic = 'move_base/observe' if useMoveBase else 'controller/goal'
self._topic_move_base_goal = topic
self._pub = ProxyPublisher({self._topic_move_base_goal: MoveBaseActionGoal})
def __init__(self, target_time, target_angle=360.0, cmd_topic='/create_node/cmd_vel', odometry_topic='/create_node/odom'):
super(RotateAngleState, self).__init__(outcomes = ['done'])
self._target_time = rospy.Duration(target_time)
self._target_angle = target_angle*3.141593/180.0
self._twist = TwistStamped()
self._twist.twist.linear.x = 0
self._twist.twist.angular.z = (self._target_angle / target_time)
self._cmd_topic = cmd_topic
self._pub = ProxyPublisher( {self._cmd_topic: TwistStamped})
self._start_time = None
self._return = None # Track the outcome so we can detect if transition is blocked
class ManualState(EventState):
'''
Project11 state where vehicle is operated from joystick commands.
'''
def __init__(self):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ManualState, self).__init__(outcomes = ['autonomous', 'standby'])
self.publishers = ProxyPublisher({'/helm':Helm})
self.p11sb = Project11StateBase()
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.
js = self.p11sb.checkJoystick()
if js is not None:
requested_state = js[1]
if requested_state is not None and requested_state != 'manual':
return requested_state
msg = js[0]
helm = Helm()
helm.header.stamp = rospy.Time.now()
helm.throttle = msg.axes[1]
helm.rudder = -msg.axes[3]
self.publishers.publish('/helm',helm)
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.
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
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, pubtopic, subtopic, pubint):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(GenericPubSub, self).__init__(outcomes=["completed", "failed"])
# Store state parameters for later use.
self._rotation = None
self._pubtopic = pubtopic
self._subtopic = subtopic
self._pubint = pubint
self._pub = ProxyPublisher({self._pubtopic: Int32})
self._sub = ProxySubscriberCached({self._subtopic: Int32})
def __init__(self, target_time, square_side_in_m, speed):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(DriveInSquareByOdom,
self).__init__(outcomes=['continue', 'failed'])
self._target_time = rospy.Duration(target_time)
self._square_side_lenght = square_side_in_m
self._speed = speed
self.vel_topic = '/cmd_vel'
self.pub = ProxyPublisher({self.vel_topic:
Twist}) # TODO set publish rate to 5 HZ
self.finished = False
def __init__(self):
super(PrayingMantisCalibrationSM, self).__init__()
self.name = 'Praying Mantis Calibration'
# parameters of this behavior
# references to used behaviors
# Additional initialization code can be added inside the following tags
# [MANUAL_INIT]
self._offset_topic = "/flor/controller/encoder_offsets"
self._pub = ProxyPublisher({self._offset_topic: JointTrajectory})
self._joint_limits = AtlasDefinitions.arm_joint_limits
# Define 90 percent positions for both arms (order of joints same as in _joint_names attribute)
# atlas_v5
# - account for fall protection pads
# - ignore the lower 3 joints, ie, the electric motor ones
left_calib_upper = [
-1.4252, -1.4649, +0.1588, +2.2767, +0.1, +0.1, +0.1
]
left_calib_lower = [
+0.5470, +1.2355, +2.9297, +0.1191, -0.1, +1.0, -0.1
]
right_calib_upper = [
+1.4914, +1.4296, +0.2118, -2.2899, +0.1, +0.1, +0.1
]
right_calib_lower = [
-0.5470, -1.2355, +2.9297, -0.1191, -0.1, -1.0, -0.1
]
# # atlas_v5 (without shoulder pads)
# left_calib_upper = [+0.5470, +1.2355, +2.9297, +2.1576, +0.1, +0.1, +0.1]
# left_calib_lower = [-1.1869, -1.4296, +0.2118, +0.1191, -1.3, +1.0, -0.1]
# right_calib_upper = [-0.5470, -1.2355, +2.9297, -2.1576, +0.1, +0.1, +0.1]
# right_calib_lower = [+1.1869, +1.4296, +0.2118, -0.1191, -1.3, -1.0, -0.1]
self._joint_calib = {
'left_arm': {
'upper': left_calib_upper,
'lower': left_calib_lower
},
'right_arm': {
'upper': right_calib_upper,
'lower': right_calib_lower
}
}
self._joint_names = AtlasDefinitions.arm_joint_names
class RemoteRecordStop(EventState):
''' Stops rosbag recording. '''
def __init__(self, topic='/meka/rosbagremote/record/named'):
self._topic = topic
self._pub = ProxyPublisher({topic: String})
super(RemoteRecordStop, self).__init__(outcomes=['done'])
def execute(self, d):
Logger.loginfo('Stopping rosbag recording')
self._pub.publish(self._topic, ':stop')
return 'done'
def __init__(self, head_angle=25):
"""Constructor"""
super(MoveBaseState, self).__init__(outcomes=['arrived', 'failed'],
input_keys=['waypoint'])
self._head_angle = head_angle
self._head_topic = '/pepper_robot/head/pose'
ProxyPublisher.createPublisher(self._pub, self._head_topic, String)
self._action_topic = "/move_base"
self._client = ProxyActionClient({self._action_topic: MoveBaseAction})
self._arrived = False
self._failed = False
def __init__(self, *args, **kwargs):
super(ManuallyTransitionableState, self).__init__(*args, **kwargs)
self._force_transition = False
self.__execute = self.execute
self.execute = self._manually_transitionable_execute
self._feedback_topic = '/flexbe/command_feedback'
self._transition_topic = '/flexbe/command/transition'
self._pub = ProxyPublisher()
self._sub = ProxySubscriberCached()
def __init__(self):
super(WebsiteDummyState1, self).__init__(outcomes=['done', 'failed'])
#self.status_topic = '/web_service_proxy'
self.status_topic = '/robot_status'
self.website_service_proxy = '/web_service_proxy'
self._wait_for_execution = True
self.current_status_pub = ProxyPublisher(
{self.status_topic: RobotStatus})
rospy.wait_for_service(self.website_service_proxy)
self.client_website_proxy = rospy.ServiceProxy(
self.website_service_proxy, WebServiceProxyMsg)
class ManuallyTransitionableState(MonitoringState):
"""
A state for that a desired outcome can be declared.
If any outcome is declared, this outcome is forced.
"""
def __init__(self, *args, **kwargs):
super(ManuallyTransitionableState, self).__init__(*args, **kwargs)
self._force_transition = False
self.__execute = self.execute
self.execute = self._manually_transitionable_execute
self._feedback_topic = '/flexbe/command_feedback'
self._transition_topic = '/flexbe/command/transition'
self._pub = ProxyPublisher()
self._sub = ProxySubscriberCached()
def _manually_transitionable_execute(self, *args, **kwargs):
if self._is_controlled and self._sub.has_buffered(
self._transition_topic):
command_msg = self._sub.get_from_buffer(self._transition_topic)
self._pub.publish(
self._feedback_topic,
CommandFeedback(command="transition",
args=[command_msg.target, self.name]))
if command_msg.target != self.name:
rospy.logwarn("--> Requested outcome for state " +
command_msg.target + " but active state is " +
self.name)
else:
self._force_transition = True
outcome = self._outcome_list[command_msg.outcome]
rospy.loginfo("--> Manually triggered outcome " + outcome +
" of state " + self.name)
return outcome
# return the normal outcome
self._force_transition = False
return self.__execute(*args, **kwargs)
def _enable_ros_control(self):
super(ManuallyTransitionableState, self)._enable_ros_control()
self._pub.createPublisher(self._feedback_topic, CommandFeedback)
self._sub.subscribe(self._transition_topic, OutcomeRequest)
self._sub.enable_buffer(self._transition_topic)
def _disable_ros_control(self):
super(ManuallyTransitionableState, self)._disable_ros_control()
self._sub.unsubscribe_topic(self._transition_topic)
class Reset_Control_State_GR(EventState):
'''
Reset control takes in the trial information from Test control and on a succesful completion starts
the Data Control. If all trials are completed then it will loop back to Test control. Direction is
used for determining a successful and unsuccseful outcome for testing purposed but will need to be
replaced with a different measure of success once it becomes more fleshed out.
TODO: More complex information for trials, update info
-- direction int TEMPORARY: Determines a succesful (1) or unsuccesful (0) outcome for testing purposes
># number_of_trials Trial information (currently just an int)
<= continue All actions completed
<= failed Trial control failed to initialize or call something TODO: Proper error checking
<= completed All Trials have been succesfully completed, go back to Test control
'''
def __init__(self):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(Reset_Control_State_GR, self).__init__(outcomes = ["continue", "failed"], input_keys=["rotation"])
# Store state parameters for later use.
self._rotation = None
self._pub = ProxyPublisher({"reset_start": Int32})
self._sub = ProxySubscriberCached({"reset_complete": Int32})
#rospy.init_node('reset_control', anonymous=True)
def execute(self, userdata):
#publish to arduino
#while(1):
if self._sub.has_msg("reset_complete"):
msg = self._sub.get_last_msg("reset_complete")
print(msg)
# in case you want to make sure the same message is not processed twice:
self._sub.remove_last_msg("reset_complete")
return "continue"
def on_enter(self, userdata):
#self._rotation = 5
#Initializes class variable from userdata, has to be done outside of constructor
if(self._rotation is None and userdata.rotation is not None):
self._rotation = userdata.rotation
self._pub.publish("reset_start", self._rotation)
class RobotStateCommandState(EventState):
'''
Publishes a robot state command message.
-- command int Command to be sent. Use class variables (e.g. STAND).
<= done Successfully published the state command message.
<= failed Failed to publish the state command message.
'''
START_HYDRAULIC_PRESSURE_OFF = 4 # "fake" start (without hydraulic pressure)
START_HYDRAULIC_PRESSURE_ON = 6 # start normally (with hydraulic pressure)
STOP = 8 # stop the pump
FREEZE = 16
STAND = 32
STAND_PREP = 33
CALIBRATE = 64 # BIASES
CALIBRATE_ARMS = 128
CALIBRATE_ARMS_FREEZE = 144
def __init__(self, command):
'''Constructor'''
super(RobotStateCommandState, self).__init__(outcomes = ['done', 'failed'])
self._topic = '/flor/controller/robot_state_command'
self._pub = ProxyPublisher({self._topic: FlorRobotStateCommand})
self._command = command
self._failed = False
def execute(self, userdata):
if self._failed:
return 'failed'
else:
return 'done'
def on_enter(self, userdata):
self._failed = False
try:
command_msg = FlorRobotStateCommand(state_command = self._command)
self._pub.publish(self._topic, command_msg)
except Exception as e:
Logger.logwarn('Failed to publish the command message:\n%s' % str(e))
self._failed = True
class VideoLoggingState(EventState):
"""
A state that controls video logging.
-- command boolean One of the available commands provided as class constants.
-- no_video boolean Only create bag files.
-- no_bags boolean Only record video.
># experiment_name string Unique name of the experiment.
<= done Executed the specified command.
"""
START = True
STOP = False
def __init__(self, command, no_video=False, no_bags=True):
"""Constructor"""
super(VideoLoggingState, self).__init__(outcomes=['done'],
input_keys=['experiment_name', 'description'])
self._topic = "/vigir_logging"
self._pub = ProxyPublisher({self._topic: OCSLogging})
self._command = command
self._no_video = no_video
self._no_bags = no_bags
def execute(self, userdata):
"""Execute this state"""
# nothing to check
return 'done'
def on_enter(self, userdata):
"""Upon entering the state"""
try:
self._pub.publish(self._topic, OCSLogging(run=self._command, no_video=self._no_video, no_bags=self._no_bags, experiment_name=userdata.experiment_name, description=userdata.description))
except Exception as e:
Logger.logwarn('Could not send video logging command:\n %s' % str(e))
def on_stop(self):
"""Stop video logging upon end of execution"""
try:
self._pub.publish(self._topic, OCSLogging(run=False))
except Exception as e:
pass
def __init__(self, *args, **kwargs):
super(OperatableStateMachine, self).__init__(*args, **kwargs)
self._message = None
self._rate = rospy.Rate(10)
self.id = None
self.autonomy = None
self._autonomy = {}
self._ordered_states = []
self._pub = ProxyPublisher()
self._sub = ProxySubscriberCached()
def __init__(self, topic, msg_type, value):
super(PublisherState, self).__init__(outcomes=['done', 'failed'])
# Store state parameter for later use.
self._topic = topic
# form message
self._message = msg_type(**value)
# create publisher
self._publisher = ProxyPublisher({topic: msg_type})
# error in enter hook
self._error = False
class RobotStateCommandState(EventState):
'''
Publishes a robot state command message.
-- command int Command to be sent. Use class variables (e.g. STAND).
<= done Successfully published the state command message.
<= failed Failed to publish the state command message.
'''
START_HYDRAULIC_PRESSURE_OFF = 4 # "fake" start (without hydraulic pressure)
START_HYDRAULIC_PRESSURE_ON = 6 # start normally (with hydraulic pressure)
STOP = 8 # stop the pump
FREEZE = 16
STAND = 32
STAND_PREP = 33
CALIBRATE = 64 # BIASES
CALIBRATE_ARMS = 128
CALIBRATE_ARMS_FREEZE = 144
def __init__(self, command):
'''Constructor'''
super(RobotStateCommandState,
self).__init__(outcomes=['done', 'failed'])
self._topic = '/flor/controller/robot_state_command'
self._pub = ProxyPublisher({self._topic: FlorRobotStateCommand})
self._command = command
self._failed = False
def execute(self, userdata):
if self._failed:
return 'failed'
else:
return 'done'
def on_enter(self, userdata):
self._failed = False
try:
command_msg = FlorRobotStateCommand(state_command=self._command)
self._pub.publish(self._topic, command_msg)
except Exception as e:
Logger.logwarn('Failed to publish the command message:\n%s' %
str(e))
self._failed = True
def __init__(self, person_stop_dist=0.5, with_j1=False, rate=10):
super(AdjustTorso, self).__init__(outcomes = ['done'])
self.with_j1 = bool(with_j1)
self.rate = rate
self.person_stop_dist = person_stop_dist
self.MAX_VEL = 1.0
self.MIN_TRAJ_DUR = 0.5
self.SPEED_SCALE = 0.5
self.JOINT_NAMES = ['torso_j0', 'torso_j1']
self.TARGET_FRAME = 'panplate'
self.LIMITS = {
self.JOINT_NAMES[0]: [-0.75, 0.75],
self.JOINT_NAMES[1]: [-0.1, 0.07],
}
self.j1_done = False
self.people_pos = None
self.js_pos = None
self.js_pos_des = None
self.t = tf.TransformListener(True, rospy.Duration(10))
self.PEOPLE_TOPIC = '/people_tracker/people'
self.TORSO_STATE_TOPIC = '/meka_roscontrol/torso_position_trajectory_controller/state'
self.TORSO_COMMAND_TOPIC = '/meka_roscontrol/torso_position_trajectory_controller/command'
self.FACES_TOPIC = '/openface2/faces'
sub_dict = {
self.TORSO_STATE_TOPIC: JointTrajectoryControllerState,
self.PEOPLE_TOPIC: People
}
if self.with_j1:
from openface2_ros_msgs.msg import Faces
sub_dict.update({
self.FACES_TOPIC: Faces
})
self._subs = ProxySubscriberCached(sub_dict)
self._pub = ProxyPublisher({self.TORSO_COMMAND_TOPIC: JointTrajectory})
frames = []
Logger.loginfo('waiting for transforms')
while frames == []:
frames = self.t.getFrameStrings()
class PauseExploration(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(PauseExploration, self).__init__(outcomes=['continue', 'pause'])
self._topic_cancel = 'move_base/cancel'
self._pub = ProxyPublisher({self._topic_cancel: Empty})
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.
return 'pause' # One of the outcomes declared above.
def on_enter(self, userdata):
# This method is called when the state becomes active, i.e. a transition from another state to this one is taken.
# It is primarily used to start actions which are associated with this state.
self._pub.publish(self._topic_cancel, Empty())
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
def on_start(self):
# This method is called when the behavior is started.
# If possible, it is generally better to initialize used resources in the constructor
# because if anything failed, the behavior would not even be started.
pass
def on_stop(self):
# This method is called whenever the behavior stops execution, also if it is cancelled.
# Use this event to clean up things like claimed resources.
pass
class TTSBulgarianFromIncommingKey(EventState):
'''
TTS in Bulgarian language, sends messages trough MQTT to the Windows TTS VM console app.
Sends a string to /ttsbg_mqtt/tts_text MQTT topic,
trough sending a ROS String message to ROS topic /ttsbg_ros/tts_text
mqtt_bridge resends the ROS message to the MQTT broker on /ttsbg_mqtt/tts_text.
Default MQTT broker IP is 192.168.1.2
># ttsbg_text String Incomming key - text to be synthesized
<= failed If behavior is unable to send the TTS message
<= done TTS message sent succesfully
'''
def __init__(self):
super(TTSBulgarianFromIncommingKey,
self).__init__(input_keys=['ttsbg_text'],
outcomes=['failed', 'done'])
self._ttsbg_text_to_be_synthesized = None
self._ttsbg_text_to_be_synthesized_topic = '/ttsbg_ros/tts_text'
self._ttsbg_command_topic = '/ttsbg_ros/command'
self._ttsbg_response_topic = '/ttsbg_ros/response'
#create publisher passing it the ttsbg_topic and msg_type
self.pub = ProxyPublisher({
self._ttsbg_text_to_be_synthesized_topic: String,
self._ttsbg_command_topic: String
})
# #create publisher passing it the ttsbg_command and msg_type
# self.pub_ttsbg_command = ProxyPublisher({self._ttsbg_command_topic: String})
#create subscriber
# self.sub_ttsbg_response = ProxySubscriberCached({self._ttsbg_response_topic: String})
self.sub = ProxySubscriberCached({self._ttsbg_response_topic: String})
def on_enter(self, userdata):
self._ttsbg_text_to_be_synthesized = userdata.ttsbg_text
def execute(self, userdata):
# Logger.loginfo('V execute sam....')
self.pub.publish(self._ttsbg_text_to_be_synthesized_topic,
self._ttsbg_text_to_be_synthesized.decode('utf-8'))
return 'done'
def on_exit(self, userdata):
pass
def on_stop(self):
pass
def __init__(self, target_name, target_path, given_outcomes, outcome_autonomy):
'''
Constructor
'''
super(MirrorState, self).__init__(outcomes=given_outcomes)
self._rate = rospy.Rate(100)
self._given_outcomes = given_outcomes
self._outcome_autonomy = outcome_autonomy
self._target_name = target_name
self._target_path = target_path
self._outcome_topic = 'flexbe/mirror/outcome'
self._pub = ProxyPublisher() #{'flexbe/behavior_update': String}
self._sub = ProxySubscriberCached({self._outcome_topic: UInt8})
def __init__(self, *args, **kwargs):
super(LockableState, self).__init__(*args, **kwargs)
self._locked = False
self._stored_outcome = None
self.__execute = self.execute
self.execute = self._lockable_execute
self._feedback_topic = 'flexbe/command_feedback'
self._lock_topic = 'flexbe/command/lock'
self._unlock_topic = 'flexbe/command/unlock'
self._pub = ProxyPublisher()
self._sub = ProxySubscriberCached()
def __init__(self, *args, **kwargs):
super(OperatableState, self).__init__(*args, **kwargs)
self.transitions = None
self.autonomy = None
self._mute = False # is set to true when used in silent state machine (don't report transitions)
self._last_requested_outcome = None
self._outcome_topic = 'flexbe/mirror/outcome'
self._request_topic = 'flexbe/outcome_request'
self._debug_topic = 'flexbe/debug/current_state'
self._pub = ProxyPublisher()
self.__execute = self.execute
self.execute = self._operatable_execute
def __init__(self):
'''
Constructor
'''
self._sm = None
# set up proxys for sm <--> GUI communication
# publish topics
self._pub = ProxyPublisher({'/flexbe/behavior_update': String,
'/flexbe/request_mirror_structure': Int32})
self._running = False
self._stopping = False
self._active_id = 0
self._starting_path = None
self._current_struct = None
self._outcome_topic = '/flexbe/mirror/outcome'
self._struct_buffer = list()
# listen for mirror message
self._sub = ProxySubscriberCached()
self._sub.subscribe(self._outcome_topic, UInt8)
self._sub.enable_buffer(self._outcome_topic)
self._sub.subscribe('/flexbe/mirror/structure', ContainerStructure, self._mirror_callback)
self._sub.subscribe('/flexbe/status', BEStatus, self._status_callback)
self._sub.subscribe('/flexbe/mirror/sync', BehaviorSync, self._sync_callback)
self._sub.subscribe('/flexbe/mirror/preempt', Empty, self._preempt_callback)
def __init__(self, topic):
"""Constructor"""
super(PublishPoseState, self).__init__(outcomes=['done'],
input_keys=['pose'])
self._topic = topic
self._pub = ProxyPublisher({self._topic: PoseStamped})
def __init__(self):
"""
Constructor
"""
self._sm = None
smach.set_loggers(
rospy.logdebug, rospy.logwarn, rospy.logdebug, rospy.logerr # hide SMACH transition log spamming
)
# set up proxys for sm <--> GUI communication
# publish topics
self._pub = ProxyPublisher({"flexbe/behavior_update": String, "flexbe/request_mirror_structure": Int32})
self._running = False
self._stopping = False
self._active_id = 0
self._starting_path = None
self._current_struct = None
self._outcome_topic = "flexbe/mirror/outcome"
self._struct_buffer = list()
# listen for mirror message
self._sub = ProxySubscriberCached()
self._sub.subscribe(self._outcome_topic, UInt8)
self._sub.enable_buffer(self._outcome_topic)
self._sub.subscribe("flexbe/mirror/structure", ContainerStructure, self._mirror_callback)
self._sub.subscribe("flexbe/status", BEStatus, self._status_callback)
self._sub.subscribe("flexbe/mirror/sync", BehaviorSync, self._sync_callback)
self._sub.subscribe("flexbe/mirror/preempt", Empty, self._preempt_callback)
class ManuallyTransitionableState(MonitoringState):
"""
A state for that a desired outcome can be declared.
If any outcome is declared, this outcome is forced.
"""
def __init__(self, *args, **kwargs):
super(ManuallyTransitionableState, self).__init__(*args, **kwargs)
self._force_transition = False
self.__execute = self.execute
self.execute = self._manually_transitionable_execute
self._feedback_topic = '/flexbe/command_feedback'
self._transition_topic = '/flexbe/command/transition'
self._pub = ProxyPublisher()
self._sub = ProxySubscriberCached()
def _manually_transitionable_execute(self, *args, **kwargs):
if self._is_controlled and self._sub.has_buffered(self._transition_topic):
command_msg = self._sub.get_from_buffer(self._transition_topic)
self._pub.publish(self._feedback_topic, CommandFeedback(command="transition", args=[command_msg.target, self.name]))
if command_msg.target != self.name:
rospy.logwarn("--> Requested outcome for state " + command_msg.target + " but active state is " + self.name)
else:
self._force_transition = True
outcome = self._outcome_list[command_msg.outcome]
rospy.loginfo("--> Manually triggered outcome " + outcome + " of state " + self.name)
return outcome
# return the normal outcome
self._force_transition = False
return self.__execute(*args, **kwargs)
def _enable_ros_control(self):
super(ManuallyTransitionableState, self)._enable_ros_control()
self._pub.createPublisher(self._feedback_topic, CommandFeedback)
self._sub.subscribe(self._transition_topic, OutcomeRequest)
self._sub.enable_buffer(self._transition_topic)
def _disable_ros_control(self):
super(ManuallyTransitionableState, self)._disable_ros_control()
self._sub.unsubscribe_topic(self._transition_topic)
class MirrorState(EventState):
'''
This state will display its possible outcomes as buttons in the GUI and is designed in a way to be created dynamically.
'''
def __init__(self, target_name, target_path, given_outcomes, outcome_autonomy):
'''
Constructor
'''
super(MirrorState, self).__init__(outcomes=given_outcomes)
self._rate = rospy.Rate(100)
self._given_outcomes = given_outcomes
self._outcome_autonomy = outcome_autonomy
self._target_name = target_name
self._target_path = target_path
self._outcome_topic = 'flexbe/mirror/outcome'
self._pub = ProxyPublisher() #{'flexbe/behavior_update': String}
self._sub = ProxySubscriberCached({self._outcome_topic: UInt8})
def execute(self, userdata):
'''
Execute this state
'''
if JumpableStateMachine.refresh:
JumpableStateMachine.refresh = False
self.on_enter(userdata)
if self._sub.has_buffered(self._outcome_topic):
msg = self._sub.get_from_buffer(self._outcome_topic)
if msg.data < len(self._given_outcomes):
rospy.loginfo("State update: %s > %s", self._target_name, self._given_outcomes[msg.data])
return self._given_outcomes[msg.data]
try:
self._rate.sleep()
except ROSInterruptException:
print 'Interrupted mirror sleep.'
def on_enter(self, userdata):
#rospy.loginfo("Mirror entering %s", self._target_path)
self._pub.publish('flexbe/behavior_update', String("/" + "/".join(self._target_path.split("/")[1:])))
def __init__(self):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(Drive_to_new, self).__init__(outcomes = ['succeeded'], input_keys = ['pose'])
self._topic = '/move_base/simple_goal'
self._pub = ProxyPublisher({self._topic: PoseStamped})
def __init__(self):
'''Constructor'''
super(LookAtTargetState, self).__init__(outcomes=['done'],
input_keys=['frame'])
self._head_control_topic = '/thor_mang/head_control_mode'
self._pub = ProxyPublisher({self._head_control_topic: HeadControlCommand})
def __init__(self):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(complete_task_StartHack, self).__init__(outcomes = ['error','done'], input_keys = ['task_details_task_id'])
self._topic_set_task_state = 'taskallocation/set_task_state'
self._pub = ProxyPublisher({self._topic_set_task_state: SetTaskState})
self._task_state_completed = TaskState()
self._task_state_completed.state = TaskState.COMPLETED
def __init__(self):
'''Constructor'''
super(SendToOperatorState, self).__init__(outcomes = ['done', 'no_connection'],
input_keys = ['data'])
self._data_topic = "/flexbe/data_to_ocs"
self._pub = ProxyPublisher({self._data_topic: String})
self._failed = False
def __init__(self):
super(ShowPictureWebinterfaceState, self).__init__(outcomes = ['tweet', 'forget'],
input_keys = ['image_name'])
self._pub_topic = '/webinterface/display_picture'
self._pub = ProxyPublisher({self._pub_topic: String})
self._sub_topic = '/webinterface/dialog_feedback'
self._sub = ProxySubscriberCached({self._sub_topic: String})
def __init__(self, useMoveBase=True):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(Send_Request_new, self).__init__(outcomes = ['succeeded'], input_keys = ['goalId','params_distance','pose'])
#self.userdata.goalId = 'none'
#self.userdata.frameId = frameId
topic = 'move_base/observe' if useMoveBase else 'controller/goal'
self._topic_move_base_goal = topic
self._pub = ProxyPublisher({self._topic_move_base_goal: MoveBaseActionGoal})
def __init__(self, command):
'''Constructor'''
super(RobotStateCommandState, self).__init__(outcomes = ['done', 'failed'])
self._topic = '/flor/controller/robot_state_command'
self._pub = ProxyPublisher({self._topic: FlorRobotStateCommand})
self._command = command
self._failed = False
class confirm_victim(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(confirm_victim, self).__init__(outcomes=["succeeded"], input_keys=["task_details_task_id"])
self._topicVictimReached = "victimReached"
self._pub = ProxyPublisher({self._topicVictimReached: VictimAnswer})
def execute(self, userdata):
answer = VictimAnswer()
answer.task_id = userdata.task_details_task_id
answer.answer = VictimAnswer.CONFIRM
self._pub.publish(self._topicVictimReached, answer)
Logger.loginfo("Victim Found, id = " + str(userdata.task_details_task_id))
return "succeeded"
def on_enter(self, userdata):
pass
def on_exit(self, userdata):
pass # Nothing to do in this example.
def on_start(self):
pass
def on_stop(self):
pass # Nothing to do in this example.
def __init__(self):
'''
Constructor
'''
super(DrivepathTestNew, self).__init__(outcomes=['reached', 'failed'])
self._failed = False
self._reached = False
self._pathTopic = '/controller/path'
self._pub = ProxyPublisher({self._pathTopic: MoveBaseActionPath})
class SendToOperatorState(EventState):
'''
Sends the provided data to the operator using a generic serialized topic.
># data object The data to be sent to the operator.
You should be aware of required bandwidth consumption.
<= done Data has been sent to onboard.
This is no guarantee that it really has been received.
<= no_connection Unable to send the data.
'''
def __init__(self):
'''Constructor'''
super(SendToOperatorState, self).__init__(outcomes = ['done', 'no_connection'],
input_keys = ['data'])
self._data_topic = "/flexbe/data_to_ocs"
self._pub = ProxyPublisher({self._data_topic: String})
self._failed = False
def execute(self, userdata):
if self._failed:
return 'no_connection'
return 'done'
def on_enter(self, userdata):
self._failed = False
try:
self._pub.publish(self._data_topic, String(pickle.dumps(userdata.data)))
except Exception as e:
Logger.logwarn('Failed to send data to OCS:\n%s' % str(e))
self._failed = True
def __init__(self, *args, **kwargs):
super(ManuallyTransitionableState, self).__init__(*args, **kwargs)
self._force_transition = False
self.__execute = self.execute
self.execute = self._manually_transitionable_execute
self._feedback_topic = '/flexbe/command_feedback'
self._transition_topic = '/flexbe/command/transition'
self._pub = ProxyPublisher()
self._sub = ProxySubscriberCached()
def __init__(self, *args, **kwargs):
super(OperatableStateMachine, self).__init__(*args, **kwargs)
self._message = None
self.id = None
self.autonomy = None
self._autonomy = {}
self._ordered_states = []
self._pub = ProxyPublisher()
self._sub = ProxySubscriberCached()
def __init__(self):
"""
Constructor
"""
super(MoveAlongPath, self).__init__(outcomes=["reached", "failed"], input_keys=["path", "speed"])
self._failed = False
self._reached = False
self._pathTopic = "/controller/path"
self._marker_topic = "/debug/path"
self._pub = ProxyPublisher({self._pathTopic: MoveBaseActionPath, self._marker_topic: MarkerArray})
class LookAtTargetState(EventState):
'''
A state that can look at any link target, e.g. one of the hands or a template.
># frame string Frame to be tracked, pass None to look straight.
<= done Command has been sent to head control.
'''
def __init__(self):
'''Constructor'''
super(LookAtTargetState, self).__init__(outcomes=['done'],
input_keys=['frame'])
self._head_control_topic = '/thor_mang/head_control_mode'
self._pub = ProxyPublisher({self._head_control_topic: HeadControlCommand})
def execute(self, userdata):
'''Execute this state'''
return 'done'
def on_enter(self, userdata):
'''Entering the state.'''
command = HeadControlCommand()
if userdata.frame is not None:
command.motion_type = HeadControlCommand.TRACK_FRAME
command.tracking_frame = userdata.frame
else:
command.motion_type = HeadControlCommand.LOOK_STRAIGHT
self._pub.publish(self._head_control_topic, command)
def __init__(self):
'''
Constructor
'''
self.be = None
Logger.initialize()
#ProxyPublisher._simulate_delay = True
#ProxySubscriberCached._simulate_delay = True
# prepare temp folder
rp = rospkg.RosPack()
self._tmp_folder = os.path.join(rp.get_path('flexbe_onboard'), 'tmp/')
if not os.path.exists(self._tmp_folder):
os.makedirs(self._tmp_folder)
sys.path.append(self._tmp_folder)
# prepare manifest folder access
manifest_folder = os.path.join(rp.get_path('flexbe_behaviors'), 'behaviors/')
rospy.loginfo("Parsing available behaviors...")
file_entries = [os.path.join(manifest_folder, filename) for filename in os.listdir(manifest_folder) if not filename.startswith('#')]
manifests = sorted([xmlpath for xmlpath in file_entries if not os.path.isdir(xmlpath)])
self._behavior_lib = dict()
for i in range(len(manifests)):
m = ET.parse(manifests[i]).getroot()
e = m.find("executable")
self._behavior_lib[i] = {"name": m.get("name"), "package": e.get("package_path").split(".")[0], "file": e.get("package_path").split(".")[1], "class": e.get("class")}
# rospy.loginfo("+++ " + self._behavior_lib[i]["name"])
self._pub = ProxyPublisher()
self._sub = ProxySubscriberCached()
self.status_topic = '/flexbe/status'
self.feedback_topic = '/flexbe/command_feedback'
self._pub.createPublisher(self.status_topic, BEStatus, _latch = True)
self._pub.createPublisher(self.feedback_topic, CommandFeedback)
# listen for new behavior to start
self._running = False
self._switching = False
self._sub.subscribe('/flexbe/start_behavior', BehaviorSelection, self._behavior_callback)
# heartbeat
self._pub.createPublisher('/flexbe/heartbeat', Empty)
self._execute_heartbeat()
rospy.sleep(0.5) # wait for publishers etc to really be set up
self._pub.publish(self.status_topic, BEStatus(code=BEStatus.READY))
rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m')
def __init__(self, *args, **kwargs):
super(OperatableState, self).__init__(*args, **kwargs)
self.transitions = None
self.autonomy = None
self._mute = False # is set to true when used in silent state machine (don't report transitions)
self._sent_outcome_requests = [] # contains those outcomes that already requested a transition
self._outcome_topic = '/flexbe/mirror/outcome'
self._request_topic = '/flexbe/outcome_request'
self._pub = ProxyPublisher()
self.__execute = self.execute
self.execute = self._operatable_execute
