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 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 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
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
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:])))
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.
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
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)
class Send_Request(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(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 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 rospy.Time.now() - self._start_time < self._target_time:
return 'succeeded' # One of the outcomes declared above.
def on_enter(self, userdata):
# This method is called when the state becomes active, i.e. a transition from another state to this one is taken.
# It is primarily used to start actions which are associated with this state.
# The following code is just for illustrating how the behavior logger works.
# Text logged by the behavior logger is sent to the operator and displayed in the GUI.
##time_to_wait = rospy.Time.now() - self._start_time - self._target_time
##if time_to_wait > 0:
##Logger.loginfo('Need to wait for %.1f seconds.' % time_to_wait)
goal = MoveBaseActionGoal()
goal.header.stamp = Time.now()
goal.header.frame_id = userdata.frameId
goal.goal.target_pose.pose.position = userdata.pose.position
goal.goal.distance = userdata.params['distance']
# "straighten up" given orientation
yaw = euler_from_quaternion([userdata.pose.orientation.x, userdata.pose.orientation.y, userdata.pose.orientation.z, self.pose.orientation.w])[2]
goal.goal.target_pose.pose.orientation.x = 0
goal.goal.target_pose.pose.orientation.y = 0
goal.goal.target_pose.pose.orientation.z = math.sin(yaw/2)
goal.goal.target_pose.pose.orientation.w = math.cos(yaw/2)
goal.goal.target_pose.header.frame_id = userdata.frameId
goal.goal_id.id = 'driveTo' + str(goal.header.stamp.secs) + '.' + str(goal.header.stamp.nsecs)
goal.goal_id.stamp = goal.header.stamp
userdata.goalId = goal.goal_id.id
self._pub.publish(self._topic_move_base_goal, goal)
return 'succeeded'
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.
# In this example, we use this event to set the correct start time.
##self._start_time = rospy.Time.now()
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.
class VigirBeOnboard(object):
'''
Implements an idle state where the robot waits for a behavior to be started.
'''
def __init__(self):
'''
Constructor
'''
self.be = None
Logger.initialize()
smach.set_loggers(
rospy.logdebug, # hide SMACH transition log spamming
rospy.logwarn,
rospy.logdebug,
rospy.logerr)
#ProxyPublisher._simulate_delay = True
#ProxySubscriberCached._simulate_delay = True
# prepare temp folder
rp = rospkg.RosPack()
self._tmp_folder = "/tmp/flexbe_onboard"
if not os.path.exists(self._tmp_folder):
os.makedirs(self._tmp_folder)
sys.path.append(self._tmp_folder)
# prepare manifest folder access
self._behavior_lib = BehaviorLibrary()
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 _behavior_callback(self, msg):
thread = threading.Thread(target=self._behavior_execution, args=[msg])
thread.daemon = True
thread.start()
def _behavior_execution(self, msg):
if self._running:
Logger.loginfo('--> Initiating behavior switch...')
self._pub.publish(
self.feedback_topic,
CommandFeedback(command="switch", args=['received']))
else:
Logger.loginfo('--> Starting new behavior...')
be = self._prepare_behavior(msg)
if be is None:
Logger.logerr(
'Dropped behavior start request because preparation failed.')
if self._running:
self._pub.publish(
self.feedback_topic,
CommandFeedback(command="switch", args=['failed']))
else:
rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m')
return
if self._running:
if self._switching:
Logger.logwarn('Already switching, dropped new start request.')
return
self._pub.publish(
self.feedback_topic,
CommandFeedback(command="switch", args=['start']))
if not self._is_switchable(be):
Logger.logerr(
'Dropped behavior start request because switching is not possible.'
)
self._pub.publish(
self.feedback_topic,
CommandFeedback(command="switch", args=['not_switchable']))
return
self._switching = True
active_state = self.be.get_current_state()
rospy.loginfo("Current state %s is kept active.",
active_state.name)
try:
be.prepare_for_switch(active_state)
self._pub.publish(
self.feedback_topic,
CommandFeedback(command="switch", args=['prepared']))
except Exception as e:
Logger.logerr('Failed to prepare behavior switch:\n%s' %
str(e))
self._switching = False
self._pub.publish(
self.feedback_topic,
CommandFeedback(command="switch", args=['failed']))
return
rospy.loginfo('Preempting current behavior version...')
self.be.preempt_for_switch()
rate = rospy.Rate(10)
while self._running:
rate.sleep()
self._switching = False
self._running = True
self.be = be
result = ""
try:
rospy.loginfo('Behavior ready, execution starts now.')
rospy.loginfo('[%s : %s]', be.name, msg.behavior_checksum)
self.be.confirm()
args = [self.be.requested_state_path
] if self.be.requested_state_path is not None else []
self._pub.publish(
self.status_topic,
BEStatus(behavior_id=self.be.id,
code=BEStatus.STARTED,
args=args))
result = self.be.execute()
if self._switching:
self._pub.publish(
self.status_topic,
BEStatus(behavior_id=self.be.id, code=BEStatus.SWITCHING))
else:
self._pub.publish(
self.status_topic,
BEStatus(behavior_id=self.be.id,
code=BEStatus.FINISHED,
args=[str(result)]))
except Exception as e:
self._pub.publish(
self.status_topic,
BEStatus(behavior_id=msg.behavior_checksum,
code=BEStatus.FAILED))
Logger.logerr('Behavior execution failed!\n%s' % str(e))
import traceback
Logger.loginfo(traceback.format_exc())
result = "failed"
try:
self._cleanup_behavior(msg.behavior_checksum)
except Exception as e:
rospy.logerr('Failed to clean up behavior:\n%s' % str(e))
self.be = None
if not self._switching:
rospy.loginfo('Behavior execution finished with result %s.',
str(result))
rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m')
self._running = False
def _prepare_behavior(self, msg):
# get sourcecode from ros package
try:
rp = rospkg.RosPack()
behavior = self._behavior_lib.get_behavior(msg.behavior_id)
if behavior is None:
raise ValueError(msg.behavior_id)
be_filepath = os.path.join(
rp.get_path(behavior["package"]), 'src/' +
behavior["package"] + '/' + behavior["file"] + '_tmp.py')
if os.path.isfile(be_filepath):
be_file = open(be_filepath, "r")
rospy.logwarn(
"Found a tmp version of the referred behavior! Assuming local test run."
)
else:
be_filepath = os.path.join(
rp.get_path(behavior["package"]), 'src/' +
behavior["package"] + '/' + behavior["file"] + '.py')
be_file = open(be_filepath, "r")
be_content = be_file.read()
be_file.close()
except Exception as e:
Logger.logerr('Failed to retrieve behavior from library:\n%s' %
str(e))
self._pub.publish(
self.status_topic,
BEStatus(behavior_id=msg.behavior_checksum,
code=BEStatus.ERROR))
return
# apply modifications if any
try:
file_content = ""
last_index = 0
for mod in msg.modifications:
file_content += be_content[last_index:mod.
index_begin] + mod.new_content
last_index = mod.index_end
file_content += be_content[last_index:]
if zlib.adler32(file_content) != msg.behavior_checksum:
mismatch_msg = (
"Checksum mismatch of behavior versions! \n"
"Attempted to load behavior: %s\n"
"Make sure that all computers are on the same version a.\n"
"Also try: rosrun flexbe_widget clear_cache" %
str(be_filepath))
raise Exception(mismatch_msg)
else:
rospy.loginfo("Successfully applied %d modifications." %
len(msg.modifications))
except Exception as e:
Logger.logerr('Failed to apply behavior modifications:\n%s' %
str(e))
self._pub.publish(
self.status_topic,
BEStatus(behavior_id=msg.behavior_checksum,
code=BEStatus.ERROR))
return
# create temp file for behavior class
try:
file_path = os.path.join(self._tmp_folder,
'tmp_%d.py' % msg.behavior_checksum)
sc_file = open(file_path, "w")
sc_file.write(file_content)
sc_file.close()
except Exception as e:
Logger.logerr(
'Failed to create temporary file for behavior class:\n%s' %
str(e))
self._pub.publish(
self.status_topic,
BEStatus(behavior_id=msg.behavior_checksum,
code=BEStatus.ERROR))
return
# import temp class file and initialize behavior
try:
package = __import__("tmp_%d" % msg.behavior_checksum,
fromlist=["tmp_%d" % msg.behavior_checksum])
clsmembers = inspect.getmembers(
package, lambda member: inspect.isclass(member) and member.
__module__ == package.__name__)
beclass = clsmembers[0][1]
be = beclass()
rospy.loginfo('Behavior ' + be.name + ' created.')
except Exception as e:
Logger.logerr('Exception caught in behavior definition:\n%s' %
str(e))
self._pub.publish(
self.status_topic,
BEStatus(behavior_id=msg.behavior_checksum,
code=BEStatus.ERROR))
return
# import contained behaviors
contain_list = {}
try:
contain_list = self._build_contains(be, "")
except Exception as e:
Logger.logerr('Failed to load contained behaviors:\n%s' % str(e))
return
# initialize behavior parameters
if len(msg.arg_keys) > 0:
rospy.loginfo('The following parameters will be used:')
try:
for i in range(len(msg.arg_keys)):
if msg.arg_keys[i] == '':
# action call has empty string as default, not a valid param key
continue
key_splitted = msg.arg_keys[i].rsplit('/', 1)
if len(key_splitted) == 1:
behavior = ''
key = key_splitted[0]
rospy.logwarn(
'Parameter key %s has no path specification, assuming: /%s'
% (key, key))
else:
behavior = key_splitted[0]
key = key_splitted[1]
found = False
if behavior == '' and hasattr(be, key):
self._set_typed_attribute(be, key, msg.arg_values[i])
# propagate to contained behaviors
for b in contain_list:
if hasattr(contain_list[b], key):
self._set_typed_attribute(contain_list[b], key,
msg.arg_values[i], b)
found = True
for b in contain_list:
if b == behavior and hasattr(contain_list[b], key):
self._set_typed_attribute(contain_list[b], key,
msg.arg_values[i], b)
found = True
if not found:
rospy.logwarn('Parameter ' + msg.arg_keys[i] +
' (set to ' + msg.arg_values[i] +
') not implemented')
except Exception as e:
Logger.logerr('Failed to initialize parameters:\n%s' % str(e))
self._pub.publish(
self.status_topic,
BEStatus(behavior_id=msg.behavior_checksum,
code=BEStatus.ERROR))
return
# build state machine
try:
be.set_up(id=msg.behavior_checksum,
autonomy_level=msg.autonomy_level,
debug=False)
be.prepare_for_execution(
self._convert_input_data(msg.input_keys, msg.input_values))
rospy.loginfo('State machine built.')
except Exception as e:
Logger.logerr('Behavior construction failed!\n%s' % str(e))
self._pub.publish(
self.status_topic,
BEStatus(behavior_id=msg.behavior_checksum,
code=BEStatus.ERROR))
return
return be
def _is_switchable(self, be):
if self.be.name != be.name:
Logger.logerr(
'Unable to switch behavior, names do not match:\ncurrent: %s <--> new: %s'
% (self.be.name, be.name))
return False
# locked inside
# locked state exists in new behavior
#if self.be.id == be.id:
#Logger.logwarn('Behavior version ID is the same.')
# Logger.logwarn('Skipping behavior switch, version ID is the same.')
# return False
# ok, can switch
return True
def _cleanup_behavior(self, behavior_checksum):
del (sys.modules["tmp_%d" % behavior_checksum])
file_path = os.path.join(self._tmp_folder,
'tmp_%d.pyc' % behavior_checksum)
try:
os.remove(file_path)
except OSError:
pass
try:
os.remove(file_path + 'c')
except OSError:
pass
def _set_typed_attribute(self, obj, name, value, behavior=''):
attr = getattr(obj, name)
if type(attr) is int:
value = int(value)
elif type(attr) is long:
value = long(value)
elif type(attr) is float:
value = float(value)
elif type(attr) is bool:
value = (value != "0")
elif type(attr) is dict:
value = yaml.load(value)
setattr(obj, name, value)
suffix = ' (' + behavior + ')' if behavior != '' else ''
rospy.loginfo(name + ' = ' + str(value) + suffix)
def _convert_input_data(self, keys, values):
result = dict()
for k, v in zip(keys, values):
try:
result[k] = self._convert_dict(cast(v))
except ValueError:
# unquoted strings will raise a ValueError, so leave it as string in this case
result[k] = str(v)
except SyntaxError as se:
Logger.logwarn(
'Unable to parse input value for key "%s", assuming string:\n%s\%s',
k, str(v), str(se))
result[k] = str(v)
return result
def _build_contains(self, obj, path):
contain_list = dict(
(path + "/" + key, value)
for (key, value) in getattr(obj, 'contains', {}).items())
add_to_list = {}
for b_id, b_inst in contain_list.items():
add_to_list.update(self._build_contains(b_inst, b_id))
contain_list.update(add_to_list)
return contain_list
def _execute_heartbeat(self):
thread = threading.Thread(target=self._heartbeat_worker)
thread.daemon = True
thread.start()
def _heartbeat_worker(self):
while True:
self._pub.publish('flexbe/heartbeat', Empty())
time.sleep(1) # sec
class _attr_dict(dict):
__getattr__ = dict.__getitem__
def _convert_dict(self, o):
if isinstance(o, list):
return [self._convert_dict(e) for e in o]
elif isinstance(o, dict):
return self._attr_dict(
(k, self._convert_dict(v)) for k, v in o.items())
else:
return o
class FlexbeOnboard(object):
"""
Controls the execution of robot behaviors.
"""
def __init__(self):
self.be = None
Logger.initialize()
self._tracked_imports = list()
# prepare temp folder
self._tmp_folder = tempfile.mkdtemp()
sys.path.append(self._tmp_folder)
rospy.on_shutdown(self._cleanup_tempdir)
# prepare manifest folder access
self._behavior_lib = BehaviorLibrary()
# prepare communication
self.status_topic = 'flexbe/status'
self.feedback_topic = 'flexbe/command_feedback'
self._pub = ProxyPublisher({
self.feedback_topic: CommandFeedback,
'flexbe/heartbeat': Empty
})
self._pub.createPublisher(self.status_topic, BEStatus, _latch=True)
self._execute_heartbeat()
# listen for new behavior to start
self._enable_clear_imports = rospy.get_param('~enable_clear_imports',
False)
self._running = False
self._run_lock = threading.Lock()
self._switching = False
self._switch_lock = threading.Lock()
self._sub = ProxySubscriberCached()
self._sub.subscribe('flexbe/start_behavior', BehaviorSelection,
self._behavior_callback)
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 _behavior_callback(self, msg):
thread = threading.Thread(target=self._behavior_execution, args=[msg])
thread.daemon = True
thread.start()
# =================== #
# Main execution loop #
# ------------------- #
def _behavior_execution(self, msg):
# sending a behavior while one is already running is considered as switching
if self._running:
Logger.loginfo('--> Initiating behavior switch...')
self._pub.publish(
self.feedback_topic,
CommandFeedback(command="switch", args=['received']))
else:
Logger.loginfo('--> Starting new behavior...')
# construct the behavior that should be executed
be = self._prepare_behavior(msg)
if be is None:
Logger.logerr(
'Dropped behavior start request because preparation failed.')
if self._running:
self._pub.publish(
self.feedback_topic,
CommandFeedback(command="switch", args=['failed']))
else:
rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m')
return
# perform the behavior switch if required
with self._switch_lock:
self._switching = True
if self._running:
self._pub.publish(
self.feedback_topic,
CommandFeedback(command="switch", args=['start']))
# ensure that switching is possible
if not self._is_switchable(be):
Logger.logerr(
'Dropped behavior start request because switching is not possible.'
)
self._pub.publish(
self.feedback_topic,
CommandFeedback(command="switch",
args=['not_switchable']))
return
# wait if running behavior is currently starting or stopping
rate = rospy.Rate(100)
while not rospy.is_shutdown():
active_state = self.be.get_current_state()
if active_state is not None or not self._running:
break
rate.sleep()
# extract the active state if any
if active_state is not None:
rospy.loginfo("Current state %s is kept active.",
active_state.name)
try:
be.prepare_for_switch(active_state)
self._pub.publish(
self.feedback_topic,
CommandFeedback(command="switch",
args=['prepared']))
except Exception as e:
Logger.logerr(
'Failed to prepare behavior switch:\n%s' % str(e))
self._pub.publish(
self.feedback_topic,
CommandFeedback(command="switch", args=['failed']))
return
# stop the rest
rospy.loginfo('Preempting current behavior version...')
self.be.preempt()
# execute the behavior
with self._run_lock:
self._switching = False
self.be = be
self._running = True
result = None
try:
rospy.loginfo('Behavior ready, execution starts now.')
rospy.loginfo('[%s : %s]', be.name, msg.behavior_checksum)
self.be.confirm()
args = [self.be.requested_state_path
] if self.be.requested_state_path is not None else []
self._pub.publish(
self.status_topic,
BEStatus(behavior_id=self.be.id,
code=BEStatus.STARTED,
args=args))
result = self.be.execute()
if self._switching:
self._pub.publish(
self.status_topic,
BEStatus(behavior_id=self.be.id,
code=BEStatus.SWITCHING))
else:
self._pub.publish(
self.status_topic,
BEStatus(behavior_id=self.be.id,
code=BEStatus.FINISHED,
args=[str(result)]))
except Exception as e:
self._pub.publish(
self.status_topic,
BEStatus(behavior_id=msg.behavior_checksum,
code=BEStatus.FAILED))
Logger.logerr('Behavior execution failed!\n%s' % str(e))
import traceback
Logger.loginfo(traceback.format_exc())
result = result or "exception" # only set result if not executed
# done, remove left-overs like the temporary behavior file
try:
# do not clear imports for now, not working correctly (e.g., flexbe/flexbe_app#66)
# only if specifically enabled
if not self._switching and self._enable_clear_imports:
self._clear_imports()
self._cleanup_behavior(msg.behavior_checksum)
except Exception as e:
rospy.logerr('Failed to clean up behavior:\n%s' % str(e))
if not self._switching:
Logger.loginfo('Behavior execution finished with result %s.',
str(result))
rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m')
self._running = False
self.be = None
# ==================================== #
# Preparation of new behavior requests #
# ------------------------------------ #
def _prepare_behavior(self, msg):
# get sourcecode from ros package
try:
behavior = self._behavior_lib.get_behavior(msg.behavior_id)
if behavior is None:
raise ValueError(msg.behavior_id)
be_filepath = self._behavior_lib.get_sourcecode_filepath(
msg.behavior_id, add_tmp=True)
if os.path.isfile(be_filepath):
be_file = open(be_filepath, "r")
rospy.logwarn(
"Found a tmp version of the referred behavior! Assuming local test run."
)
else:
be_filepath = self._behavior_lib.get_sourcecode_filepath(
msg.behavior_id)
be_file = open(be_filepath, "r")
try:
be_content = be_file.read()
finally:
be_file.close()
except Exception as e:
Logger.logerr('Failed to retrieve behavior from library:\n%s' %
str(e))
self._pub.publish(
self.status_topic,
BEStatus(behavior_id=msg.behavior_checksum,
code=BEStatus.ERROR))
return
# apply modifications if any
try:
file_content = ""
last_index = 0
for mod in msg.modifications:
file_content += be_content[last_index:mod.
index_begin] + mod.new_content
last_index = mod.index_end
file_content += be_content[last_index:]
if zlib.adler32(file_content.encode()
) & 0x7fffffff != msg.behavior_checksum:
mismatch_msg = (
"Checksum mismatch of behavior versions! \n"
"Attempted to load behavior: %s\n"
"Make sure that all computers are on the same version a.\n"
"Also try: rosrun flexbe_widget clear_cache" %
str(be_filepath))
raise Exception(mismatch_msg)
else:
rospy.loginfo("Successfully applied %d modifications." %
len(msg.modifications))
except Exception as e:
Logger.logerr('Failed to apply behavior modifications:\n%s' %
str(e))
self._pub.publish(
self.status_topic,
BEStatus(behavior_id=msg.behavior_checksum,
code=BEStatus.ERROR))
return
# create temp file for behavior class
try:
file_path = os.path.join(self._tmp_folder,
'tmp_%d.py' % msg.behavior_checksum)
with open(file_path, "w") as sc_file:
sc_file.write(file_content)
except Exception as e:
Logger.logerr(
'Failed to create temporary file for behavior class:\n%s' %
str(e))
self._pub.publish(
self.status_topic,
BEStatus(behavior_id=msg.behavior_checksum,
code=BEStatus.ERROR))
return
# import temp class file and initialize behavior
try:
with self._track_imports():
package = __import__(
"tmp_%d" % msg.behavior_checksum,
fromlist=["tmp_%d" % msg.behavior_checksum])
clsmembers = inspect.getmembers(
package, lambda member: (inspect.isclass(member) and member
.__module__ == package.__name__))
beclass = clsmembers[0][1]
be = beclass()
rospy.loginfo('Behavior ' + be.name + ' created.')
except Exception as e:
Logger.logerr('Exception caught in behavior definition:\n%s\n'
'See onboard terminal for more information.' %
str(e))
import traceback
traceback.print_exc()
self._pub.publish(
self.status_topic,
BEStatus(behavior_id=msg.behavior_checksum,
code=BEStatus.ERROR))
if self._enable_clear_imports:
self._clear_imports()
return
# initialize behavior parameters
if len(msg.arg_keys) > 0:
rospy.loginfo('The following parameters will be used:')
try:
for i in range(len(msg.arg_keys)):
# action call has empty string as default, not a valid param key
if msg.arg_keys[i] == '':
continue
found = be.set_parameter(msg.arg_keys[i], msg.arg_values[i])
if found:
name_split = msg.arg_keys[i].rsplit('/', 1)
behavior = name_split[0] if len(name_split) == 2 else ''
key = name_split[-1]
suffix = ' (' + behavior + ')' if behavior != '' else ''
rospy.loginfo(key + ' = ' + msg.arg_values[i] + suffix)
else:
rospy.logwarn('Parameter ' + msg.arg_keys[i] +
' (set to ' + msg.arg_values[i] +
') not defined')
except Exception as e:
Logger.logerr('Failed to initialize parameters:\n%s' % str(e))
self._pub.publish(
self.status_topic,
BEStatus(behavior_id=msg.behavior_checksum,
code=BEStatus.ERROR))
return
# build state machine
try:
be.set_up(id=msg.behavior_checksum,
autonomy_level=msg.autonomy_level,
debug=False)
be.prepare_for_execution(
self._convert_input_data(msg.input_keys, msg.input_values))
rospy.loginfo('State machine built.')
except Exception as e:
Logger.logerr('Behavior construction failed!\n%s\n'
'See onboard terminal for more information.' %
str(e))
import traceback
traceback.print_exc()
self._pub.publish(
self.status_topic,
BEStatus(behavior_id=msg.behavior_checksum,
code=BEStatus.ERROR))
if self._enable_clear_imports:
self._clear_imports()
return
return be
# ================ #
# Helper functions #
# ---------------- #
def _is_switchable(self, be):
if self.be.name != be.name:
Logger.logerr(
'Unable to switch behavior, names do not match:\ncurrent: %s <--> new: %s'
% (self.be.name, be.name))
return False
# locked inside
# locked state exists in new behavior
# ok, can switch
return True
def _cleanup_behavior(self, behavior_checksum):
file_path = os.path.join(self._tmp_folder,
'tmp_%d.pyc' % behavior_checksum)
try:
os.remove(file_path)
except OSError:
pass
try:
os.remove(file_path + 'c')
except OSError:
pass
def _clear_imports(self):
for module in self._tracked_imports:
if module in sys.modules:
del sys.modules[module]
self._tracked_imports = list()
def _cleanup_tempdir(self):
try:
os.remove(self._tmp_folder)
except OSError:
pass
def _convert_input_data(self, keys, values):
result = dict()
for k, v in zip(keys, values):
# action call has empty string as default, not a valid input key
if k == '':
continue
try:
result[k] = self._convert_dict(cast(v))
except ValueError:
# unquoted strings will raise a ValueError, so leave it as string in this case
result[k] = str(v)
except SyntaxError as se:
Logger.loginfo(
'Unable to parse input value for key "%s", assuming string:\n%s\n%s'
% (k, str(v), str(se)))
result[k] = str(v)
return result
def _execute_heartbeat(self):
thread = threading.Thread(target=self._heartbeat_worker)
thread.daemon = True
thread.start()
def _heartbeat_worker(self):
while True:
self._pub.publish('flexbe/heartbeat', Empty())
time.sleep(1)
def _convert_dict(self, o):
if isinstance(o, list):
return [self._convert_dict(e) for e in o]
elif isinstance(o, dict):
return self._attr_dict(
(k, self._convert_dict(v)) for k, v in list(o.items()))
else:
return o
class _attr_dict(dict):
__getattr__ = dict.__getitem__
@contextlib.contextmanager
def _track_imports(self):
previous_modules = set(sys.modules.keys())
try:
yield
finally:
self._tracked_imports.extend(
set(sys.modules.keys()) - previous_modules)
class DrivepathTest(EventState):
'''
Lets the robot move along a given path.
># path PoseStamped[] Array of Positions of the robot.
># speed float Speed of the robot
<= reached Robot is now located at the specified waypoint.
<= failed Failed to send a motion request to the action server.
'''
def __init__(self):
'''
Constructor
'''
super(DrivepathTest, self).__init__(outcomes=['reached', 'failed'], input_keys = ['poses'])
self._failed = False
self._reached = False
self._path = MoveBaseActionPath()
self._pathTopic = '/controller/path'
self._pub = ProxyPublisher({self._pathTopic: MoveBaseActionPath})
def execute(self, userdata):
'''
Execute this state
'''
if self._failed:
return 'failed'
if self._reached:
return 'reached'
def on_enter(self, userdata):
self._failed = False
self._path.goal.target_path.poses = userdata.poses
self._path.goal.target_path.header.frame_id = 'map'
self._pub.publish(self._pathTopic, self._path)
self._reached = True
def on_stop(self):
pass
def on_exit(self, userdata):
pass
def on_pause(self):
self._move_client.cancel(self._action_topic)
def on_resume(self, userdata):
self.on_enter(userdata)
class VigirBehaviorMirror(object):
'''
classdocs
'''
def __init__(self):
'''
Constructor
'''
self._sm = None
smach.set_loggers (
rospy.logdebug, # hide SMACH transition log spamming
rospy.logwarn,
rospy.logdebug,
rospy.logerr
)
# 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 _mirror_callback(self, msg):
rate = rospy.Rate(10)
while self._stopping:
rate.sleep()
if self._running:
rospy.logwarn('Received a new mirror structure while mirror is already running, adding to buffer (ID: %s).' % str(msg.behavior_id))
elif self._active_id != 0 and msg.behavior_id != self._active_id:
rospy.logwarn('ID of received mirror structure (%s) does not match expected ID (%s), will ignore.' % (str(msg.behavior_id), str(self._active_id)))
return
else:
rospy.loginfo('Received a new mirror structure for ID %s' % str(msg.behavior_id))
self._struct_buffer.append(msg)
if self._active_id == msg.behavior_id:
self._struct_buffer = list()
self._mirror_state_machine(msg)
rospy.loginfo('Mirror built.')
self._execute_mirror()
def _status_callback(self, msg):
if msg.code == BEStatus.STARTED:
thread = threading.Thread(target=self._start_mirror, args=[msg])
thread.daemon = True
thread.start()
else:
thread = threading.Thread(target=self._stop_mirror, args=[msg])
thread.daemon = True
thread.start()
def _start_mirror(self, msg):
rate = rospy.Rate(10)
while self._stopping:
rate.sleep()
if self._running:
rospy.logwarn('Tried to start mirror while it is already running, will ignore.')
return
if len(msg.args) > 0:
self._starting_path = "/" + msg.args[0][1:].replace("/", "_mirror/") + "_mirror"
self._active_id = msg.behavior_id
while self._sm is None and len(self._struct_buffer) > 0:
struct = self._struct_buffer[0]
self._struct_buffer = self._struct_buffer[1:]
if struct.behavior_id == self._active_id:
self._mirror_state_machine(struct)
rospy.loginfo('Mirror built.')
else:
rospy.logwarn('Discarded mismatching buffered structure for ID %s' % str(struct.behavior_id))
if self._sm is None:
rospy.logwarn('Missing correct mirror structure, requesting...')
rospy.sleep(0.2) # no clean way to wait for publisher to be ready...
self._pub.publish('flexbe/request_mirror_structure', Int32(msg.behavior_id))
self._active_id = msg.behavior_id
return
self._execute_mirror()
def _stop_mirror(self, msg):
self._stopping = True
if self._sm is not None and self._running:
if msg is not None and msg.code == BEStatus.FINISHED:
rospy.loginfo('Onboard behavior finished successfully.')
self._pub.publish('flexbe/behavior_update', String())
elif msg is not None and msg.code == BEStatus.SWITCHING:
self._starting_path = None
rospy.loginfo('Onboard performing behavior switch.')
elif msg is not None and msg.code == BEStatus.READY:
rospy.loginfo('Onboard engine just started, stopping currently running mirror.')
self._pub.publish('flexbe/behavior_update', String())
elif msg is not None:
rospy.logwarn('Onboard behavior failed!')
self._pub.publish('flexbe/behavior_update', String())
PreemptableState.preempt = True
rate = rospy.Rate(10)
while self._running:
rate.sleep()
else:
rospy.loginfo('No onboard behavior is active.')
self._active_id = 0
self._sm = None
self._current_struct = None
self._sub.remove_last_msg(self._outcome_topic, clear_buffer=True)
if msg is not None and msg.code != BEStatus.SWITCHING:
rospy.loginfo('\033[92m--- Behavior Mirror ready! ---\033[0m')
self._stopping = False
def _sync_callback(self, msg):
if msg.behavior_id == self._active_id:
thread = threading.Thread(target=self._restart_mirror, args=[msg])
thread.daemon = True
thread.start()
else:
rospy.logerr('Cannot synchronize! Different behavior is running onboard, please stop execution!')
thread = threading.Thread(target=self._stop_mirror, args=[None])
thread.daemon = True
thread.start()
def _restart_mirror(self, msg):
rospy.loginfo('Restarting mirror for synchronization...')
self._sub.remove_last_msg(self._outcome_topic, clear_buffer=True)
if self._sm is not None and self._running:
PreemptableState.preempt = True
rate = rospy.Rate(10)
while self._running:
rate.sleep()
self._sm = None
if msg.current_state_checksum in self._state_checksums:
current_state_path = self._state_checksums[msg.current_state_checksum]
self._starting_path = "/" + current_state_path[1:].replace("/", "_mirror/") + "_mirror"
rospy.loginfo('Current state: %s' % current_state_path)
try:
self._mirror_state_machine(self._current_struct)
rospy.loginfo('Mirror built.')
self._execute_mirror()
except (AttributeError, smach.InvalidStateError):
rospy.loginfo('Stopping synchronization because behavior has stopped.')
def _execute_mirror(self):
self._running = True
rospy.loginfo("Executing mirror...")
if self._starting_path is not None:
LockableStateMachine.path_for_switch = self._starting_path
rospy.loginfo("Starting mirror in state " + self._starting_path)
self._starting_path = None
result = 'preempted'
try:
result = self._sm.execute()
except Exception as e:
rospy.loginfo('Catched exception on preempt:\n%s' % str(e))
if self._sm is not None:
self._sm.destroy()
self._running = False
rospy.loginfo('Mirror finished with result ' + result)
def _mirror_state_machine(self, msg):
self._current_struct = msg
self._state_checksums = dict()
root = None
for con_msg in msg.containers:
if con_msg.path.find('/') == -1:
root = con_msg.path
break
self._add_node(msg, root)
# calculate checksums of all states
for con_msg in msg.containers:
if con_msg.path.find('/') != -1:
self._state_checksums[zlib.adler32(con_msg.path)] = con_msg.path
def _add_node(self, msg, path):
#rospy.loginfo('Add node: '+path)
container = None
for con_msg in msg.containers:
if con_msg.path == path:
container = con_msg
break
transitions = None
if container.transitions is not None:
transitions = {}
for i in range(len(container.transitions)):
transitions[container.outcomes[i]] = container.transitions[i] + '_mirror'
path_frags = path.split('/')
container_name = path_frags[len(path_frags)-1]
if len(container.children) > 0:
sm_outcomes = []
for outcome in container.outcomes: sm_outcomes.append(outcome + '_mirror')
sm = JumpableStateMachine(outcomes=sm_outcomes)
with sm:
for child in container.children:
self._add_node(msg, path+'/'+child)
if len(transitions) > 0:
container_transitions = {}
for i in range(len(container.transitions)): container_transitions[sm_outcomes[i]] = transitions[container.outcomes[i]]
JumpableStateMachine.add(container_name + '_mirror', sm, transitions=container_transitions)
else:
self._sm = sm
else:
JumpableStateMachine.add(container_name + '_mirror', MirrorState(container_name, path, container.outcomes, container.autonomy), transitions=transitions)
def _jump_callback(self, msg):
start_time = rospy.Time.now()
current_elapsed = 0
jumpable = True
while self._sm is None or not self._sm.is_running():
current_elapsed = rospy.Time.now() - start_time
if current_elapsed > rospy.Duration.from_sec(10):
jumpable = False
break
rospy.sleep(0.05)
if jumpable:
self._sm.jump_to(msg.stateName)
else:
rospy.logwarn('Could not jump in mirror: Mirror does not exist or is not running!')
def _preempt_callback(self, msg):
if self._sm is not None and self._sm.is_running():
rospy.logwarn('Explicit preempting is currently ignored, mirror should be preempted by onboard behavior.')
else:
rospy.logwarn('Could not preempt mirror because it seems not to be running!')
class OperatableState(PreemptableState):
"""
A state that supports autonomy levels and silent mode.
Also, it allows being tracked by a GUI or anything else
as it reports each transition and its initial structure.
An additional method is provided to report custom status messages to the widget.
"""
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 _build_msg(self, prefix, msg):
"""
Adds this state to the initial structure message.
@type prefix: string
@param prefix: A path consisting of the container hierarchy containing this state.
@type msg: ContainerStructure
@param msg: The message that will finally contain the structure message.
"""
# set path
name = prefix + self.name
# no children
children = None
# set outcomes
outcomes = self._outcome_list
# set transitions and autonomy levels
transitions = []
autonomy = []
for i in range(len(outcomes)):
outcome = outcomes[i]
if outcome == 'preempted': # set preempt transition
transitions.append('preempted')
autonomy.append(-1)
else:
transitions.append(str(self.transitions[outcome]))
autonomy.append(self.autonomy[outcome])
# add to message
msg.containers.append(Container(name, children, outcomes, transitions, autonomy))
def _operatable_execute(self, *args, **kwargs):
outcome = self.__execute(*args, **kwargs)
if self._is_controlled:
# reset previously requested outcome if applicable
if self._last_requested_outcome is not None and outcome == OperatableState._loopback_name:
self._pub.publish(self._request_topic, OutcomeRequest(outcome=255, target=self._parent._get_path() + "/" + self.name))
self._last_requested_outcome = None
# request outcome because autonomy level is too low
if not self._force_transition and (self.autonomy.has_key(outcome) and self.autonomy[outcome] >= OperatableStateMachine.autonomy_level or
outcome != OperatableState._loopback_name and
self._get_path() in rospy.get_param('/flexbe/breakpoints', [])):
if outcome != self._last_requested_outcome:
self._pub.publish(self._request_topic, OutcomeRequest(outcome=self._outcome_list.index(outcome), target=self._parent._get_path() + "/" + self.name))
rospy.loginfo("<-- Want result: %s > %s", self.name, outcome)
StateLogger.log_state_execution(self._get_path(), self.__class__.__name__, outcome, not self._force_transition, False)
self._last_requested_outcome = outcome
outcome = OperatableState._loopback_name
# autonomy level is high enough, report the executed transition
elif outcome != OperatableState._loopback_name:
self._sent_outcome_requests = []
rospy.loginfo("State result: %s > %s", self.name, outcome)
self._pub.publish(self._outcome_topic, UInt8(self._outcome_list.index(outcome)))
self._pub.publish(self._debug_topic, String("%s > %s" % (self._get_path(), outcome)))
StateLogger.log_state_execution(self._get_path(), self.__class__.__name__, outcome, not self._force_transition, True)
self._force_transition = False
return outcome
def _notify_skipped(self):
super(OperatableState, self)._notify_skipped()
def _enable_ros_control(self):
super(OperatableState, self)._enable_ros_control()
self._pub.createPublisher(self._outcome_topic, UInt8)
self._pub.createPublisher(self._debug_topic, String)
self._pub.createPublisher(self._request_topic, OutcomeRequest)
class LeapMotionMonitor(EventState):
'''
Receive and classify LeapMotion messages. Can detect following situations: `no_object` detected, `still_object`, `moving_object`.
-- leap_motion_topic string Leap Motion topic.
-- exit_states string[] Stop monitoring and return outcome if detected situation in this list.
-- pose_topic string Topic for publishing detected object pose for vizualizaton purpose (may be Empty).
-- parameters float[] Parameteres: detection_period (s), position_tolerance (m), orientation_tolerance (rad).
#> pose object Object pose before exit.
<= no_object No object detected. `pose` key contains zero pose.
<= still_object Object presents and it is not moving. `pose` key contains averaged pose.
<= moving_object Object presents and it is moving. `pose` key contains last pose.
'''
def __init__(self, leap_motion_topic, exit_states, pose_topic = '', parameters = [2.0, 0.02, 0.2]):
super(LeapMotionMonitor, self).__init__(outcomes = ['no_object', 'still_object', 'moving_object'],
output_keys = ['pose'])
# store state parameter for later use.
self._leap_topic = leap_motion_topic
self._pose_topic = pose_topic
self._exit_states = exit_states
self._detecton_period = parameters[0]
self._position_tolerance = parameters[1]
self._orientation_tolerance = parameters[2]
# setup proxies
self._leap_subscriber = ProxySubscriberCached({ self._leap_topic: leapros })
if self._pose_topic:
self._pose_publisher = ProxyPublisher({ self._pose_topic: PoseStamped })
else:
self._pose_publisher = None
# pose buffers
self._pose = None
self._pose_prev = None
self._pose_averaged = None
self._nonstillness_stamp = None
def on_enter(self, userdata):
# reset pose buffers
self._pose = None
self._pose_prev = None
self._pose_averaged = None
self._nonstillness_stamp = None
Logger.loginfo('LeapMotionMonitor: start monitoring.')
def execute(self, userdata):
# check if new message is available
if self._leap_subscriber.has_msg(self._leap_topic):
# GET LEAP MOTION message
leap_msg = self._leap_subscriber.get_last_msg(self._leap_topic)
#Logger.loginfo('LeapMotionMonitor: leap_motion: \n%s' % str(leap_msg.palmpos))
# STORE POSE IN BUFFER
self._pose = PoseVectorized()
self._pose.header = Header(frame_id = 'leap_motion', stamp = rospy.Time.now())
self._pose.position = numpy.array([leap_msg.palmpos.x, leap_msg.palmpos.y, leap_msg.palmpos.z]) / 1000.0
# get axis directions from leap_msg
# TODO use quaternion_from_rxyx
axis_z = - numpy.array([leap_msg.normal.x, leap_msg.normal.y,leap_msg.normal.z, 0])
axis_y = - numpy.array([leap_msg.direction.x, leap_msg.direction.y,leap_msg.direction.z, 0])
axis_x = numpy.cross(axis_y[:3], axis_z[:3])
axis_x = numpy.append(axis_x, 0)
# construct transformation matrix
mat = numpy.matrix([ axis_x, axis_y, axis_z, numpy.array([0, 0, 0, 1]) ]).transpose()
#Logger.loginfo('mat: \n%s' % str(mat))
# obtain quaternion
self._pose.orientation = quaternion_from_matrix(mat)
# PUBLISH POSE
if self._pose_topic:
self._pose_publisher.publish(self._pose_topic, self._pose.toPoseStamped())
# CHECK FOR FIRST CALL
if self._pose_prev == None:
self._pose_prev = PoseVectorized(self._pose)
self._pose_averaged = PoseVectorized(self._pose)
self._nonstillness_stamp = rospy.Time.now()
return None
# POSE AVERAGING
self._pose_averaged.average_exp(self._pose, self._detecton_period)
# CHECK FOR NO_OBJECT
if PoseVectorized.eq(self._pose, self._pose_prev):
# messages are identical, now check how long they are in this state
if (self._pose.header.stamp - self._pose_prev.header.stamp).to_sec() > self._detecton_period:
# NO_OBJECT is detected
if 'no_object' in self._exit_states:
Logger.loginfo('LeapMotionMonitor: no_object state is detected.')
return 'no_object'
# wait until detection_period
return None
else:
self._pose_prev = self._pose
# CHECK FOR STILL_OBJECT
if PoseVectorized.eq_approx(self._pose, self._pose_averaged, self._position_tolerance, self._orientation_tolerance):
# check if averaging is performed long enought
if (self._pose.header.stamp - self._nonstillness_stamp).to_sec() > self._detecton_period:
# STILL_OBJECT is_detected
if 'still_object' in self._exit_states:
Logger.loginfo('LeapMotionMonitor: still_object state is detected.')
return 'still_object'
# wait for detection_period from the start
return None
else:
self._nonstillness_stamp = rospy.Time.now()
# DEFAULT: MOVING OBJECT
if 'moving_object' in self._exit_states:
Logger.loginfo('LeapMotionMonitor: moving_object state is detected.')
return 'moving_object'
else:
return None
def on_exit(self, userdata):
userdata.pose = self._pose.toPoseStamped()
class OperatableState(PreemptableState):
"""
A state that supports autonomy levels and silent mode.
Also, it allows being tracked by a GUI or anything else
as it reports each transition and its initial structure.
An additional method is provided to report custom status messages to the widget.
"""
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
def _build_msg(self, prefix, msg):
"""
Adds this state to the initial structure message.
@type prefix: string
@param prefix: A path consisting of the container hierarchy containing this state.
@type msg: ContainerStructure
@param msg: The message that will finally contain the structure message.
"""
# set path
name = prefix + self.name
# no children
children = None
# set outcomes
outcomes = self._outcome_list
# set transitions and autonomy levels
transitions = []
autonomy = []
for i in range(len(outcomes)):
outcome = outcomes[i]
if outcome == 'preempted': # set preempt transition
transitions.append('preempted')
autonomy.append(-1)
else:
transitions.append(str(self.transitions[outcome]))
autonomy.append(self.autonomy[outcome])
# add to message
msg.containers.append(Container(name, children, outcomes, transitions, autonomy))
def _operatable_execute(self, *args, **kwargs):
outcome = self.__execute(*args, **kwargs)
if self._is_controlled:
log_requested_outcome = outcome
# request outcome because autonomy level is too low
if not self._force_transition and (self.autonomy.has_key(outcome) and self.autonomy[outcome] >= OperatableStateMachine.autonomy_level):
if self._sent_outcome_requests.count(outcome) == 0:
self._pub.publish(self._request_topic, OutcomeRequest(outcome=self._outcome_list.index(outcome), target=self._parent._get_path() + "/" + self.name))
rospy.loginfo("<-- Want result: %s > %s", self.name, outcome)
StateLogger.log_state_execution(self._get_path(), self.__class__.__name__, outcome, not self._force_transition, False)
self._sent_outcome_requests.append(outcome)
outcome = OperatableState._loopback_name
# autonomy level is high enough, report the executed transition
elif outcome != OperatableState._loopback_name:
self._sent_outcome_requests = []
rospy.loginfo("State result: %s > %s", self.name, outcome)
self._pub.publish(self._outcome_topic, UInt8(self._outcome_list.index(outcome)))
StateLogger.log_state_execution(self._get_path(), self.__class__.__name__, outcome, not self._force_transition, True)
self._force_transition = False
return outcome
def _enable_ros_control(self):
super(OperatableState, self)._enable_ros_control()
self._pub.createPublisher(self._outcome_topic, UInt8)
self._pub.createPublisher(self._request_topic, OutcomeRequest)
class VigirBehaviorMirror(object):
'''
classdocs
'''
def __init__(self):
'''
Constructor
'''
self._sm = None
smach.set_loggers(
rospy.logdebug, # hide SMACH transition log spamming
rospy.logwarn,
rospy.logdebug,
rospy.logerr)
# 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._sync_lock = threading.Lock()
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 _mirror_callback(self, msg):
rate = rospy.Rate(10)
while self._stopping:
rate.sleep()
if self._running:
rospy.logwarn(
'Received a new mirror structure while mirror is already running, adding to buffer (checksum: %s).'
% str(msg.behavior_id))
elif self._active_id != 0 and msg.behavior_id != self._active_id:
rospy.logwarn(
'checksum of received mirror structure (%s) does not match expected checksum (%s), will ignore.'
% (str(msg.behavior_id), str(self._active_id)))
return
else:
rospy.loginfo('Received a new mirror structure for checksum %s' %
str(msg.behavior_id))
self._struct_buffer.append(msg)
if self._active_id == msg.behavior_id:
self._struct_buffer = list()
self._mirror_state_machine(msg)
rospy.loginfo('Mirror built.')
self._execute_mirror()
def _status_callback(self, msg):
if msg.code == BEStatus.STARTED:
thread = threading.Thread(target=self._start_mirror, args=[msg])
thread.daemon = True
thread.start()
else:
thread = threading.Thread(target=self._stop_mirror, args=[msg])
thread.daemon = True
thread.start()
def _start_mirror(self, msg):
with self._sync_lock:
rate = rospy.Rate(10)
while self._stopping:
rate.sleep()
if self._running:
rospy.logwarn(
'Tried to start mirror while it is already running, will ignore.'
)
return
if len(msg.args) > 0:
self._starting_path = "/" + msg.args[0][1:].replace(
"/", "_mirror/") + "_mirror"
self._active_id = msg.behavior_id
while self._sm is None and len(self._struct_buffer) > 0:
struct = self._struct_buffer[0]
self._struct_buffer = self._struct_buffer[1:]
if struct.behavior_id == self._active_id:
self._mirror_state_machine(struct)
rospy.loginfo('Mirror built.')
else:
rospy.logwarn(
'Discarded mismatching buffered structure for checksum %s'
% str(struct.behavior_id))
if self._sm is None:
rospy.logwarn(
'Missing correct mirror structure, requesting...')
rospy.sleep(
0.2
) # no clean wayacquire to wait for publisher to be ready...
self._pub.publish('flexbe/request_mirror_structure',
Int32(msg.behavior_id))
self._active_id = msg.behavior_id
return
self._execute_mirror()
def _stop_mirror(self, msg):
with self._sync_lock:
self._stopping = True
if self._sm is not None and self._running:
if msg is not None and msg.code == BEStatus.FINISHED:
rospy.loginfo('Onboard behavior finished successfully.')
self._pub.publish('flexbe/behavior_update', String())
elif msg is not None and msg.code == BEStatus.SWITCHING:
self._starting_path = None
rospy.loginfo('Onboard performing behavior switch.')
elif msg is not None and msg.code == BEStatus.READY:
rospy.loginfo(
'Onboard engine just started, stopping currently running mirror.'
)
self._pub.publish('flexbe/behavior_update', String())
elif msg is not None:
rospy.logwarn('Onboard behavior failed!')
self._pub.publish('flexbe/behavior_update', String())
PreemptableState.preempt = True
rate = rospy.Rate(10)
while self._running:
rate.sleep()
else:
rospy.loginfo('No onboard behavior is active.')
self._active_id = 0
self._sm = None
self._current_struct = None
self._sub.remove_last_msg(self._outcome_topic, clear_buffer=True)
if msg is not None and msg.code != BEStatus.SWITCHING:
rospy.loginfo('\033[92m--- Behavior Mirror ready! ---\033[0m')
self._stopping = False
def _sync_callback(self, msg):
if msg.behavior_id == self._active_id:
thread = threading.Thread(target=self._restart_mirror, args=[msg])
thread.daemon = True
thread.start()
else:
rospy.logerr(
'Cannot synchronize! Different behavior is running onboard, please stop execution!'
)
thread = threading.Thread(target=self._stop_mirror, args=[None])
thread.daemon = True
thread.start()
def _restart_mirror(self, msg):
with self._sync_lock:
rospy.loginfo('Restarting mirror for synchronization...')
self._sub.remove_last_msg(self._outcome_topic, clear_buffer=True)
if self._sm is not None and self._running:
PreemptableState.preempt = True
rate = rospy.Rate(10)
while self._running:
rate.sleep()
self._sm = None
if msg.current_state_checksum in self._state_checksums:
current_state_path = self._state_checksums[
msg.current_state_checksum]
self._starting_path = "/" + current_state_path[1:].replace(
"/", "_mirror/") + "_mirror"
rospy.loginfo('Current state: %s' % current_state_path)
try:
self._mirror_state_machine(self._current_struct)
rospy.loginfo('Mirror built.')
except (AttributeError, smach.InvalidStateError):
rospy.loginfo(
'Stopping synchronization because behavior has stopped.')
self._execute_mirror()
def _execute_mirror(self):
self._running = True
rospy.loginfo("Executing mirror...")
if self._starting_path is not None:
LockableStateMachine.path_for_switch = self._starting_path
rospy.loginfo("Starting mirror in state " + self._starting_path)
self._starting_path = None
result = 'preempted'
try:
result = self._sm.execute()
except Exception as e:
rospy.loginfo('Caught exception on preempt:\n%s' % str(e))
result = 'preempted'
if self._sm is not None:
self._sm.destroy()
self._running = False
rospy.loginfo('Mirror finished with result ' + result)
def _mirror_state_machine(self, msg):
self._current_struct = msg
self._state_checksums = dict()
root = None
for con_msg in msg.containers:
if con_msg.path.find('/') == -1:
root = con_msg.path
break
self._add_node(msg, root)
# calculate checksums of all states
for con_msg in msg.containers:
if con_msg.path.find('/') != -1:
self._state_checksums[zlib.adler32(
con_msg.path)] = con_msg.path
def _add_node(self, msg, path):
#rospy.loginfo('Add node: '+path)
container = None
for con_msg in msg.containers:
if con_msg.path == path:
container = con_msg
break
transitions = None
if container.transitions is not None:
transitions = {}
for i in range(len(container.transitions)):
transitions[container.
outcomes[i]] = container.transitions[i] + '_mirror'
path_frags = path.split('/')
container_name = path_frags[len(path_frags) - 1]
if len(container.children) > 0:
sm_outcomes = []
for outcome in container.outcomes:
sm_outcomes.append(outcome + '_mirror')
sm = JumpableStateMachine(outcomes=sm_outcomes)
with sm:
for child in container.children:
self._add_node(msg, path + '/' + child)
if len(transitions) > 0:
container_transitions = {}
for i in range(len(container.transitions)):
container_transitions[sm_outcomes[i]] = transitions[
container.outcomes[i]]
JumpableStateMachine.add(container_name + '_mirror',
sm,
transitions=container_transitions)
else:
self._sm = sm
else:
JumpableStateMachine.add(container_name + '_mirror',
MirrorState(container_name, path,
container.outcomes,
container.autonomy),
transitions=transitions)
def _jump_callback(self, msg):
start_time = rospy.Time.now()
current_elapsed = 0
jumpable = True
while self._sm is None or not self._sm.is_running():
current_elapsed = rospy.Time.now() - start_time
if current_elapsed > rospy.Duration.from_sec(10):
jumpable = False
break
rospy.sleep(0.05)
if jumpable:
self._sm.jump_to(msg.stateName)
else:
rospy.logwarn(
'Could not jump in mirror: Mirror does not exist or is not running!'
)
def _preempt_callback(self, msg):
if self._sm is not None and self._sm.is_running():
rospy.logwarn(
'Explicit preempting is currently ignored, mirror should be preempted by onboard behavior.'
)
else:
rospy.logwarn(
'Could not preempt mirror because it seems not to be running!')
class OperatableStateMachine(PreemptableStateMachine):
"""
A state machine that can be operated.
It synchronizes its current state with the mirror and supports some control mechanisms.
"""
autonomy_level = 3
silent_mode = False
def __init__(self, *args, **kwargs):
super(OperatableStateMachine, self).__init__(*args, **kwargs)
self._message = None
self._rate = rospy.Rate(10)
self._do_rate_sleep = True
self.id = None
self.autonomy = None
self._autonomy = {}
self._ordered_states = []
self._inner_sync_request = False
self._pub = ProxyPublisher()
self._sub = ProxySubscriberCached()
def execute(self, parent_ud=smach.UserData()):
# First time in this state machine
if self._current_state is None:
# Spew some info
smach.loginfo(
"State machine starting in initial state '%s' with userdata: \n\t%s"
% (self._current_label, list(self.userdata.keys())))
# Set initial state
self._set_current_state(self._initial_state_label)
# Initialize preempt state
self._preempted_label = None
self._preempted_state = None
# Call start callbacks
self.call_start_cbs()
# Copy input keys
self._copy_input_keys(parent_ud, self.userdata)
container_outcome = self._loopback_name
if self.id is not None:
# only top-level statemachine should loop for outcome
while container_outcome == self._loopback_name and not smach.is_shutdown(
):
# Update the state machine
container_outcome = self._async_execute(parent_ud)
else:
container_outcome = self._async_execute(parent_ud)
if smach.is_shutdown():
container_outcome = 'preempted'
return container_outcome
def _async_execute(self, parent_ud=smach.UserData()):
"""Run the state machine on entry to this state.
This will set the "closed" flag and spin up the execute thread. Once
this flag has been set, it will prevent more states from being added to
the state machine.
"""
# This will prevent preempts from getting propagated to non-existent children
with self._state_transitioning_lock:
# Check state consistency
try:
self.check_consistency()
except (smach.InvalidStateError, smach.InvalidTransitionError):
smach.logerr("Container consistency check failed.")
return None
# Set running flag
self._is_running = True
# Initialize container outcome
container_outcome = None
# Step through state machine
if self._is_running and not smach.is_shutdown():
# Update the state machine
container_outcome = self._update_once()
if self._do_rate_sleep and self._current_state is not None and not isinstance(
self._current_state, OperatableStateMachine):
try:
# sleep with the rate of the state and update container rate accordingly
self._rate = self._current_state._rate
self._rate.sleep()
except ROSInterruptException:
rospy.logwarn('Interrupted rate sleep.')
if container_outcome is not None and container_outcome != self._loopback_name:
# Copy output keys
self._copy_output_keys(self.userdata, parent_ud)
else:
container_outcome = self._loopback_name
# provide explicit sync as back-up functionality
# should be used only if there is no other choice
# since it requires additional 8 byte + header update bandwith and time to restart mirror
if self._inner_sync_request and self._get_deep_state() is not None:
self._inner_sync_request = False
if self.id is None:
self._parent._inner_sync_request = True
else:
msg = BehaviorSync()
msg.behavior_id = self.id
msg.current_state_checksum = zlib.adler32(
self._get_deep_state()._get_path())
self._pub.publish('flexbe/mirror/sync', msg)
# We're no longer running
self._is_running = False
return container_outcome
@staticmethod
def add(label, state, transitions=None, autonomy=None, remapping=None):
"""
Add a state to the opened state machine.
@type label: string
@param label: The label of the state being added.
@param state: An instance of a class implementing the L{State} interface.
@param transitions: A dictionary mapping state outcomes to other state
labels or container outcomes.
@param autonomy: A dictionary mapping state outcomes to their required
autonomy level
@param remapping: A dictionary mapping local userdata keys to userdata
keys in the container.
"""
self = StateMachine._currently_opened_container()
# add loopback transition to loopback states
if isinstance(state, LoopbackState):
transitions[LoopbackState._loopback_name] = label
autonomy[LoopbackState._loopback_name] = -1
if isinstance(state, OperatableStateMachine):
transitions[OperatableStateMachine._loopback_name] = label
autonomy[OperatableStateMachine._loopback_name] = -1
self._ordered_states.append(state)
state.name = label
state.transitions = transitions
state.autonomy = autonomy
state._parent = self
StateMachine.add(label, state, transitions, remapping)
self._autonomy[label] = autonomy
def replace(self, new_state):
old_state = self._states[new_state.name]
new_state.transitions = old_state.transitions
new_state.autonomy = old_state.autonomy
new_state._parent = old_state._parent
self._ordered_states[self._ordered_states.index(old_state)] = new_state
self._states[new_state.name] = new_state
def destroy(self):
self._notify_stop()
self._disable_ros_control()
self._sub.unsubscribe_topic('flexbe/command/autonomy')
self._sub.unsubscribe_topic('flexbe/command/sync')
self._sub.unsubscribe_topic('flexbe/command/attach')
self._sub.unsubscribe_topic('flexbe/request_mirror_structure')
StateLogger.shutdown()
def confirm(self, name, id):
"""
Confirms the state machine and triggers the creation of the structural message.
It is mandatory to call this function at the top-level state machine
between building it and starting its execution.
@type name: string
@param name: The name of this state machine to identify it.
"""
self.name = name
self.id = id
self._pub.createPublisher(
'flexbe/mirror/sync', BehaviorSync, _latch=True
) # Update mirror with currently active state (high bandwidth mode)
self._pub.createPublisher('flexbe/mirror/preempt', Empty,
_latch=True) # Preempts the mirror
self._pub.createPublisher(
'flexbe/mirror/structure',
ContainerStructure) # Sends the current structure to the mirror
self._pub.createPublisher('flexbe/log',
BehaviorLog) # Topic for logs to the GUI
self._pub.createPublisher(
'flexbe/command_feedback', CommandFeedback
) # Gives feedback about executed commands to the GUI
self._sub.subscribe('flexbe/command/autonomy', UInt8,
self._set_autonomy_level)
self._sub.subscribe('flexbe/command/sync', Empty, self._sync_callback)
self._sub.subscribe('flexbe/command/attach', UInt8,
self._attach_callback)
self._sub.subscribe('flexbe/request_mirror_structure', Int32,
self._mirror_structure_callback)
StateLogger.initialize(name)
if OperatableStateMachine.autonomy_level != 255:
self._enable_ros_control()
rospy.sleep(0.5) # no clean way to wait for publisher to be ready...
self._notify_start()
def _set_autonomy_level(self, msg):
""" Sets the current autonomy level. """
if OperatableStateMachine.autonomy_level != msg.data:
rospy.loginfo('--> Autonomy changed to %d', msg.data)
if msg.data < 0:
self.preempt()
else:
OperatableStateMachine.autonomy_level = msg.data
self._pub.publish('flexbe/command_feedback',
CommandFeedback(command="autonomy", args=[]))
def _sync_callback(self, msg):
rospy.loginfo("--> Synchronization requested...")
msg = BehaviorSync()
msg.behavior_id = self.id
while self._get_deep_state() is None:
rospy.sleep(0.1)
msg.current_state_checksum = zlib.adler32(
self._get_deep_state()._get_path())
self._pub.publish('flexbe/mirror/sync', msg)
self._pub.publish('flexbe/command_feedback',
CommandFeedback(command="sync", args=[]))
rospy.loginfo("<-- Sent synchronization message for mirror.")
def _attach_callback(self, msg):
rospy.loginfo("--> Enabling control...")
# set autonomy level
OperatableStateMachine.autonomy_level = msg.data
# enable control of states
self._enable_ros_control()
self._inner_sync_request = True
# send command feedback
cfb = CommandFeedback(command="attach")
cfb.args.append(self.name)
self._pub.publish('flexbe/command_feedback', cfb)
rospy.loginfo("<-- Sent attach confirm.")
def _mirror_structure_callback(self, msg):
rospy.loginfo("--> Creating behavior structure for mirror...")
msg = self._build_msg('')
msg.behavior_id = self.id
self._pub.publish('flexbe/mirror/structure', msg)
rospy.loginfo("<-- Sent behavior structure for mirror.")
def _transition_allowed(self, label, outcome):
return self._autonomy[label][
outcome] < OperatableStateMachine.autonomy_level
def _build_msg(self, prefix, msg=None):
"""
Adds this state machine to the initial structure message.
@type prefix: string
@param prefix: A path consisting of the container hierarchy containing this state.
@type msg: ContainerStructure
@param msg: The message that will finally contain the structure message.
"""
# set children
children = []
for state in self._ordered_states:
children.append(str(state.name))
# set name
name = prefix + (self.name if self.id is None else '')
if msg is None:
# top-level state machine (has no transitions)
self._message = ContainerStructure()
outcomes = list(self._outcomes)
transitions = None
autonomy = None
else:
# lower-level state machine
self._message = msg
outcomes = list(self.transitions)
# set transitions and autonomy
transitions = []
autonomy = []
for i in range(len(self.transitions)):
outcome = outcomes[i]
if outcome == 'preempted': # set preempt transition
transitions.append('preempted')
autonomy.append(-1)
else:
transitions.append(str(self.transitions[outcome]))
autonomy.append(self.autonomy[outcome])
# add to message
self._message.containers.append(
Container(name, children, outcomes, transitions, autonomy))
# build message for children
for state in self._ordered_states:
state._build_msg(name + '/', self._message)
# top-level state machine returns the message
if msg is None:
return self._message
def _notify_start(self):
for state in self._ordered_states:
if isinstance(state, LoopbackState):
state.on_start()
if isinstance(state, OperatableStateMachine):
state._notify_start()
def _enable_ros_control(self):
self._is_controlled = True
for state in self._ordered_states:
if isinstance(state, LoopbackState):
state._enable_ros_control()
if isinstance(state, OperatableStateMachine):
state._enable_ros_control()
def _notify_stop(self):
for state in self._ordered_states:
if isinstance(state, LoopbackState):
state.on_stop()
state._disable_ros_control()
if isinstance(state, OperatableStateMachine):
state._notify_stop()
def _disable_ros_control(self):
self._is_controlled = False
for state in self._ordered_states:
if isinstance(state, LoopbackState):
state._disable_ros_control()
if isinstance(state, OperatableStateMachine):
state._disable_ros_control()
def on_exit(self, userdata):
if self._current_state is not None:
ud = smach.Remapper(
self.userdata, self._current_state.get_registered_input_keys(),
self._current_state.get_registered_output_keys(),
self._remappings[self._current_state.name])
self._current_state._entering = True
self._current_state.on_exit(ud)
self._current_state = None
class SetJointStateBase(Dummy):
'''
Base class for states which move robot to named pose using FollowJointState controller.
Pose is loaded from binary parameter from Parameter Server as JointState message.
Then state activate FollowJointState controller and publish pose.
Movement is considered finished when position error is less then given tolerance.
-- controller string FollowJointState controller namespace.
-- tolerance float Position tolerance (rad).
-- timeout float Movement timeout (s).
-- joint_topic string Topic where actual pose published.
<= done Finished.
<= failed Failed to activate FollowJointState controller.
<= timeout Timeout reached.
'''
def __init__(self,
controller='motion/controller/joint_state_head',
tolerance=0.17,
timeout=10.0,
joint_topic="joint_states",
outcomes=['done', 'failed', 'timeout']):
super(SetJointStateBase, self).__init__(outcomes=outcomes)
# Store topic parameter for later use.
self._controller = controller
self._joint_topic = joint_topic
self._tolerance = tolerance
self._timeout = Duration.from_sec(timeout)
# create proxies
self._action_client = ProxyActionClient(
{self._controller: SetOperationalAction})
self._pose_publisher = ProxyPublisher(
{self._controller + '/in_joints_ref': JointState})
self._pose_subscriber = ProxySubscriberCached(
{self._joint_topic: JointState})
# timestamp
self._timestamp = None
# error in enter hook
self._error = False
def load_joint_state_msg(self, pose_ns, pose_param):
# derive parameter full name
if pose_ns:
pose_param = pose_ns + '/' + pose_param
# Load JointState message from Parameter Server
try:
goal_raw = rospy.get_param(pose_param)
except KeyError as e:
raise KeyError, "SetJointStateBase: Unable to get '" + pose_param + "' parameter."
if not isinstance(goal_raw, xmlrpclib.Binary):
raise TypeError, "SetJointStateBase: ROS parameter '" + pose_param + "' is not a binary data."
# deserialize
self._target_joint_state = JointState()
self._target_joint_state.deserialize(goal_raw.data)
# create joint index to simplify tolerance check
self._joint_target_pose = {
name: position
for name, position in izip(self._target_joint_state.name,
self._target_joint_state.position)
}
def on_enter(self, userdata):
self._error = False
# activate controller
actiavtion_request = SetOperationalGoal()
actiavtion_request.operational = True
actiavtion_request.resources = self._target_joint_state.name
try:
self._action_client.send_goal(self._controller, actiavtion_request)
except Exception as e:
Logger.logwarn(
'SetJointStateBase: Failed to send the SetOperational command:\n%s'
% str(e))
self._error = True
return
# set start timestamp
self._timestamp = Time.now()
def execute(self, userdata):
# error in start hook
if self._error:
return 'failed'
# check if controller is active
if not self._action_client.is_active(self._controller):
Logger.loginfo(
'SetJointStateBase: controller was deactivated by external cause.'
)
return 'failed'
# check if time elasped
if Time.now() - self._timestamp > self._timeout:
Logger.loginfo('SetJointStateBase: timeout.')
return 'timeout'
# publish goal pose
self._pose_publisher.publish(self._controller + '/in_joints_ref',
self._target_joint_state)
# check tolerance
joints_msg = self._pose_subscriber.get_last_msg(self._joint_topic)
on_position = True
for name, pos in izip(joints_msg.name, joints_msg.position):
target_pos = self._joint_target_pose.get(name)
if (target_pos != None):
if abs(target_pos - pos) > self._tolerance:
on_position = False
break
if on_position:
Logger.loginfo('SetJointStateBase: on position')
return 'done'
def on_exit(self, userdata):
if self._action_client.is_active(self._controller):
try:
self._action_client.cancel(self._controller)
except Exception as e:
Logger.logwarn('SetJointStateBase: failed to deactivate `' +
self._controller + '` controller:\n%s' % str(e))
class TurnState(EventState):
'''
Turns the robot in place to a set angle and with set speed.
Uses /odom data.
-- angle float Angle to turn to in degrees, +/- for direction.
-- speed float Turn speed
<= failed If behavior is unable to ready on time
<= done Example for a failure outcome.
'''
def __init__(self, angle, speed):
super(TurnState, self).__init__(outcomes=['failed', 'done'])
self.odom_data = None
self.initial_orientation = None
self.cur_orientation = None
self._angle = angle
self._speed = speed
self._turn = False
self._turn_angle = None
self.cmd_pub = Twist()
self._vel_topic = '/cmd_vel'
self._odom_topic = '/odom'
#create publisher passing it the vel_topic_name and msg_type
self.pub_cmd_vel = ProxyPublisher({self._vel_topic: Twist})
#create subscriber
self.sub_odom = ProxySubscriberCached({self._odom_topic: Odometry})
def execute(self, userdata):
if self.sub_odom.has_msg(self._odom_topic):
self.data = self.sub_odom.get_last_msg(self._odom_topic)
self.sub_odom.remove_last_msg(self._odom_topic)
# Logger.loginfo('New Current orientation from Odom data.pose.pose.orientation: %s' % self.data.pose.pose.orientation)
self.cur_orientation = self.data.pose.pose.orientation
cur_angle = transformations.euler_from_quaternion((self.cur_orientation.x, self.cur_orientation.y,
self.cur_orientation.z, self.cur_orientation.w))
start_angle = transformations.euler_from_quaternion((self.initial_orientation.x, self.initial_orientation.y,
self.initial_orientation.z, self.initial_orientation.w))
# Logger.loginfo('initial orientation: %f' % start_angle[2])
# Logger.loginfo('current orientation: %f' % cur_angle[2])
turned_so_far = math.fabs(cur_angle[2] - start_angle[2])
# Logger.loginfo ("The Robot turned so far: %s" % turned_so_far)
if (turned_so_far >= self._turn_angle):
# Turn completed
self._turn = False
Logger.loginfo ("Turn successfully completed!")
return 'done'
# Turn the robot
if self._turn:
self.cmd_pub.linear.x = 0.0
if self._angle > 0:
self.cmd_pub.angular.z = self._speed
else:
self.cmd_pub.angular.z = -self._speed
self.pub_cmd_vel.publish(self._vel_topic, self.cmd_pub)
else: # Send stop command to the robot if turning is completed
self.cmd_pub.angular.z = 0.0
self.pub_cmd_vel.publish(self._vel_topic, self.cmd_pub)
def on_exit(self, userdata):
self.cmd_pub.angular.z = 0.0
self.pub_cmd_vel.publish(self._vel_topic, self.cmd_pub)
Logger.loginfo("Turn in place COMPLETED! Exiting the state!")
# def on_start(self):
def on_enter(self, userdata):
Logger.loginfo("Turn in place READY!")
# self._start_time = rospy.Time.now() #bug detected! (move to on_enter)
if not self.pub_cmd_vel:
Logger.loginfo('Failed to setup the publisher.')
return 'failed'
# Read current orientation from /odom
if self.sub_odom.has_msg(self._odom_topic):
self.data = self.sub_odom.get_last_msg(self._odom_topic)
# Logger.loginfo('Set Initial Current Robot orientation: %s' % self.data.pose.pose.orientation)
self.cur_orientation = self.data.pose.pose.orientation
# TODO Seting publishing rate - should I do this in FlexBe????
r = rospy.Rate(30)
# Convert the input turn angle value from degrees to Rad
self._turn_angle = math.fabs((self._angle * math.pi)/ 180)
Logger.loginfo ("Set TurnAngle _ angle to turn to in Rad: %s" % self._turn_angle)
# Save the initial robot orientation in self.initial_orientation for latter use
# if not self.initial_orientation:
Logger.loginfo('Save the initial robot orientation ')
self.initial_orientation = self.cur_orientation
Logger.loginfo('Initial Robot orientation: %s' % self.initial_orientation)
# Enable turning
self._turn = True
def on_stop(self):
Logger.loginfo("Turn in place STOPPED!")
self.initial_orientation = None
self.cmd_pub.angular.z = 0.0
self.cmd_pub.linear.x = 0.0
self.pub_cmd_vel.publish(self._vel_topic, self.cmd_pub)
def sub_odom_callback(self, data):
self.data = data
class CreateWaypointState(EventState):
'''
This state defines the starting point for a pure pursuit system..
-- target_frame: string Coordinate frame of target point (default: 'map')
-- target_x: float Target point x-coordinate (m)
-- target_y: float Target point y-coordinate (m)
-- marker_topic: string topic of the RViz marker used for visualization (default: '/pure_pursuit_marker')
-- marker_size: float Size of RViz marker used for target (default: 0.05)
#> target_point object The target point (becomes next node's prior)
<= done Reached the end of target relevance
'''
def __init__(self,
target_frame='map',
target_x=1.0,
target_y=0.1,
marker_topic='/pure_pursuit_marker',
marker_size=0.05):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(CreateWaypointState, self).__init__(outcomes=['done'],
output_keys=['target_point'])
# Store state parameter for later use.
self._target = PointStamped()
self._target.header.stamp = rospy.Time.now()
self._target.header.frame_id = target_frame
self._target.point = Point(target_x, target_y, 0.0)
self._done = None # Track the outcome so we can detect if transition is blocked
self._marker_topic = marker_topic
self._marker_pub = ProxyPublisher({self._marker_topic: Marker})
self._marker = Marker()
self._marker.header.frame_id = target_frame
self._marker.header.stamp = rospy.Time.now()
self._marker.ns = "pure_pursuit_waypoints"
self._marker.id = int(target_x * 1000000) + int(target_y * 1000)
self._marker.type = Marker.SPHERE
self._marker.action = Marker.ADD
self._marker.pose.position.x = target_x
self._marker.pose.position.y = target_y
self._marker.pose.position.z = 0.0
self._marker.pose.orientation.x = 0.0
self._marker.pose.orientation.y = 0.0
self._marker.pose.orientation.z = 0.0
self._marker.pose.orientation.w = 1.0
self._marker.scale.x = marker_size
self._marker.scale.y = marker_size
self._marker.scale.z = marker_size
self._marker.color.a = 1.0 # Don't forget to set the alpha!
self._marker.color.r = 0.0
self._marker.color.g = 1.0
self._marker.color.b = 1.0
# This is only to pass data along to the next state
def execute(self, userdata):
userdata.target_point = self._target # Set the user data for passing to next node
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.
self._marker.action = Marker.MODIFY
self._marker.color.a = 1.0
self._marker.color.r = 0.0
self._marker.color.g = 1.0 # Indicate this target is active
self._marker.color.b = 0.0
self._marker_pub.publish(self._marker_topic, self._marker)
def on_exit(self, userdata):
# This method is called when the state becomes active, i.e. a transition from another state to this one is taken.
self._marker.color.a = 1.0 # Don't forget to set the alpha!
self._marker.color.r = 0.8 # Indicate this target is no longer active
self._marker.color.g = 0.0
self._marker.color.b = 0.0
self._marker_pub.publish(self._marker_topic, self._marker)
def on_start(self):
# This method is called when the state becomes active, i.e. a transition from another state to this one is taken.
self._marker.action = Marker.ADD
self._marker.color.a = 1.0 # Set alpha otherwise the marker is invisible
self._marker.color.r = 0.0
self._marker.color.g = 1.0
self._marker.color.b = 1.0 # Indicate this target is planned
self._marker_pub.publish(self._marker_topic, self._marker)
class VigirBeOnboard(object):
'''
Implements an idle state where the robot waits for a behavior to be started.
'''
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 _behavior_callback(self, msg):
thread = threading.Thread(target=self._behavior_execution, args=[msg])
thread.daemon = True
thread.start()
def _behavior_execution(self, msg):
if self._running:
Logger.loginfo('--> Initiating behavior switch...')
self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['received']))
else:
Logger.loginfo('--> Starting new behavior...')
be = self._prepare_behavior(msg)
if be is None:
Logger.logerr('Dropped behavior start request because preparation failed.')
if self._running:
self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['failed']))
else:
rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m')
return
if self._running:
if self._switching:
Logger.logwarn('Already switching, dropped new start request.')
return
self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['start']))
if not self._is_switchable(be):
Logger.logerr('Dropped behavior start request because switching is not possible.')
self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['not_switchable']))
return
self._switching = True
active_state = self.be.get_current_state()
rospy.loginfo("Current state %s is kept active.", active_state.name)
try:
be.prepare_for_switch(active_state)
self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['prepared']))
except Exception as e:
Logger.logerr('Failed to prepare behavior switch:\n%s' % str(e))
self._switching = False
self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['failed']))
return
rospy.loginfo('Preempting current behavior version...')
self.be.preempt_for_switch()
rate = rospy.Rate(10)
while self._running:
rate.sleep()
self._switching = False
self._running = True
self.be = be
result = ""
try:
rospy.loginfo('Behavior ready, execution starts now.')
rospy.loginfo('[%s : %s]', be.name, msg.behavior_checksum)
self.be.confirm()
args = [self.be.requested_state_path] if self.be.requested_state_path is not None else []
self._pub.publish(self.status_topic, BEStatus(behavior_id=self.be.id, code=BEStatus.STARTED, args=args))
result = self.be.execute()
if self._switching:
self._pub.publish(self.status_topic, BEStatus(behavior_id=self.be.id, code=BEStatus.SWITCHING))
else:
self._pub.publish(self.status_topic, BEStatus(behavior_id=self.be.id, code=BEStatus.FINISHED))
except Exception as e:
self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.FAILED))
Logger.logerr('Behavior execution failed!\n%s' % str(e))
result = "failed"
try:
self._cleanup_behavior(msg.behavior_checksum)
except Exception as e:
rospy.logerr('Failed to clean up behavior:\n%s' % str(e))
self.be = None
if not self._switching:
rospy.loginfo('Behavior execution finished with result %s.', str(result))
rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m')
self._running = False
def _prepare_behavior(self, msg):
# get sourcecode from ros package
try:
rp = rospkg.RosPack()
behavior = self._behavior_lib[msg.behavior_id]
be_filepath = os.path.join(rp.get_path(behavior["package"]), 'src/' + behavior["package"] + '/' + behavior["file"] + '_tmp.py')
if os.path.isfile(be_filepath):
be_file = open(be_filepath, "r")
rospy.logwarn("Found a tmp version of the referred behavior! Assuming local test run.")
else:
be_filepath = os.path.join(rp.get_path(behavior["package"]), 'src/' + behavior["package"] + '/' + behavior["file"] + '.py')
be_file = open(be_filepath, "r")
be_content = be_file.read()
be_file.close()
except Exception as e:
Logger.logerr('Failed to retrieve behavior from library:\n%s' % str(e))
self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR))
return
# apply modifications if any
try:
file_content = ""
last_index = 0
for mod in msg.modifications:
file_content += be_content[last_index:mod.index_begin] + mod.new_content
last_index = mod.index_end
file_content += be_content[last_index:]
if zlib.adler32(file_content) != msg.behavior_checksum:
mismatch_msg = ("checksum mismatch of behavior versions! \n"
"Attempted to load behavior: %s" % str(be_filepath))
raise Exception(mismatch_msg)
else:
rospy.loginfo("Successfully applied %d modifications." % len(msg.modifications))
except Exception as e:
Logger.logerr('Failed to apply behavior modifications:\n%s' % str(e))
self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR))
return
# create temp file for behavior class
try:
file_path = os.path.join(self._tmp_folder, 'tmp_%d.py' % msg.behavior_checksum)
sc_file = open(file_path, "w")
sc_file.write(file_content)
sc_file.close()
except Exception as e:
Logger.logerr('Failed to create temporary file for behavior class:\n%s' % str(e))
self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR))
return
# import temp class file and initialize behavior
try:
package = __import__("tmp_%d" % msg.behavior_checksum, fromlist=["tmp_%d" % msg.behavior_checksum])
clsmembers = inspect.getmembers(package, lambda member: inspect.isclass(member) and member.__module__ == package.__name__)
beclass = clsmembers[0][1]
be = beclass()
rospy.loginfo('Behavior ' + be.name + ' created.')
except Exception as e:
Logger.logerr('Unable to import temporary behavior file:\n%s' % str(e))
self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR))
return
# import contained behaviors
contain_list = {}
try:
contain_list = self._build_contains(be, "")
except Exception as e:
Logger.logerr('Failed to load contained behaviors:\n%s' % str(e))
return
# initialize behavior parameters
if len(msg.arg_keys) > 0:
rospy.loginfo('The following parameters will be used:')
try:
for i in range(len(msg.arg_keys)):
key_splitted = msg.arg_keys[i].rsplit('/', 1)
behavior = key_splitted[0]
key = key_splitted[1]
found = False
if behavior == '' and hasattr(be, key):
self._set_typed_attribute(be, key, msg.arg_values[i])
found = True
for b in contain_list:
if hasattr(contain_list[b], key):
self._set_typed_attribute(contain_list[b], key, msg.arg_values[i], b)
found = True
if not found:
rospy.logwarn('Parameter ' + msg.arg_keys[i] + ' (set to ' + msg.arg_values[i] + ') not implemented')
except Exception as e:
Logger.logerr('Failed to initialize parameters:\n%s' % str(e))
self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR))
return
# build state machine
try:
be.set_up(id=msg.behavior_checksum, autonomy_level=msg.autonomy_level, debug=False)
be.prepare_for_execution()
rospy.loginfo('State machine built.')
except Exception as e:
Logger.logerr('Behavior construction failed!\n%s' % str(e))
self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR))
return
return be
def _is_switchable(self, be):
if self.be.name != be.name:
Logger.logerr('Unable to switch behavior, names do not match:\ncurrent: %s <--> new: %s' % (self.be.name, be.name))
return False
# locked inside
# locked state exists in new behavior
#if self.be.id == be.id:
#Logger.logwarn('Behavior version ID is the same.')
# Logger.logwarn('Skipping behavior switch, version ID is the same.')
# return False
# ok, can switch
return True
def _cleanup_behavior(self, behavior_checksum):
del(sys.modules["tmp_%d" % behavior_checksum])
file_path = os.path.join(self._tmp_folder, 'tmp_%d.pyc' % behavior_checksum)
os.remove(file_path)
def _set_typed_attribute(self, obj, name, value, behavior=''):
attr = getattr(obj, name)
if type(attr) is int:
value = int(value)
elif type(attr) is long:
value = long(value)
elif type(attr) is float:
value = float(value)
elif type(attr) is bool:
value = (value != "0")
setattr(obj, name, value)
suffix = ' (' + behavior + ')' if behavior != '' else ''
rospy.loginfo(name + ' = ' + str(value) + suffix)
def _build_contains(self, obj, path):
contain_list = dict((path+"/"+key,value) for (key,value) in getattr(obj, 'contains', {}).items())
add_to_list = {}
for b_id, b_inst in contain_list.items():
add_to_list.update(self._build_contains(b_inst, b_id))
contain_list.update(add_to_list)
return contain_list
def _execute_heartbeat(self):
thread = threading.Thread(target=self._heartbeat_worker)
thread.daemon = True
thread.start()
def _heartbeat_worker(self):
while True:
self._pub.publish('/flexbe/heartbeat', Empty())
time.sleep(1) # sec
class ExeRotateState(EventState):
def __init__(self, cmd_topic, Kp, max_vel, thr_rad):
super(ExeRotateState, self).__init__(output_keys = ['outcome'],
input_keys = ['path'],
outcomes = ['done'])
# Controller gain
self._Kp = Kp
self._max_vel = max_vel
# Controller threshold
self._thr = thr_rad
# Velocity command interface
self._cmd_topic = cmd_topic
self._pub = ProxyPublisher({self._cmd_topic: Twist}) # pass required clients as dict (topic: type)
# Current state feedback interface
self._tf_listener = tf.TransformListener()
self._error = False
def execute(self, userdata):
if self._error:
Logger.loginfo("Reading current orientation is failed!")
return 'done'
else:
# check goal is achieved
if abs(self.goal_pose - self.cur_pose) < self._thr :
return 'done'
# control
cmd = Twist()
cmd.linear.x = 0
cmd.angular.z = self._Kp*(self.goal_pose - self.cur_pose)
if cmd.angular.z > self._max_vel :
cmd.angular.z = self._max_vel
elif cmd.angular.z < -self._max_vel :
cmd.angular.z = -self._max_vel
self._pub.publish(self._cmd_topic, cmd)
try:
# get pose feed back
[cur_trans, cur_quat] = self._tf_listener.lookupTransform('/map','/base_footprint',rospy.Time(0))
cur_euler = tf.transformations.euler_from_quaternion( cur_quat )
self.cur_pose = cur_euler[2]
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
self._error = True
def on_enter(self, userdata):
# Set goal orientation as last element of given path
goal_euler = self.QuaternionToEuler( userdata.path.poses[0].pose.orientation )
self.goal_pose = goal_euler[2]
# Check current pose can be read
try:
[cur_trans, cur_quat] = self._tf_listener.lookupTransform('/map','/base_footprint',rospy.Time(0))
cur_euler = tf.transformations.euler_from_quaternion( cur_quat )
self.cur_pose = cur_euler[2]
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
self._error = True
def on_exit(self, userdata):
pass
def QuaternionToEuler(self, quat):
return tf.transformations.euler_from_quaternion( [quat.x, quat.y, quat.z, quat.w] )
class MoveAlongPath(EventState):
"""
Lets the robot move along a given path.
># path PoseStamped[] Array of Positions of the robot.
># speed float Speed of the robot
<= reached Robot is now located at the specified waypoint.
<= failed Failed to send a motion request to the action server.
"""
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})
def execute(self, userdata):
"""
Execute this state
"""
if self._failed:
return "failed"
if self._reached:
return "reached"
def on_enter(self, userdata):
ma = MarkerArray()
self._path = MoveBaseActionPath()
for i in range(len(userdata.path.poses)):
marker = Marker(type=Marker.ARROW)
marker.header = userdata.path.header
marker.pose = userdata.path.poses[i].pose
marker.scale.x = 0.2
marker.scale.y = 0.02
marker.scale.z = 0.02
marker.color.b = 1.0
marker.color.r = 0.9 - 0.7 * i / len(userdata.path.poses)
marker.color.g = 0.9 - 0.7 * i / len(userdata.path.poses)
marker.color.a = 0.8 - 0.5 * i / len(userdata.path.poses)
marker.id = i
ma.markers.append(marker)
self._failed = False
self._path.goal.target_path.poses = userdata.path.poses
self._path.goal.target_path.header.frame_id = "map"
self._pub.publish(self._pathTopic, self._path)
self._pub.publish(self._marker_topic, ma)
self._reached = True
def on_stop(self):
pass
def on_exit(self, userdata):
pass
def on_pause(self):
pass
def on_resume(self, userdata):
self.on_enter(userdata)
class EventState(OperatableState):
"""
A state that allows implementing certain events.
"""
def __init__(self, *args, **kwargs):
super(EventState, self).__init__(*args, **kwargs)
self._entering = True
self._skipped = False
self.__execute = self.execute
self.execute = self._event_execute
self._paused = False
self._last_active_container = None
self._feedback_topic = 'flexbe/command_feedback'
self._repeat_topic = 'flexbe/command/repeat'
self._pause_topic = 'flexbe/command/pause'
self._pub = ProxyPublisher()
self._sub = ProxySubscriberCached()
def _event_execute(self, *args, **kwargs):
if self._is_controlled and self._sub.has_msg(self._pause_topic):
msg = self._sub.get_last_msg(self._pause_topic)
if msg.data:
self._sub.remove_last_msg(self._pause_topic)
rospy.loginfo("--> Pausing in state %s", self.name)
self._pub.publish(self._feedback_topic,
CommandFeedback(command="pause"))
self._last_active_container = PriorityContainer.active_container
# claim priority to propagate pause event
PriorityContainer.active_container = self._get_path()
self._paused = True
if self._paused and not PreemptableState.preempt:
self._notify_skipped()
return self._loopback_name
if self._entering:
self._entering = False
self.on_enter(*args, **kwargs)
if self._skipped and not PreemptableState.preempt:
self._skipped = False
self.on_resume(*args, **kwargs)
execute_outcome = self.__execute(*args, **kwargs)
repeat = False
if self._is_controlled and self._sub.has_msg(self._repeat_topic):
rospy.loginfo("--> Repeating state %s", self.name)
self._sub.remove_last_msg(self._repeat_topic)
self._pub.publish(self._feedback_topic,
CommandFeedback(command="repeat"))
repeat = True
if execute_outcome != self._loopback_name and not PreemptableState.switching and not PreemptableState.preempt \
or repeat:
self._entering = True
self.on_exit(*args, **kwargs)
return execute_outcome
def _notify_skipped(self):
if not self._skipped:
self.on_pause()
self._skipped = True
if self._is_controlled and self._sub.has_msg(self._pause_topic):
msg = self._sub.get_last_msg(self._pause_topic)
if not msg.data:
self._sub.remove_last_msg(self._pause_topic)
rospy.loginfo("--> Resuming in state %s", self.name)
self._pub.publish(self._feedback_topic,
CommandFeedback(command="resume"))
PriorityContainer.active_container = self._last_active_container
self._last_active_container = None
self._paused = False
super(EventState, self)._notify_skipped()
def _enable_ros_control(self):
super(EventState, self)._enable_ros_control()
self._pub.createPublisher(self._feedback_topic, CommandFeedback)
self._sub.subscribe(self._repeat_topic, Empty)
self._sub.subscribe(self._pause_topic, Bool)
def _disable_ros_control(self):
super(EventState, self)._disable_ros_control()
self._sub.unsubscribe_topic(self._repeat_topic)
self._sub.unsubscribe_topic(self._pause_topic)
self._last_active_container = None
if self._paused:
PriorityContainer.active_container = None
# Events
# (just implement the ones you need)
def on_start(self):
"""
Will be executed once when the behavior starts.
"""
pass
def on_stop(self):
"""
Will be executed once when the behavior stops or is preempted.
"""
pass
def on_pause(self):
"""
Will be executed each time this state is paused.
"""
pass
def on_resume(self, userdata):
"""
Will be executed each time this state is resumed.
"""
pass
def on_enter(self, userdata):
"""
Will be executed each time the state is entered from any other state (but not from itself).
"""
pass
def on_exit(self, userdata):
"""
Will be executed each time the state will be left to any other state (but not to itself).
"""
pass
class PrayingMantisCalibrationSM(Behavior):
'''
A behavior that moves ATLAS into the "praying mantis" pose upon startup in order to get consistent joint encoder offsets for calibration purposes.
'''
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
# [/MANUAL_INIT]
# Behavior comments:
# O 47 211 /Perform_Checks/Manipulate_Limits
# Without this output_key, Check Behavior complains. Because traj_past_limits could in theory be undefined during runtime.
def create(self):
initial_mode = "stand"
motion_mode = "manipulate"
mantis_mode = "manipulate_limits"
percent_past_limits = 0.10 # before: 0.075
# x:788 y:72, x:474 y:133
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.target_limits = 'upper'
_state_machine.userdata.cycle_counter = 1
_state_machine.userdata.stand_posture = None # calculated
_state_machine.userdata.offsets = {'left_arm': dict(), 'right_arm': dict()}
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
self._percent_past_limits = percent_past_limits
# Create STAND posture trajectory
_state_machine.userdata.stand_posture = AtlasFunctions.gen_stand_posture_trajectory()
# [/MANUAL_CREATE]
# x:222 y:281, x:349 y:167
_sm_determine_offsets_0 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['cycle_counter', 'offsets'], output_keys=['offsets'])
with _sm_determine_offsets_0:
# x:61 y:53
OperatableStateMachine.add('Get_Left_Joint_Positions',
CurrentJointPositionsState(planning_group="l_arm_group"),
transitions={'retrieved': 'Determine_Closest_Limits_Left', 'failed': 'failed'},
autonomy={'retrieved': Autonomy.Off, 'failed': Autonomy.Low},
remapping={'joint_positions': 'joint_positions'})
# x:319 y:54
OperatableStateMachine.add('Determine_Closest_Limits_Left',
CalculationState(calculation=self.get_closest_limits_left),
transitions={'done': 'Store_Offsets_Left'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'joint_positions', 'output_value': 'joint_limits'})
# x:598 y:162
OperatableStateMachine.add('Get_Right_Joint_Positions',
CurrentJointPositionsState(planning_group="r_arm_group"),
transitions={'retrieved': 'Determine_Closest_Limits_Right', 'failed': 'failed'},
autonomy={'retrieved': Autonomy.Off, 'failed': Autonomy.Low},
remapping={'joint_positions': 'joint_positions'})
# x:584 y:275
OperatableStateMachine.add('Determine_Closest_Limits_Right',
CalculationState(calculation=self.get_closest_limits_right),
transitions={'done': 'Store_Offsets_Right'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'joint_positions', 'output_value': 'joint_limits'})
# x:608 y:54
OperatableStateMachine.add('Store_Offsets_Left',
FlexibleCalculationState(calculation=self.store_offsets_left, input_keys=['limits', 'value', 'offsets', 'counter']),
transitions={'done': 'Get_Right_Joint_Positions'},
autonomy={'done': Autonomy.Off},
remapping={'limits': 'joint_limits', 'value': 'joint_positions', 'offsets': 'offsets', 'counter': 'cycle_counter', 'output_value': 'offsets'})
# x:340 y:274
OperatableStateMachine.add('Store_Offsets_Right',
FlexibleCalculationState(calculation=self.store_offsets_right, input_keys=['limits', 'value', 'offsets', 'counter']),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={'limits': 'joint_limits', 'value': 'joint_positions', 'offsets': 'offsets', 'counter': 'cycle_counter', 'output_value': 'offsets'})
# x:528 y:401, x:707 y:282
_sm_manipulate_limits_1 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['cycle_counter', 'offsets'], output_keys=['offsets', 'traj_past_limits'])
with _sm_manipulate_limits_1:
# x:100 y:156
OperatableStateMachine.add('Prevent_Runtime_Failure',
CalculationState(calculation=lambda x: dict()),
transitions={'done': 'Go_to_MANIPULATE_LIMITS'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'cycle_counter', 'output_value': 'traj_past_limits'})
# x:387 y:55
OperatableStateMachine.add('Wait_for_Control_Mode_change',
WaitState(wait_time=1.0),
transitions={'done': 'Get_Left_Joint_Positions'},
autonomy={'done': Autonomy.Low})
# x:895 y:279
OperatableStateMachine.add('Gen_Traj_from_90%_to_110%',
CalculationState(calculation=self.gen_traj_past_limits),
transitions={'done': 'Go_to_110%_Joint_Limits'},
autonomy={'done': Autonomy.Low},
remapping={'input_value': 'current_joint_values', 'output_value': 'traj_past_limits'})
# x:893 y:391
OperatableStateMachine.add('Go_to_110%_Joint_Limits',
ExecuteTrajectoryBothArmsState(controllers=['left_arm_traj_controller', 'right_arm_traj_controller']),
transitions={'done': 'Determine_Offsets', 'failed': 'Determine_Offsets'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.High},
remapping={'trajectories': 'traj_past_limits'})
# x:651 y:385
OperatableStateMachine.add('Determine_Offsets',
_sm_determine_offsets_0,
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'cycle_counter': 'cycle_counter', 'offsets': 'offsets'})
# x:648 y:54
OperatableStateMachine.add('Get_Left_Joint_Positions',
CurrentJointPositionsState(planning_group="l_arm_group"),
transitions={'retrieved': 'Get_Right_Joint_Positions', 'failed': 'failed'},
autonomy={'retrieved': Autonomy.Off, 'failed': Autonomy.High},
remapping={'joint_positions': 'joint_positions_left'})
# x:904 y:53
OperatableStateMachine.add('Get_Right_Joint_Positions',
CurrentJointPositionsState(planning_group="r_arm_group"),
transitions={'retrieved': 'Generate_Joint_Positions_Struct', 'failed': 'failed'},
autonomy={'retrieved': Autonomy.Off, 'failed': Autonomy.High},
remapping={'joint_positions': 'joint_positions_right'})
# x:886 y:168
OperatableStateMachine.add('Generate_Joint_Positions_Struct',
FlexibleCalculationState(calculation=lambda ik: {'left_arm': ik[0], 'right_arm': ik[1]}, input_keys=['left', 'right']),
transitions={'done': 'Gen_Traj_from_90%_to_110%'},
autonomy={'done': Autonomy.Off},
remapping={'left': 'joint_positions_left', 'right': 'joint_positions_right', 'output_value': 'current_joint_values'})
# x:92 y:55
OperatableStateMachine.add('Go_to_MANIPULATE_LIMITS',
ChangeControlModeActionState(target_mode=mantis_mode),
transitions={'changed': 'Wait_for_Control_Mode_change', 'failed': 'failed'},
autonomy={'changed': Autonomy.Off, 'failed': Autonomy.High})
# x:574 y:247, x:276 y:549
_sm_update_calibration_2 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['offsets'])
with _sm_update_calibration_2:
# x:46 y:44
OperatableStateMachine.add('Process_Offsets',
CalculationState(calculation=self.process_offsets),
transitions={'done': 'Print_Offset_Info'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'offsets', 'output_value': 'offsets'})
# x:227 y:45
OperatableStateMachine.add('Print_Offset_Info',
CalculationState(calculation=self.print_offset_info),
transitions={'done': 'Publish_Offsets'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'offsets', 'output_value': 'none'})
# x:390 y:158
OperatableStateMachine.add('Ask_Perform_Update',
OperatorDecisionState(outcomes=['update', 'no_update'], hint="Do you want to apply the calculated offsets for calibration?", suggestion=None),
transitions={'update': 'Convert_Offset_Data', 'no_update': 'finished'},
autonomy={'update': Autonomy.Full, 'no_update': Autonomy.Full})
# x:232 y:337
OperatableStateMachine.add('Update_Calibration',
UpdateJointCalibrationState(joint_names=self._joint_names['left_arm'][0:4] + self._joint_names['right_arm'][0:4]),
transitions={'updated': 'Calibration_Successful', 'failed': 'Calibration_Failed'},
autonomy={'updated': Autonomy.Low, 'failed': Autonomy.High},
remapping={'joint_offsets': 'offset_list'})
# x:241 y:242
OperatableStateMachine.add('Convert_Offset_Data',
CalculationState(calculation=lambda o: o['left_arm']['avg'] + o['right_arm']['avg']),
transitions={'done': 'Update_Calibration'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'offsets', 'output_value': 'offset_list'})
# x:522 y:337
OperatableStateMachine.add('Calibration_Successful',
LogState(text="Successfully updated calibration offsets.", severity=Logger.REPORT_INFO),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:246 y:445
OperatableStateMachine.add('Calibration_Failed',
LogState(text="Failed to apply calibration offsets!", severity=Logger.REPORT_ERROR),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:399 y:44
OperatableStateMachine.add('Publish_Offsets',
CalculationState(calculation=self.publish_offsets),
transitions={'done': 'Ask_Perform_Update'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'offsets', 'output_value': 'none'})
# x:978 y:197, x:394 y:80
_sm_perform_checks_3 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['cycle_counter', 'target_limits', 'offsets'], output_keys=['cycle_counter', 'offsets'])
with _sm_perform_checks_3:
# x:105 y:74
OperatableStateMachine.add('Go_to_Intermediate_Mode',
ChangeControlModeActionState(target_mode=motion_mode),
transitions={'changed': 'Gen_Traj_to_90%_Limits', 'failed': 'failed'},
autonomy={'changed': Autonomy.Off, 'failed': Autonomy.High})
# x:653 y:274
OperatableStateMachine.add('Manipulate_Limits',
_sm_manipulate_limits_1,
transitions={'finished': 'Gen_Traj_back_to_90%_Limits', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'cycle_counter': 'cycle_counter', 'offsets': 'offsets', 'traj_past_limits': 'traj_past_limits'})
# x:903 y:78
OperatableStateMachine.add('Increment_Cycle_counter',
CalculationState(calculation=lambda counter: counter + 1),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'cycle_counter', 'output_value': 'cycle_counter'})
# x:344 y:277
OperatableStateMachine.add('Move_to_90%_Joint_Limits',
MoveitStartingPointState(vel_scaling=0.3),
transitions={'reached': 'Manipulate_Limits', 'failed': 'Move_to_90%_Joint_Limits'},
autonomy={'reached': Autonomy.Low, 'failed': Autonomy.Full},
remapping={'trajectories': 'trajectories_90'})
# x:114 y:276
OperatableStateMachine.add('Gen_Traj_to_90%_Limits',
CalculationState(calculation=self.gen_traj_pre_limits),
transitions={'done': 'Move_to_90%_Joint_Limits'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'target_limits', 'output_value': 'trajectories_90'})
# x:636 y:78
OperatableStateMachine.add('Go_back_to_90%_Joint_Limits',
ExecuteTrajectoryBothArmsState(controllers=['left_arm_traj_controller', 'right_arm_traj_controller']),
transitions={'done': 'Increment_Cycle_counter', 'failed': 'failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.High},
remapping={'trajectories': 'traj_back_to_90'})
# x:636 y:172
OperatableStateMachine.add('Gen_Traj_back_to_90%_Limits',
FlexibleCalculationState(calculation=self.gen_traj_back_from_limits, input_keys=['trajectories_90', 'traj_past_limits']),
transitions={'done': 'Go_back_to_90%_Joint_Limits'},
autonomy={'done': Autonomy.Off},
remapping={'trajectories_90': 'trajectories_90', 'traj_past_limits': 'traj_past_limits', 'output_value': 'traj_back_to_90'})
with _state_machine:
# x:110 y:52
OperatableStateMachine.add('Initial_Control_Mode',
ChangeControlModeActionState(target_mode=initial_mode),
transitions={'changed': 'Perform_Checks', 'failed': 'failed'},
autonomy={'changed': Autonomy.High, 'failed': Autonomy.High})
# x:712 y:317
OperatableStateMachine.add('Initial_Mode_before_exit',
ChangeControlModeActionState(target_mode=initial_mode),
transitions={'changed': 'Update_Calibration', 'failed': 'failed'},
autonomy={'changed': Autonomy.Off, 'failed': Autonomy.High})
# x:122 y:302
OperatableStateMachine.add('Perform_Checks',
_sm_perform_checks_3,
transitions={'finished': 'Are_We_Done_Yet?', 'failed': 'Intermediate_Mode_before_exit'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'cycle_counter': 'cycle_counter', 'target_limits': 'target_limits', 'offsets': 'offsets'})
# x:126 y:505
OperatableStateMachine.add('Are_We_Done_Yet?',
DecisionState(outcomes=["done", "more"], conditions=lambda counter: "done" if counter >= 2 else "more"),
transitions={'done': 'Intermediate_Mode_before_exit', 'more': 'Setup_next_Cycle'},
autonomy={'done': Autonomy.Low, 'more': Autonomy.High},
remapping={'input_value': 'cycle_counter'})
# x:15 y:404
OperatableStateMachine.add('Setup_next_Cycle',
CalculationState(calculation=lambda lim: 'lower' if lim == 'upper' else 'upper'),
transitions={'done': 'Perform_Checks'},
autonomy={'done': Autonomy.Low},
remapping={'input_value': 'target_limits', 'output_value': 'target_limits'})
# x:725 y:186
OperatableStateMachine.add('Update_Calibration',
_sm_update_calibration_2,
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'offsets': 'offsets'})
# x:726 y:427
OperatableStateMachine.add('Move_to_Stand_Posture',
MoveitStartingPointState(vel_scaling=0.3),
transitions={'reached': 'Initial_Mode_before_exit', 'failed': 'Move_to_Stand_Posture'},
autonomy={'reached': Autonomy.Off, 'failed': Autonomy.Full},
remapping={'trajectories': 'stand_posture'})
# x:412 y:427
OperatableStateMachine.add('Intermediate_Mode_before_exit',
ChangeControlModeActionState(target_mode=motion_mode),
transitions={'changed': 'Move_to_Stand_Posture', 'failed': 'failed'},
autonomy={'changed': Autonomy.Off, 'failed': Autonomy.High})
return _state_machine
# Private functions can be added inside the following tags
# [MANUAL_FUNC]
def gen_traj_pre_limits(self, limits_side):
"""Create trajectories for going to 90 percent of joint limits (either upper or lower limits)"""
joint_config = {'left_arm': self._joint_calib['left_arm'][limits_side],
'right_arm': self._joint_calib['right_arm'][limits_side]
}
return AtlasFunctions.gen_arm_trajectory_from_joint_configuration(joint_config)
def _get_closest_limits(self, side, current_values):
"""
Selects the closest limit with respect to the current value (upper or lower bound).
"""
limits = self._joint_limits[side]
closest_limit = list()
for i in range(len(current_values)):
near_limit = 'upper' if abs(limits['upper'][i] - current_values[i]) < abs(limits['lower'][i] - current_values[i]) else 'lower'
closest_limit.append(limits[near_limit][i])
rospy.loginfo("Limit joint positions: %s" % str(closest_limit))
rospy.loginfo("Current joint positions: %s" % str(current_values))
return closest_limit
def get_closest_limits_left(self, current_values):
return self._get_closest_limits('left_arm', current_values)
def get_closest_limits_right(self, current_values):
return self._get_closest_limits('right_arm', current_values)
def gen_traj_past_limits(self, current_joint_values):
"""
Given all joint limits, generate a trajectory that takes the joints to 110%% percent past limits.
atlas_v5 update: Do not push the lower 3 joints (electric ones) path the limits.
"""
result = dict()
for arm in ['left_arm', 'right_arm']:
current_values = current_joint_values[arm]
arm_limits = self._get_closest_limits(arm, current_values)
arm_target = list()
arm_effort = list()
percentage = self._percent_past_limits
# Push the upper 4 joints against the limits
for i in range(0,4):
near_limit = 'upper' if self._joint_limits[arm]['upper'][i] == arm_limits[i] else 'lower'
limit_range = self._joint_limits[arm]['upper'][i] - self._joint_limits[arm]['lower'][i]
offset_sign = 1 if near_limit is 'upper' else -1
arm_target.append(arm_limits[i] + offset_sign * percentage * limit_range)
arm_effort.append(float(offset_sign))
# "Ignore" the lower 3 joints (electric motor ones)
for i in range(4,7):
arm_target.append(current_values[i])
arm_effort.append(0.0) # Zero effort stands for not applying additional force
trajectory = JointTrajectory()
trajectory.joint_names = self._joint_names[arm]
point = JointTrajectoryPoint()
point.positions = arm_target
point.velocities = [0.0] * len(arm_target) # David's controller expects zero velocities
point.effort = arm_effort
point.time_from_start = rospy.Duration.from_sec(2.5)
trajectory.points.append(point)
# rospy.loginfo("110%% joint positions for %s arm: %s" % (arm, str(arm_target[0:4]))) # Only report the relevant joints
result[arm] = trajectory
return result
def gen_traj_back_from_limits(self, input_keys):
"""The resulting trajectory points are the same as for going to 90%% of limits, but with the efforts set for David's controllers."""
traj_pre_limits = input_keys[0]
traj_past_limits = input_keys[1]
traj_back_to_90 = dict()
for arm in ['left_arm', 'right_arm']:
trajectory = traj_pre_limits[arm] # Start with 90% of joint limits as the trajectory points
trajectory.points[0].effort = traj_past_limits[arm].points[0].effort # Set efforts as per David's controllers
trajectory.points[0].time_from_start = rospy.Duration.from_sec(1.0)
# David's controller expects zero velocities
trajectory.points[0].velocities = [0.0] * len(trajectory.points[0].positions)
traj_back_to_90[arm] = trajectory
return traj_back_to_90
def store_offsets(self, side, input_keys):
limits = input_keys[0][0:4] # Ignore the lower 3 joints
values = input_keys[1][0:4] # --//-- --//--
offsets = input_keys[2]
counter = input_keys[3]
offsets[side][counter] = [limit - value for limit, value in zip(limits, values)]
msg = JointTrajectory()
msg.joint_names = self._joint_names[side][0:4] # Ignore the lower 3 joints
point = JointTrajectoryPoint()
point.positions = values
point.velocities = offsets[side][counter]
msg.points.append(point)
self._pub.publish(self._offset_topic, msg)
Logger.loginfo("Publishing %s arm offsets to %s" % (side, self._offset_topic))
return offsets
def publish_offsets(self, offsets, arms = ['left_arm', 'right_arm'], current_values = []):
for side in arms:
msg = JointTrajectory()
msg.joint_names = self._joint_names[side]
point = JointTrajectoryPoint()
point.positions = current_values
point.velocities = offsets[side]['avg']
msg.points.append(point)
self._pub.publish(self._offset_topic, msg)
Logger.loginfo("Publishing %s arm offsets to %s" % (side, self._offset_topic))
def store_offsets_left(self, input_keys):
return self.store_offsets('left_arm', input_keys)
def store_offsets_right(self, input_keys):
return self.store_offsets('right_arm', input_keys)
def process_offsets(self, offsets):
for side in ['left_arm', 'right_arm']:
# transposes list of lists from iteration,joint to joint,iteration
iteration_values = map(list, zip(*offsets[side].values()))
# Calculate the average offset and the deviation from the average
offsets[side]['avg'] = [sum(joint_entries)/float(len(joint_entries)) for joint_entries in iteration_values]
offsets[side]['diff'] = [max(map(lambda x: abs(x-avg),joint_entries)) for joint_entries,avg in zip(iteration_values, offsets[side]['avg'])]
return offsets
def print_offset_info(self, offsets):
sides = ['left_arm', 'right_arm']
for side in sides:
Logger.loginfo("Joint order (%s): %s" % (side, str(self._joint_names[side][0:4])))
rounded_offsets = [round(offset, 3) for offset in offsets[side]['avg']] # round due to comms_bridge
Logger.loginfo("Offsets (%s): %s" % (side, str(rounded_offsets)))
# Logger.loginfo("Max deviation from average (%s): %s" % (side, str(offsets[side]['diff'])))
pprint.pprint(offsets) # Pretty print to the "onboard" terminal
class JoystickJointControl(EventState):
'''
Control joint position with joystick.
If button is pressed joint is moving with defined in configuration speed.
For axes speed is modulated with axis value. If speed is equal to zero,
then position of joint is defined directly by axis position.
Note that one button may control multiple joints.
-- joints_topic string Topic with current robot pose (JointState)
-- goal_joints_topic string Topic to publish desired joint positions.
-- joy_topic string Joystick
-- buttons object Array with tuples: (button number, speed, joint name, min, max).
-- axes object Array with tuples: (axis number, speed, joint name, min, max).
-- timeout number Exit state if no joystick input is provided for timeout. If timeout is negative waits forever.
<= timeout No joystick activity for given timeout.
'''
def __init__(self, joints_topic='joint_states', goal_joints_topic='goal_joint_states', joy_topic = 'joy', buttons = [], axes = [], timeout = 5.0):
super(JoystickJointControl, self).__init__(outcomes = ['done'])
# store state parameter for later use.
self._joints_topic = joints_topic
self._goal_topic = goal_joints_topic
self._joy_topic = joy_topic
self._timeout = timeout
# Event (button or axis) description type.
# EventDescription = namedtuple('EventDescription', ['ev_num', 'speed', 'joint', 'min', 'max'])
class EventDescription():
def __init__(self, args):
# check arg type
if not isinstance(args, (list,tuple)) or len(args) != 5:
raise ValueError('Button/axis event description must be a tuple with 5 elements.')
# parse arg in fields
( self.ev_num, self.speed, self.joint, self.min, self.max ) = args
# check fields types
if not isinstance(self.ev_num, int):
raise ValueError('Button/axis number must be integer.')
if not isinstance(self.speed, (int, float)):
raise ValueError('Speself must be float.')
if not isinstance(self.max, (int, float)) or not isinstance(self.min, (int,float)):
raise ValueError('Min/max position must be float.')
if not isinstance(self.joint, str):
raise ValueError('Joint name must be string.')
# parse buttons and axes
self._buttons = [ EventDescription(button_conf) for button_conf in buttons ]
self._axes = [ EventDescription(axis_conf) for axis_conf in axes ]
# subscribe to topics
self._joy_subscriber = ProxySubscriberCached({ self._joy_topic: Joy })
self._joints_subscriber = ProxySubscriberCached({ self._joints_topic: JointState })
# setup publiser proxies
self._goal_publisher = ProxyPublisher({ self._goal_topic: JointState })
# buffers
self._timestamp = None
self._activity_timestamp = None
self._goal_position = {} # map { joint: position } for all controlled joints
def on_enter(self, userdata):
# reset timestamp
self._timestamp = rospy.Time.now()
self._activity_timestamp = self._timestamp
# fill joint goal array
self._goal_position = {}
# get current joints positions
joints_msg = self._joints_subscriber.get_last_msg(self._joints_topic)
# check message
if joints_msg == None or len(joints_msg.name) != len (joints_msg.position):
Logger.logerr('JoystickJointControl: robot pose is unavailable or incorrect on topic `%s`' % self._joints_topic)
raise RuntimeError('JoystickJointControl: robot pose is unavailable or incorrect on topic `%s`' % self._joints_topic)
for event in self._buttons + self._axes:
# find coresponding joint
is_found = False
for i in range(0,len(joints_msg.name)):
if joints_msg.name[i] == event.joint:
is_found = True
self._goal_position[event.joint] = clip(joints_msg.position[i], event.min, event.max)
# check if joint is found
if not is_found:
Logger.logerr('JoystickJointControl: unable to determine `%s` joint position. (min + max)/2 is used.' % event.joint)
self._goal_position[event.joint] = (event.min + event.max) / 2.0 # default value
Logger.loginfo('JoystickJointControl: start monitoring.')
def execute(self, userdata):
# calculate time shift
timestamp = rospy.Time.now()
dt = (timestamp - self._timestamp).to_sec()
self._timestamp = timestamp
activity_detected = False
# check if new message is available
if self._joy_subscriber.has_msg(self._joy_topic):
# get messages
joy_msg = self._joy_subscriber.get_last_msg(self._joy_topic)
# TODO skip non-used axes and buttons
# check if any button is pressed
for i in range(0, len(joy_msg.buttons)):
# skip not pressed buttons
if not joy_msg.buttons[i]:
continue
# find event handlers
for event in self._buttons:
if event.ev_num == i:
# modify position accordingly
self._goal_position[event.joint] = clip(self._goal_position[event.joint] + event.speed*dt, event.min, event.max)
activity_detected = True
# check if any axis is non-zero
for i in range(0, len(joy_msg.axes)):
# find event handlers
for event in self._axes:
if event.ev_num == i:
# modify position accordingly
if event.speed != 0.0:
# add shift
self._goal_position[event.joint] = clip(self._goal_position[event.joint] + event.speed*joy_msg.axes[i]*dt, event.min, event.max)
else:
# set position
self._goal_position[event.joint] = 0.5*((event.max - event.min)*joy_msg.axes[i] + event.max + event.min)
if joy_msg.axes[i] != 0.0:
activity_detected = True
# publish pose
goal_msg = JointState()
goal_msg.header.stamp = timestamp
for joint, position in self._goal_position.items():
goal_msg.name.append(joint)
goal_msg.position.append(position)
self._goal_publisher.publish(self._goal_topic, goal_msg)
# check activity
if activity_detected:
self._activity_timestamp = timestamp
return None
# check activity timeout
if (timestamp - self._activity_timestamp).to_sec() > self._timeout:
return 'done'
else:
return None
def on_exit(self, userdata):
Logger.loginfo('JoystickJointControl: stop monitoring.')
class OperatableStateMachine(PreemptableStateMachine):
"""
A state machine that can be operated.
It synchronizes its current state with the mirror and supports some control mechanisms.
"""
autonomy_level = 3
silent_mode = False
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()
@staticmethod
def add(label, state, transitions = None, autonomy = None, remapping = None):
"""
Add a state to the opened state machine.
@type label: string
@param label: The label of the state being added.
@param state: An instance of a class implementing the L{State} interface.
@param transitions: A dictionary mapping state outcomes to other state
labels or container outcomes.
@param autonomy: A dictionary mapping state outcomes to their required
autonomy level
@param remapping: A dictionary mapping local userdata keys to userdata
keys in the container.
"""
self = StateMachine._currently_opened_container()
# add loopback transition to loopback states
if isinstance(state, LoopbackState):
transitions[LoopbackState._loopback_name] = label
autonomy[LoopbackState._loopback_name] = -1
self._ordered_states.append(state)
state.name = label
state.transitions = transitions
state.autonomy = autonomy
state._parent = self
StateMachine.add(label, state, transitions, remapping)
self._autonomy[label] = autonomy
def replace(self, new_state):
old_state = self._states[new_state.name]
new_state.transitions = old_state.transitions
new_state.autonomy = old_state.autonomy
new_state._parent = old_state._parent
self._ordered_states[self._ordered_states.index(old_state)] = new_state
self._states[new_state.name] = new_state
def destroy(self):
self._notify_stop()
self._disable_ros_control()
self._sub.unsubscribe_topic('/flexbe/command/autonomy')
self._sub.unsubscribe_topic('/flexbe/command/sync')
self._sub.unsubscribe_topic('/flexbe/request_mirror_structure')
StateLogger.shutdown()
def confirm(self, name, id):
"""
Confirms the state machine and triggers the creation of the structural message.
It is mandatory to call this function at the top-level state machine
between building it and starting its execution.
@type name: string
@param name: The name of this state machine to identify it.
"""
self.name = name
self.id = id
self._pub.createPublisher('/flexbe/mirror/sync', BehaviorSync, _latch = True) # Update mirror with currently active state (high bandwidth mode)
self._pub.createPublisher('/flexbe/mirror/preempt', Empty, _latch = True) # Preempts the mirror
self._pub.createPublisher('/flexbe/mirror/structure', ContainerStructure) # Sends the current structure to the mirror
self._pub.createPublisher('/flexbe/log', BehaviorLog) # Topic for logs to the GUI
self._pub.createPublisher('/flexbe/command_feedback', CommandFeedback) # Gives feedback about executed commands to the GUI
self._sub.subscribe('/flexbe/command/autonomy', UInt8, self._set_autonomy_level)
self._sub.subscribe('/flexbe/command/sync', Empty, self._sync_callback)
self._sub.subscribe('/flexbe/request_mirror_structure', Int32, self._mirror_structure_callback)
StateLogger.initialize(name)
if OperatableStateMachine.autonomy_level != 255:
self._enable_ros_control()
rospy.sleep(0.5) # no clean way to wait for publisher to be ready...
self._notify_start()
def _set_autonomy_level(self, msg):
""" Sets the current autonomy level. """
if OperatableStateMachine.autonomy_level != msg.data:
rospy.loginfo('--> Autonomy changed to %d', msg.data)
if msg.data < 0:
self.preempt()
else:
OperatableStateMachine.autonomy_level = msg.data
self._pub.publish('/flexbe/command_feedback', CommandFeedback(command="autonomy", args=[]))
def _sync_callback(self, msg):
rospy.loginfo("--> Synchronization requested...")
msg = BehaviorSync()
msg.behavior_id = self.id
msg.current_state_checksum = zlib.adler32(self._get_deep_state()._get_path())
self._pub.publish('/flexbe/mirror/sync', msg)
self._pub.publish('/flexbe/command_feedback', CommandFeedback(command="sync", args=[]))
rospy.loginfo("<-- Sent synchronization message for mirror.")
def _mirror_structure_callback(self, msg):
rospy.loginfo("--> Creating behavior structure for mirror...")
msg = self._build_msg('')
msg.behavior_id = self.id
self._pub.publish('/flexbe/mirror/structure', msg)
rospy.loginfo("<-- Sent behavior structure for mirror.")
def _transition_allowed(self, label, outcome):
return self._autonomy[label][outcome] < OperatableStateMachine.autonomy_level
def _build_msg(self, prefix, msg = None):
"""
Adds this state machine to the initial structure message.
@type prefix: string
@param prefix: A path consisting of the container hierarchy containing this state.
@type msg: ContainerStructure
@param msg: The message that will finally contain the structure message.
"""
# set children
children = []
for state in self._ordered_states:
children.append(str(state.name))
# set name
name = prefix + self.name
if msg is None:
# top-level state machine (has no transitions)
self._message = ContainerStructure()
outcomes = list(self._outcomes)
transitions = None
autonomy = None
else:
# lower-level state machine
self._message = msg
outcomes = list(self.transitions)
# set transitions and autonomy
transitions = []
autonomy = []
for i in range(len(self.transitions)):
outcome = outcomes[i]
if outcome == 'preempted': # set preempt transition
transitions.append('preempted')
autonomy.append(-1)
else:
transitions.append(str(self.transitions[outcome]))
autonomy.append(self.autonomy[outcome])
# add to message
self._message.containers.append(Container(name, children, outcomes, transitions, autonomy))
# build message for children
for state in self._ordered_states:
state._build_msg(name+'/', self._message)
# top-level state machine returns the message
if msg is None:
return self._message
def _notify_start(self):
for state in self._ordered_states:
if isinstance(state, LoopbackState):
state.on_start()
if isinstance(state, OperatableStateMachine):
state._notify_start()
def _enable_ros_control(self):
for state in self._ordered_states:
if isinstance(state, LoopbackState):
state._enable_ros_control()
if isinstance(state, OperatableStateMachine):
state._enable_ros_control()
def _notify_stop(self):
for state in self._ordered_states:
if isinstance(state, LoopbackState):
state.on_stop()
state._disable_ros_control()
if isinstance(state, OperatableStateMachine):
state._notify_stop()
def _disable_ros_control(self):
for state in self._ordered_states:
if isinstance(state, LoopbackState):
state._disable_ros_control()
if isinstance(state, OperatableStateMachine):
state._disable_ros_control()
class CreateTimedStopState(EventState):
'''
This state publishes a constant zero TwistStamped command based on parameters. The state monitors the iRobot Create odometry and
returns a failed outcome if speed is not near zero within the timeout
-- timeout float Time which needs to have passed since the behavior started. (default: 0.2)
-- odom_topic string topic of the iRobot Create sensor state (default: '/create_node/odom')
-- cmd_topic string topic name of the robot command (default: '/create_node/cmd_vel')
<= done Given time has passed.
<= failed A bumper was activated prior to completion
'''
def __init__(self,
timeout=0.2,
cmd_topic='/create_node/cmd_vel',
odom_topic='/create_node/odom'):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(CreateTimedStopState, self).__init__(outcomes=['done', 'failed'])
# Store state parameter for later use.
self._timeout = rospy.Duration(timeout)
self._twist = TwistStamped()
# 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._start_time = None
self._done = None # Track the outcome so we can detect if transition is blocked
self._odom_topic = odom_topic
self._cmd_topic = cmd_topic
self._odom_sub = ProxySubscriberCached({self._odom_topic: Odometry})
self._pub = ProxyPublisher({self._cmd_topic: TwistStamped})
def execute(self, userdata):
# This method is called periodically while the state is active.
# If no outcome is returned, the state will stay active.
if (rospy.Time.now() - self._start_time) > self._timeout:
# Normal completion, verify stoppage
if (self._sub.has_msg(self._odom_topic)):
odom = self._sub.get_last_msg(self._odom_topic)
speed = odom.twist.twist.linear.x * odom.twist.twist.linear.x + odom.twist.twist.angular.z * odom.twist.twist.angular.z
if (speed > 5.0e-4):
Logger.logwarn(
'Stop failed twist linear = %f,%f,%f angular=%f, %f, %f'
%
(odom.twist.twist.linear.x, odom.twist.twist.linear.y,
odom.twist.twist.linear.z, odom.twist.twist.angular.x,
odom.twist.twist.angular.y,
odom.twist.twist.angular.z))
self._done = 'failed'
return 'failed'
self._done = 'done'
return 'done'
# Normal operation publish the zero twist
self._twist.header.stamp = rospy.Time.now()
self._pub.publish(self._cmd_topic, self._twist)
return None
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.
self._start_time = rospy.Time.now()
self._done = None # reset the completion flag
class KylePubInputState(EventState):
'''
This state publishes an open loop constant TwistStamped command based on parameters.
-- target_time float Time which needs to have passed since the behavior started.
-- velocity float Body velocity (m/s)
-- rotation_rate float Angular rotation (radians/s)
-- cmd_topic string topic name of the robot velocity command (default: 'cmd_vel')
<= done Given time has passed.
'''
def __init__(self, cmd_topic='cmd_vel', increaseBy = 1):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(KylePubInputState, self).__init__(outcomes = ['done'],
input_keys=['valueToIncrease'])
# Store state parameter for later use.
#self._target_time = rospy.Duration(target_time
#self._twist.twist.linear.x = velocity
#self._twist.twist.angular.z = rotation_rate
# 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._start_time = None
self._count = Int32()
self._increaseBy = Int32()
self._increaseBy.data = increaseBy
self._done = None # Track the outcome so we can detect if transition is blocked
self._cmd_topic = cmd_topic
self._pub = ProxyPublisher( {self._cmd_topic: Int32})
def execute(self, userdata):
# This method is called periodically while the state is active.
# If no outcome is returned, the state will stay active.
# if (self._done):
# We have completed the state, and therefore must be blocked by autonomy level
# Stop the robot, but and return the prior outcome
# ts = TwistStamped() # Zero twist to stop if blocked
# ts.header.stamp = rospy.Time.now()
# self._pub.publish(self._cmd_topic, ts)
# return self._done
# if rospy.Time.now() - self._start_time > self._target_time:
# Normal completion, do not bother repeating the publish
# self._done = 'done'
# return 'done'
self._count.data = userdata.valueToIncrease.data
self._count.data = self._count.data
self._count.data = self._count.data + self._increaseBy.data
# Normal operation
#self._twist.twist.linear.x = 1.0
#self._twist.twist.angular.z = 1.0
self._pub.publish(self._cmd_topic, self._count)
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.
self._start_time = rospy.Time.now()
self._done = None # reset the completion flag
class PurePursuitState(EventState):
"""
This state calculates the twist required to follow a constant curvature arc to the pure pursuit intersection point.
The command is published as a TwistStamped command based on parameters.
If arc motion is used, the arc should be less than or equal to pi/2 radians. Use multiple targets for longer arcs.
-- desired_velocity float Desired velocity in m/s (default: 0.2)
-- max_rotation_rate float Maximum rotation rate radians/s (default: 10.0)
-- target_frame: string Coordinate frame of target point (default: 'map')
-- target_x: float Target point x-coordinate (m)
-- target_y: float Target point y-coordinate (m)
-- target_type: string Desired motion ('line','arc') (default: 'line')
-- lookahead_distance: float Lookahead distance (m) (default: 0.25)
-- timeout float Transform timeout (seconds) (default: 0.08)
-- recover_mode bool Recover path (typically on startup) (default: False)
-- center_x: float Center point x-coordinate for circle defining arc motion (default: 0.0)
-- center_y: float Center point y-coordinate for circle defining arc motion (default: 0.0)
-- cmd_topic string topic name of the robot command (default: '/create_node/cmd_vel')
-- odometry_topic: string topic of the iRobot Create sensor state (default: '/create_node/odom'
-- marker_topic: string topic of the RViz marker used for visualization (default: '/pure_pursuit_marker')
-- marker_size: float Size of RViz marker used for target (default: 0.05)
># indice int The index
># path Path The path
#> indice int The index + 1
#> plan Path The path
<= done Reached the end of target relevance
<= continue Continue following the path
<= failed A bumper was activated prior to completion
"""
def __init__(self,
desired_velocity=0.2,
max_rotation_rate=10.0,
target_frame='map',
target_x=1.0,
target_y=0.1,
target_type='line',
lookahead_distance=0.25,
timeout=0.08,
recover_mode=False,
center_x=0.0,
center_y=0.0,
cmd_topic='cmd_vel',
odometry_topic='odom',
marker_topic='pure_pursuit_marker',
marker_size=0.05):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(PurePursuitState,
self).__init__(outcomes=['done', 'continue', 'failed'],
input_keys=['indice', 'plan'],
output_keys=['indice', 'plan'])
# Store state parameter for later use.
self._twist = TwistStamped()
self._twist.twist.linear.x = desired_velocity
self._twist.twist.angular.z = 0.0
self._desired_velocity = desired_velocity
self._max_rotation_rate = max_rotation_rate # Used to clamp the rotation calculations
self._current_position = PointStamped()
self._current_position.header.stamp = rospy.Time.now()
self._current_position.header.frame_id = target_frame
self._target = PointStamped()
self._target.header.stamp = rospy.Time.now()
self._target.header.frame_id = target_frame
self._target.point = Point(target_x, target_y, 0.0)
self._prior = PointStamped()
self._prior.header.stamp = rospy.Time.now()
self._prior.header.frame_id = target_frame
self._lookahead = lookahead_distance
self._recover_mode = recover_mode
self._target_type = target_type
self._target_frame = target_frame
if (self._target_type == 'arc'):
self._radius = np.sqrt((center_x - target_x)**2 +
(center_y - target_y)**2)
Logger.loginfo(
'Using arc type with center=(%d, %d) target=(%d,%d) radius=%d'
% (self._center.point.x, self._center.point.y,
self._target.point.x, self._target.point.y, self._radius))
self._odom_frame = 'odom' # Update with the first odometry message
self._robot_frame = 'base_footprint'
self._failed = False
self._timeout = rospy.Duration(timeout) # transform timeout
# 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._start_time = None
self._return = None # Track the outcome so we can detect if transition is blocked
self._odom_topic = odometry_topic
self._marker_topic = marker_topic
self._cmd_topic = cmd_topic
self._listener = ProxyTransformListener()
self._odom_sub = ProxySubscriberCached({self._odom_topic: Odometry})
self._pub = ProxyPublisher({self._cmd_topic: TwistStamped})
if (self._marker_topic != ""):
self._marker_pub = ProxyPublisher({self._marker_topic: Marker})
else:
self._marker_pub = None
self._marker = Marker()
self._marker.header.frame_id = self._target_frame
self._marker.header.stamp = rospy.Time.now()
self._marker.ns = "pure_pursuit_waypoints"
self._marker.id = int(target_x * 1000000) + int(target_y * 1000)
self._marker.type = Marker.SPHERE
self._marker.action = Marker.ADD
self._marker.pose.position.x = target_x
self._marker.pose.position.y = target_y
self._marker.pose.position.z = 0.0
self._marker.pose.orientation.x = 0.0
self._marker.pose.orientation.y = 0.0
self._marker.pose.orientation.z = 0.0
self._marker.pose.orientation.w = 1.0
self._marker.scale.x = marker_size
self._marker.scale.y = marker_size
self._marker.scale.z = marker_size
self._marker.color.a = 1.0 # Don't forget to set the alpha!
self._marker.color.r = 0.0
self._marker.color.g = 0.0
self._marker.color.b = 1.0
self._reference_marker = Marker()
self._reference_marker.header.frame_id = self._target_frame
self._reference_marker.header.stamp = rospy.Time.now()
self._reference_marker.ns = "pure_pursuit_reference"
self._reference_marker.id = 1
self._reference_marker.type = Marker.SPHERE
self._reference_marker.action = Marker.ADD
self._reference_marker.pose.position.x = target_x
self._reference_marker.pose.position.y = target_y
self._reference_marker.pose.position.z = 0.0
self._reference_marker.pose.orientation.x = 0.0
self._reference_marker.pose.orientation.y = 0.0
self._reference_marker.pose.orientation.z = 0.0
self._reference_marker.pose.orientation.w = 1.0
self._reference_marker.scale.x = marker_size * 0.75
self._reference_marker.scale.y = marker_size * 0.75
self._reference_marker.scale.z = marker_size * 0.75
self._reference_marker.color.a = 0.0 # Add, but make invisible at first
self._reference_marker.color.r = 1.0
self._reference_marker.color.g = 0.0
self._reference_marker.color.b = 1.0
self._local_target_marker = Marker()
self._local_target_marker.header.frame_id = self._target_frame
self._local_target_marker.header.stamp = rospy.Time.now()
self._local_target_marker.ns = "pure_pursuit_target"
self._local_target_marker.id = 1
self._local_target_marker.type = Marker.SPHERE
self._local_target_marker.action = Marker.ADD
self._local_target_marker.pose.position.x = target_x
self._local_target_marker.pose.position.y = target_y
self._local_target_marker.pose.position.z = 0.0
self._local_target_marker.pose.orientation.x = 0.0
self._local_target_marker.pose.orientation.y = 0.0
self._local_target_marker.pose.orientation.z = 0.0
self._local_target_marker.pose.orientation.w = 1.0
self._local_target_marker.scale.x = marker_size
self._local_target_marker.scale.y = marker_size
self._local_target_marker.scale.z = marker_size
self._local_target_marker.color.a = 0.0 # Add, but make invisible at first
self._local_target_marker.color.r = 1.0
self._local_target_marker.color.g = 0.0
self._local_target_marker.color.b = 1.0
# Transform point into odometry frame
def transformOdom(self, point):
try:
# Get transform
self._listener.listener().waitForTransform(self._odom_frame,
point.header.frame_id,
point.header.stamp,
self._timeout)
return self._listener.listener().transformPoint(
self._odom_frame, point)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
Logger.logwarn('Failed to get the transformation\n%s' % str(e))
self._failed = True
return None
except Exception as e:
Logger.logwarn(
'Failed to get the transformation due to unknown error\n %s' %
str(e))
self._failed = True
return None
# Transform point into map frame
def transformMap(self, odometry):
odom_position = PointStamped()
odom_position.header = odometry.header
odom_position.point = odometry.pose.pose.position
try:
# Get transform
self._listener.listener().waitForTransform(
self._target_frame, odometry.header.frame_id,
odometry.header.stamp, self._timeout)
return self._listener.listener().transformPoint(
self._target_frame, odom_position)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
Logger.logwarn(
'Failed to get the transformation to target_frame\n%s' %
str(e))
self._failed = True
return None
except Exception as e:
Logger.logwarn(
'Failed to get the transformation to target frame due to unknown error\n %s'
% str(e))
self._failed = True
return None
# Transform point into robot body frame
def transformBody(self, point):
try:
# Get transform
self._listener.listener().waitForTransform(self._robot_frame,
point.header.frame_id,
point.header.stamp,
self._timeout)
return self._listener.listener().transformPoint(
self._robot_frame, point)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
Logger.logwarn('Failed to get the transformation\n%s' % str(e))
self._failed = True
return None
except:
Logger.logwarn(
'Failed to get the transformation due to unknown error\n')
self._failed = True
return None
def execute(self, userdata):
# This method is called periodically while the state is active.
# If no outcome is returned, the state will stay active.
if (self._return):
# We have completed the state, and therefore must be blocked by autonomy level
# Stop the robot, but and return the prior outcome
ts = TwistStamped()
ts.header.stamp = rospy.Time.now()
self._pub.publish(self._cmd_topic, ts)
return self._return
# Get the latest odometry data
if (self._sub.has_msg(self._odom_topic)):
self._last_odom = self._sub.get_last_msg(self._odom_topic)
# Update the current pose
self._current_position = self.transformMap(self._last_odom)
if (self._failed):
self._return = 'failed'
return 'failed'
# Transform the target points into the current odometry frame
self._target.header.stamp = self._last_odom.header.stamp
local_target = self._target
#self.transformOdom(self._target)
# If target point is withing lookahead distance then we are done
dr = np.sqrt(
(local_target.point.x - self._current_position.point.x)**2 +
(local_target.point.y - self._current_position.point.y)**2)
if (dr < self._lookahead):
Logger.loginfo(
' %s Lookahead circle is past target - done! target=(%f, %f, %f) robot=(%f,%f, %f) dr=%f lookahead=%f '
% (self.name, local_target.point.x, local_target.point.y,
local_target.point.z, self._current_position.point.x,
self._current_position.point.y,
self._current_position.point.z, dr, self._lookahead))
if (userdata.indice == len(userdata.plan.poses) - 1):
self._return = 'done'
return 'done'
else:
self._return = 'continue'
return 'continue'
# Transform the prior target point into the current odometry frame
self._prior.header.stamp = self._last_odom.header.stamp
local_prior = self._prior #self.transformOdom(self._prior)
if (self._failed):
self._return = 'failed'
return 'failed'
# Assume we can go the desired velocity
self._twist.twist.linear.x = self._desired_velocity
lookahead = None
lookahead = self.calculateLineTwist(local_prior, local_target)
if (lookahead is None):
# Did not get a lookahead point so we either failed, completed segment, or are deliberately holding the prior velocity (to recover from minor perturbations)
if (self._return is not None):
Logger.logerr("No lookahead!")
return self._return # return what was set (either 'failed' or 'done')
# Sanity check the rotation rate
if (math.fabs(self._twist.twist.angular.z) > self._max_rotation_rate):
self._twist.twist.linear.x = self._desired_velocity * self._max_rotation_rate / math.fabs(
self._twist.twist.angular.z) # decrease the speed
self._twist.twist.angular.z = math.copysign(
self._max_rotation_rate, self._twist.twist.angular.z)
# Normal operation - publish the latest calculated twist
self._twist.header.stamp = rospy.Time.now()
self._pub.publish(self._cmd_topic, self._twist)
if (self._marker_pub):
self._marker_pub.publish(self._marker_topic,
self._reference_marker)
self._marker_pub.publish(self._marker_topic,
self._local_target_marker)
return None
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.
self._start_time = rospy.Time.now()
self._return = None # reset the completion flag
self._failed = False # reset the failed flag
if (self._marker_pub):
self._marker.action = Marker.MODIFY
self._marker.color.a = 1.0
self._marker.color.r = 0.0
self._marker.color.g = 1.0 # Indicate this target is active
self._marker.color.b = 0.0
self._marker_pub.publish(self._marker_topic, self._marker)
if (userdata.indice > 0
and userdata.indice < len(userdata.plan.poses)):
Logger.loginfo(" Access data for index %d" % (userdata.indice))
self._target.point = Point(
userdata.plan.poses[userdata.indice].pose.position.x,
userdata.plan.poses[userdata.indice].pose.position.y, 0.0)
self._prior.point = Point(
userdata.plan.poses[userdata.indice - 1].pose.position.x,
userdata.plan.poses[userdata.indice - 1].pose.position.y, 0.0)
Logger.loginfo(
" Moving toward target %d =%f,%f from prior=%f,%f" %
(userdata.indice, self._target.point.x, self._target.point.y,
self._prior.point.x, self._prior.point.y))
else:
Logger.logerr(
" Invalid index %d - cannot access the path points!" %
(userdata.indice))
self._target.point = None
self._prior.point = None
self._failed = True
self._return = 'failed'
# Increment the index for the next segment
userdata.indice += 1
def on_exit(self, userdata):
# This method is called when the state becomes active, i.e. a transition from another state to this one is taken.
elapsed_time = rospy.Time.now() - self._start_time
# userdata.target_point = self._target # Set the user data for passing to next node
#Logger.logdebug(" Spent %f seconds in this segment target=%f,%f position=%f,%f" %
# (elapsed_time.to_sec(),self._target.point.x, self._target.point.y,
# self._current_position.point.x, self._current_position.point.y))
if (self._marker_pub):
self._marker.color.a = 1.0 # Don't forget to set the alpha!
self._marker.color.r = 0.8 # Indicate this target is no longer active
self._marker.color.g = 0.0
self._marker.color.b = 0.0
self._marker_pub.publish(self._marker_topic, self._marker)
def on_start(self):
self._return = None # Clear completion flag
# Wait for odometry message
while (not self._odom_sub.has_msg(self._odom_topic)):
Logger.logwarn('Waiting for odometry message from the robot ')
rospy.sleep(0.05)
while (not self._odom_sub.has_msg(self._odom_topic)):
Logger.logwarn(
'Waiting for odometry message to become available from the robot '
)
rospy.sleep(0.25)
self._last_odom = self._sub.get_last_msg(self._odom_topic)
self._odom_frame = self._last_odom.header.frame_id
#Logger.loginfo(' odometry frame id <%s>' % (self._odom_frame))
# Update the target transformation
self._target.header.stamp = self._last_odom.header.stamp
while (self.transformOdom(self._target) is None):
Logger.logwarn(
'Waiting for tf transformations to odometry frame to become available from the robot '
)
rospy.sleep(0.25)
self._target.header.stamp = rospy.Time.now()
while (self.transformMap(self._last_odom) is None):
Logger.logwarn(
'Waiting for tf transformations to map frame become available from the robot '
)
rospy.sleep(0.25)
self._last_odom = self._sub.get_last_msg(self._odom_topic)
self._current_position = self.transformMap(self._last_odom)
# This method is called when the state becomes active, i.e. a transition from another state to this one is taken.
if (self._marker_pub):
self._marker.action = Marker.ADD
self._marker.color.a = 1.0 # Set alpha otherwise the marker is invisible
self._marker.color.r = 0.0
self._marker.color.g = 0.0
self._marker.color.b = 1.0 # Indicate this target is planned
self._marker_pub.publish(self._marker_topic, self._marker)
self._marker_pub.publish(self._marker_topic,
self._reference_marker)
self._marker_pub.publish(self._marker_topic,
self._local_target_marker)
self._marker.action = Marker.MODIFY
self._reference_marker.action = Marker.MODIFY
self._reference_marker.color.a = 1.0 # Set alpha so it will become visible on next publish
self._local_target_marker.action = Marker.MODIFY
self._local_target_marker.color.a = 1.0 # Set alpha so it will become visible on next publish
# Method to calculate the lookahead point given line segment from prior to target
def calculateLineTwist(self, local_prior, local_target):
# Define a line segment from prior to the target (assume 2D)
pv = Vector3(local_target.point.x - local_prior.point.x,
local_target.point.y - local_prior.point.y, 0.0)
qv = Vector3(local_prior.point.x - self._current_position.point.x,
local_prior.point.y - self._current_position.point.y, 0.0)
# Find intersection of line segment with lookahead circle centered at the robot
a = pv.x * pv.x + pv.y * pv.y #
b = 2.0 * (qv.x * pv.x + qv.y * pv.y)
c = (qv.x * qv.x + qv.y * qv.y) - self._lookahead * self._lookahead
if (a < 0.001):
Logger.logerr(
' %s Invalid prior and target for line: target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) pv=(%f,%f) qv=(%f,%f) a=%f b=%f c=%f '
% (self.name, local_target.point.x, local_target.point.y,
local_target.point.z, local_prior.point.x,
local_prior.point.y, local_prior.point.z,
self._current_position.point.x,
self._current_position.point.y,
self._current_position.point.z, pv.x, pv.y, qv.x, qv.y, a,
b, c))
self._return = 'failed'
return None
discrim = b * b - 4 * a * c
if (discrim < 0.0):
# No intersection - this should not be unless bad start or localization perturbation
Logger.logwarn(
' %s No path recovery - no intersection for line: target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) pv=(%f,%f) qv=(%f,%f) a=%f b=%f c=%f discrim=%f '
% (self.name, local_target.point.x, local_target.point.y,
local_target.point.z, local_prior.point.x,
local_prior.point.y, local_prior.point.z,
self._current_position.point.x,
self._current_position.point.y,
self._current_position.point.z, pv.x, pv.y, qv.x, qv.y, a,
b, c, discrim))
self._return = 'failed'
return None
else:
# solve quadratic equation for intersection points
sqd = math.sqrt(discrim)
t1 = (-b - sqd) / (2 * a) # min value
t2 = (-b + sqd) / (2 * a) # max value
if (t2 < t1):
Logger.logwarn(
' %s Say what! t1=%f t2=%f sqd=%f target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) pv=(%f,%f) qv=(%f,%f) a=%f b=%f c=%f discrim=%f '
% (self.name, t1, t2, sqd, local_target.point.x,
local_target.point.y, local_target.point.z,
local_prior.point.x, local_prior.point.y,
local_prior.point.z, self._current_position.point.x,
self._current_position.point.y,
self._current_position.point.z, pv.x, pv.y, qv.x, qv.y,
a, b, c, discrim))
self._return = 'failed'
return None
if (t2 < 0.0):
# all intersections are behind the segment - this should not be unless bad start or localization perturbation
if (t2 > -0.1):
# likely due to localization perturbation
Logger.logwarn(
' %s Circle is before segment - continue prior motion! t1=%f t2=%f sqd=%f target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) pv=(%f,%f) qv=(%f,%f) a=%f b=%f c=%f discrim=%f '
% (self.name, t1, t2, sqd, local_target.point.x,
local_target.point.y, local_target.point.z,
local_prior.point.x, local_prior.point.y,
local_prior.point.z, self._current_position.point.x,
self._current_position.point.y,
self._current_position.point.z, pv.x, pv.y, qv.x,
qv.y, a, b, c, discrim))
return None
else:
Logger.logwarn(
' %s Circle is before segment! t1=%f t2=%f sqd=%f target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) pv=(%f,%f) qv=(%f,%f) a=%f b=%f c=%f discrim=%f '
% (self.name, t1, t2, sqd, local_target.point.x,
local_target.point.y, local_target.point.z,
local_prior.point.x, local_prior.point.y,
local_prior.point.z, self._current_position.point.x,
self._current_position.point.y,
self._current_position.point.z, pv.x, pv.y, qv.x,
qv.y, a, b, c, discrim))
self._return = 'failed'
return None
elif (t1 > 1.0):
# all intersections are past the segment
Logger.loginfo(
' %s Circle is past segment - done! t1=%f t2=%f sqd=%f target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) pv=(%f,%f) qv=(%f,%f) a=%f b=%f c=%f discrim=%f '
% (self.name, t1, t2, sqd, local_target.point.x,
local_target.point.y, local_target.point.z,
local_prior.point.x, local_prior.point.y,
local_prior.point.z, self._current_position.point.x,
self._current_position.point.y,
self._current_position.point.z, pv.x, pv.y, qv.x, qv.y,
a, b, c, discrim))
self._return = 'done'
return None
elif (t1 < 0.0 and t2 > 1.0):
# Segment is contained inside the lookahead circle
Logger.loginfo(
' %s Circle contains segment - move along! t1=%f t2=%f sqd=%f target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) pv=(%f,%f) qv=(%f,%f) a=%f b=%f c=%f discrim=%f '
% (self.name, t1, t2, sqd, local_target.point.x,
local_target.point.y, local_target.point.z,
local_prior.point.x, local_prior.point.y,
local_prior.point.z, self._current_position.point.x,
self._current_position.point.y,
self._current_position.point.z, pv.x, pv.y, qv.x, qv.y,
a, b, c, discrim))
self._return = 'done'
return None
elif (t2 > 1.0):
# The lookahead circle extends beyond the target point - we are finished here
Logger.loginfo(
' %s Circle extends past segment - done! t1=%f t2=%f sqd=%f target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) pv=(%f,%f) qv=(%f,%f) a=%f b=%f c=%f discrim=%f '
% (self.name, t1, t2, sqd, local_target.point.x,
local_target.point.y, local_target.point.z,
local_prior.point.x, local_prior.point.y,
local_prior.point.z, self._current_position.point.x,
self._current_position.point.y,
self._current_position.point.z, pv.x, pv.y, qv.x, qv.y,
a, b, c, discrim))
self._return = 'done'
return None
else:
# This is the normal case
# Must be line segment
control = deepcopy(local_prior)
control.point.x = control.point.x + t2 * pv.x # Control point in the odometry frame
control.point.y = control.point.y + t2 * pv.y # Control point in the odometry frame
control.point.z = control.point.z + t2 * pv.z # Control point in the odometry frame
self._reference_marker.pose.position = control.point
self._local_target_marker.pose.position = local_target.point
control_robot = self.transformBody(control)
curvature = 2.0 * control_robot.point.y / (self._lookahead *
self._lookahead)
self._twist.twist.angular.z = curvature * self._desired_velocity
return control_robot
return None
class DrivepathTestNew(EventState):
'''
Lets the robot move along a given path.
># path PoseStamped[] Array of Positions of the robot.
># speed float Speed of the robot
<= reached Robot is now located at the specified waypoint.
<= failed Failed to send a motion request to the action server.
'''
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})
def execute(self, userdata):
'''
Execute this state
'''
if self._failed:
return 'failed'
if self._reached:
return 'reached'
def on_enter(self, userdata):
self._path = MoveBaseActionPath()
self._point = PoseStamped()
self._point.pose.orientation.w = 1
self._point.pose.position.x = 0
self._path.goal.target_path.poses.append(self._point)
Logger.loginfo('%(x).3f %(y).3f %(z).3f' % {'x': self._point.pose.orientation.x, 'y': self._point.pose.orientation.y, 'z': self._point.pose.orientation.z})
self._failed = False
self._pub.publish(self._pathTopic, self._path)
self._reached = True
def on_stop(self):
pass
def on_exit(self, userdata):
pass
def on_pause(self):
self._move_client.cancel(self._action_topic)
def on_resume(self, userdata):
self.on_enter(userdata)
class DepthControl(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, rate):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(DepthControl, self).__init__(outcomes=['finished', 'failed'])
self._failed = False
self.Kpw = 0.1
self.Vmax = 0.2
self.Vconst = 0.5
self.Wmax = 0.5
self.set_rate(rate) ## change the rate of 'execute'
self.cmd_vel = '/cmd_vel'
self.depth = '/soybot/center_camera/depth/image_raw'
self._sub = ProxySubscriberCached({self.depth: Image})
self._pub = ProxyPublisher({self.cmd_vel: Twist})
self.bridge = CvBridge()
## so da para mudar rate por meio do GUI. Podemos pegar rosparam quando for rodar sem GUI.
def normalize(self, data):
data_n = (data - np.min(data)) / (np.max(data) - np.min(data)) * 255
return data_n
def preprocessing(self, imagem):
imagem_corrigida = deepcopy(imagem)
imagem_corrigida.setflags(write=1)
where_are_NaNs = np.isnan(imagem_corrigida)
imagem_corrigida[where_are_NaNs] = 3.0
img = self.normalize(imagem_corrigida).astype(np.uint8)
yf, xf = img.shape
x = 150
img = img[200:yf, x:xf - x]
#img = cv2.medianBlur(img,15)
imga = deepcopy(img)
img = cv2.createCLAHE(clipLimit=40.0, tileGridSize=(1, 640)).apply(img)
#imagempp = cv2.adaptiveThreshold(img,255,cv2.ADAPTIVE_THRESH_GAUSSIAN_C, cv2.THRESH_BINARY,11,10)
_, imagempp = cv2.threshold(img, 0, 255,
cv2.THRESH_BINARY + cv2.THRESH_OTSU)
a = np.sum(~imagempp[0:imagempp.shape[0] // 2, :])
#Logger.log('{}'.format(a), Logger.REPORT_HINT)
if (a <= 2052240): # 4855200 para //5
end_row = 1
else:
end_row = 0
return img, imagempp, imga, end_row
def actuate(self, theta, found_setpoint):
Vel = Twist()
if (found_setpoint == 1):
Vel.linear.x = self.Vconst
Vel.angular.z = theta * self.Kpw
if abs(Vel.angular.z) > self.Wmax:
Vel.angular.z = np.sign(Vel.angular.z) * self.Wmax
else:
Vel.linear.x = 0
Vel.angular.z = 0
try:
self._pub.publish(self.cmd_vel, Vel)
except Exception as e:
Logger.logwarn('Failed to send velocity:\n%s' % str(e))
self._failed = True
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._sub.has_msg(self.depth):
data = self._sub.get_last_msg(self.depth)
depth = self.bridge.imgmsg_to_cv2(data,
desired_encoding="passthrough")
_, imagempp, _, end_row = self.preprocessing(depth)
if (end_row):
self.actuate(0, 0)
return 'finished'
frame, mask_show, centroid_1, mask_show2 = imp.get_centroid(
~imagempp, ~imagempp, ~imagempp, ~imagempp)
y, x = imagempp.shape
if (centroid_1[0] > 0):
theta = (x / 2 - centroid_1[0])
theta = round(atan2(theta, y / 2), 2)
found_setpoint = 1
else:
found_setpoint = 0
self.actuate(theta, found_setpoint)
if (self._failed):
return 'failed'
def on_enter(self, userdata):
# This method is called when the state becomes active, i.e. a transition from another state to this one is taken.
# It is primarily used to start actions which are associated with this state.
# 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.
self.failed = False
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.
# In this example, we use this event to set the correct start time.
#self._start_time = rospy.Time.now()
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.
class EventState(OperatableState):
"""
A state that allows implementing certain events.
"""
def __init__(self, *args, **kwargs):
super(EventState, self).__init__(*args, **kwargs)
self._entering = True
self._skipped = False
self.__execute = self.execute
self.execute = self._event_execute
self._paused = False
self._last_active_container = None
self._feedback_topic = "flexbe/command_feedback"
self._repeat_topic = "flexbe/command/repeat"
self._pause_topic = "flexbe/command/pause"
self._pub = ProxyPublisher()
self._sub = ProxySubscriberCached()
def _event_execute(self, *args, **kwargs):
if self._is_controlled and self._sub.has_msg(self._pause_topic):
msg = self._sub.get_last_msg(self._pause_topic)
if msg.data:
self._sub.remove_last_msg(self._pause_topic)
rospy.loginfo("--> Pausing in state %s", self.name)
self._pub.publish(self._feedback_topic, CommandFeedback(command="pause"))
self._last_active_container = PriorityContainer.active_container
PriorityContainer.active_container = ""
self._paused = True
if self._paused and not PreemptableState.preempt:
self._notify_skipped()
return self._loopback_name
if self._entering:
self._entering = False
self.on_enter(*args, **kwargs)
if self._skipped:
self._skipped = False
self.on_resume(*args, **kwargs)
execute_outcome = self.__execute(*args, **kwargs)
repeat = False
if self._is_controlled and self._sub.has_msg(self._repeat_topic):
rospy.loginfo("--> Repeating state %s", self.name)
self._sub.remove_last_msg(self._repeat_topic)
self._pub.publish(self._feedback_topic, CommandFeedback(command="repeat"))
repeat = True
if execute_outcome != self._loopback_name and not PreemptableState.switching or repeat:
self._entering = True
self.on_exit(*args, **kwargs)
return execute_outcome
def _notify_skipped(self):
if not self._skipped:
self.on_pause()
self._skipped = True
if self._is_controlled and self._sub.has_msg(self._pause_topic):
msg = self._sub.get_last_msg(self._pause_topic)
if not msg.data:
self._sub.remove_last_msg(self._pause_topic)
rospy.loginfo("--> Resuming in state %s", self.name)
self._pub.publish(self._feedback_topic, CommandFeedback(command="resume"))
PriorityContainer.active_container = self._last_active_container
self._last_active_container = None
self._paused = False
super(EventState, self)._notify_skipped()
def _enable_ros_control(self):
super(EventState, self)._enable_ros_control()
self._pub.createPublisher(self._feedback_topic, CommandFeedback)
self._sub.subscribe(self._repeat_topic, Empty)
self._sub.subscribe(self._pause_topic, Bool)
def _disable_ros_control(self):
super(EventState, self)._disable_ros_control()
self._sub.unsubscribe_topic(self._repeat_topic)
self._sub.unsubscribe_topic(self._pause_topic)
self._last_active_container = None
if self._paused:
PriorityContainer.active_container = None
# Events
# (just implement the ones you need)
def on_start(self):
"""
Will be executed once when the behavior starts.
"""
pass
def on_stop(self):
"""
Will be executed once when the behavior stops or is preempted.
"""
pass
def on_pause(self):
"""
Will be executed each time this state is paused.
"""
pass
def on_resume(self, userdata):
"""
Will be executed each time this state is resumed.
"""
pass
def on_enter(self, userdata):
"""
Will be executed each time the state is entered from any other state (but not from itself).
"""
pass
def on_exit(self, userdata):
"""
Will be executed each time the state will be left to any other state (but not to itself).
"""
pass
class SweetieBotRandHeadMovements(EventState):
'''
Periodically reposition eyes and head of SweetieBot to random point using given FollowJointState controller.
-- controller string FollowJointState controller name without `controller` prefix.
-- duration float How long this state will be executed (seconds).
-- interval float[2] Array of floats, maximal and minimal interval between movements.
-- max2356 float[4] Max values for joint52, joint53, eyes_pitch, eyes_yaw.
-- min2356 float[4] Min values for joint52, joint53, eyes_pitch, eyes_yaw.
># config dict Dictionary with keys 'duration', 'interval', 'max2356', 'min2356' to override default configuration. dict or key values can be set to None to use default value from parameters.
<= done Finished.
<= failed Failed to activate FollowJointState controller.
'''
def __init__(self,
controller='joint_state_head',
duration=120,
interval=[3, 5],
max2356=[0.3, 0.3, 1.5, 1.5],
min2356=[-0.3, -0.3, -1.5, -1.5]):
super(SweetieBotRandHeadMovements,
self).__init__(outcomes=['done', 'failed'],
input_keys=['config'])
# Store topic parameter for later use.
self._controller = 'motion/controller/' + controller
# create proxies
self._set_operational_caller = ProxyServiceCaller(
{self._controller + '/set_operational': SetBool})
self._publisher = ProxyPublisher(
{self._controller + '/in_joints_ref': JointState})
# state
self._start_timestamp = None
self._movement_timestamp = None
self._movement_duration = Duration()
# user input
self.set_movement_parameters(duration, interval, min2356, max2356)
# error in enter hook
self._error = False
def set_movement_parameters(self, duration, interval, min2356, max2356):
if not isinstance(duration, (int, float)) or duration < 0:
raise ValueError("Duration must be nonegative number.")
if len(interval) != 2 or any([
not isinstance(t, (int, float)) for t in interval
]) or any([t < 0 for t in interval]) or interval[0] > interval[1]:
raise ValueError("Interval must represent interval of time.")
if len(min2356) != 4 or any(
[not isinstance(v, (int, float)) for v in min2356]):
raise ValueError("min2356: list of four numbers was expected.")
if len(max2356) != 4 or any(
[not isinstance(v, (int, float)) for v in max2356]):
raise ValueError("max2356: list of four numbers was expected.")
self._duration = Duration.from_sec(duration)
self._interval = interval
self._min2356 = min2356
self._max2356 = max2356
def on_enter(self, userdata):
self._error = False
# override configuration if necessary
try:
if userdata.config != None:
# process dict
duration = userdata.config.get("duration",
self._duration.to_sec())
interval = userdata.config.get("interval", self._interval)
min2356 = userdata.config.get("min2356", self._min2356)
max2356 = userdata.config.get("max2356", self._max2356)
# check parameters
self.set_movement_parameters(duration, interval, min2356,
max2356)
except Exception as e:
Logger.logwarn('Failed to process `config` input key:\n%s' %
str(e))
self._error = True
return
# activate head controller
try:
res = self._set_operational_caller.call(
self._controller + '/set_operational', True)
except Exception as e:
Logger.logwarn('Failed to activate `' + self._controller +
'` controller:\n%s' % str(e))
self._error = True
return
if not res.success:
Logger.logwarn('Failed to activate `' + self._controller +
'` controller (SetBoolResponse: success = false).')
self._error = True
# set start timestamp
self._start_timestamp = Time.now()
self._movement_timestamp = Time.now()
Logger.loginfo(
'Start random pose generation (eyes, head), controller `%s`.' %
self._controller)
def execute(self, userdata):
if self._error:
return 'failed'
# check if time elasped
if Time.now() - self._start_timestamp > self._duration:
return 'done'
# check if we have tosend new point
if Time.now() - self._movement_timestamp > self._movement_duration:
# determine new interval
self._movement_timestamp = Time.now()
self._movement_duration = Duration.from_sec(
random.uniform(*self._interval))
# form message
msg = JointState()
msg.header = Header(stamp=Time.now())
msg.name = ['joint52', 'joint53', 'eyes_pitch', 'eyes_yaw']
# random durection
x = random.uniform(0, 1)
y = random.uniform(0, 1)
# compute head position
msg.position = [0, 0, 0, 0]
msg.position[0] = self._min2356[0] * x + self._max2356[0] * (1 - x)
msg.position[1] = self._min2356[1] * y + self._max2356[1] * (1 - y)
# compute eyes position
msg.position[2] = self._min2356[2] * y + self._max2356[2] * (1 - y)
msg.position[3] = self._min2356[3] * x + self._max2356[3] * (1 - x)
# send message
try:
self._publisher.publish(self._controller + '/in_joints_ref',
msg)
except Exception as e:
Logger.logwarn('Failed to send JointState message `' +
self._topic + '`:\n%s' % str(e))
def on_exit(self, userdata):
try:
res = self._set_operational_caller.call(
self._controller + '/set_operational', False)
except Exception as e:
Logger.logwarn('Failed to deactivate `' + self._controller +
'` controller:\n%s' % str(e))
return
Logger.loginfo('Done random pose generation for eyes and head.')
class LockableState(ManuallyTransitionableState):
"""
A state that can be locked.
When locked, no transition can be done regardless of the resulting outcome.
However, if any outcome would be triggered, the outcome will be stored
and the state won't be executed anymore until it is unlocked and the stored outcome is set.
"""
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 _lockable_execute(self, *args, **kwargs):
if self._is_controlled and self._sub.has_msg(self._lock_topic):
msg = self._sub.get_last_msg(self._lock_topic)
self._sub.remove_last_msg(self._lock_topic)
self._execute_lock(msg.data)
if self._is_controlled and self._sub.has_msg(self._unlock_topic):
msg = self._sub.get_last_msg(self._unlock_topic)
self._sub.remove_last_msg(self._unlock_topic)
self._execute_unlock(msg.data)
if self._locked:
if self._stored_outcome is None or self._stored_outcome == 'None':
self._stored_outcome = self.__execute(*args, **kwargs)
return None
if not self._locked and not self._stored_outcome is None and not self._stored_outcome == 'None':
if self._parent.transition_allowed(self.name, self._stored_outcome):
outcome = self._stored_outcome
self._stored_outcome = None
return outcome
else:
return None
outcome = self.__execute(*args, **kwargs)
if outcome is None or outcome == 'None':
return None
if not self._parent is None and not self._parent.transition_allowed(self.name, outcome):
self._stored_outcome = outcome
return None
return outcome
def _execute_lock(self, target):
found_target = False
if target == self._get_path():
found_target = True
self._locked = True
else:
found_target = self._parent.lock(target)
self._pub.publish(self._feedback_topic, CommandFeedback(command="lock", args=[target, target if found_target else ""]))
if not found_target:
rospy.logwarn("--> Wanted to lock %s, but could not find it in current path %s.", target, self._get_path())
else:
rospy.loginfo("--> Locking in state %s", target)
def _execute_unlock(self, target):
found_target = False
if target == self._get_path():
found_target = True
self._locked = False
else:
found_target = self._parent.unlock(target)
self._pub.publish(self._feedback_topic, CommandFeedback(command="unlock", args=[target, target if found_target else ""]))
if not found_target:
rospy.logwarn("--> Wanted to unlock %s, but could not find it in current path %s.", target, self._get_path())
else:
rospy.loginfo("--> Unlocking in state %s", target)
def _enable_ros_control(self):
super(LockableState, self)._enable_ros_control()
self._pub.createPublisher(self._feedback_topic, CommandFeedback)
self._sub.subscribe(self._lock_topic, String)
self._sub.subscribe(self._unlock_topic, String)
def _disable_ros_control(self):
super(LockableState, self)._disable_ros_control()
self._sub.unsubscribe_topic(self._lock_topic)
self._sub.unsubscribe_topic(self._unlock_topic)
def is_locked(self):
return self._locked
class TurnState(EventState):
'''
Turn state for a ground robot. This state allows the robot to turn at a certain angle
at a specified velocity/ speed.
-- t_speed float Speed at which to turn the robot
-- turn_angle float The angle that the robot should make
-- forward_dist float free distance in front of robot
<= failed If behavior is unable to succeed on time
<= done forward distance becomes sufficantly large
'''
def __init__(self, t_speed, turn_angle, forward_dist, timeout):
super(TurnState, self).__init__(outcomes=['failed', 'done'])
self._t_speed = t_speed
self._turn_angle = turn_angle
self._forward_dist = forward_dist
self._timeout = timeout
self._start_time = None
self.depth = None
self.vel_topic = '/cmd_vel'
self.scan_topic = '/depth_data'
#create publisher passing it the vel_topic_name and msg_type
self.pub = ProxyPublisher({self.vel_topic: Twist_float})
#create subscriber
self.scan_sub = ProxySubscriberCached({self.scan_topic: Depth})
self.scan_sub.set_callback(self.scan_topic, self.scan_callback)
def execute(self, userdata):
if not self.cmd_pub: # check if message in self.cmd_pub to publish to /cmd_vel else we exit
Logger.loginfo('messesage does not exist')
return 'failed'
#run obstacle checks [index 0: left, 360: middle, 719: right]
if (self.depth is not None):
Logger.loginfo('FWD free distance is: %s' % self.depth.center)
Logger.loginfo('Turn angle is : %s' % self._turn_angle)
if self.depth.center >= self._forward_dist:
return 'done'
#measure distance travelled
elapsed_time = (rospy.Time.now() - self._start_time).to_sec()
if elapsed_time >= self._timeout:
Logger.loginfo('Reached timeout')
return 'failed'
#drive
self.pub.publish(self.vel_topic, self.cmd_pub)
def on_enter(self, userdata):
Logger.loginfo("Turn RIGHT STARTED!")
#set robot speed here
self.cmd_pub = Twist_float()
self.cmd_pub.vel = self._t_speed
self.cmd_pub.angle = self._turn_angle
self._start_time = rospy.Time.now()
def on_exit(self, userdata):
self.cmd_pub.vel = 0.0
self.cmd_pub.angle = 0.0
self.pub.publish(self.vel_topic, self.cmd_pub)
Logger.loginfo("Turn RIGHT ENDED!")
def on_start(self):
Logger.loginfo("Drive FWD READY!")
self._start_time = rospy.Time.now() #bug detected! (move to on_enter)
def on_stop(self):
Logger.loginfo("Turn RIGHT STOPPED!")
def scan_callback(self, data):
self.depth = data
class PrayingMantisCalibrationSM(Behavior):
'''
A behavior that moves ATLAS into the "praying mantis" pose upon startup in order to get consistent joint encoder offsets for calibration purposes.
'''
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
# [/MANUAL_INIT]
# Behavior comments:
# O 47 211 /Perform_Checks/Manipulate_Limits
# Without this output_key, Check Behavior complains. Because traj_past_limits could in theory be undefined during runtime.
def create(self):
initial_mode = "stand"
motion_mode = "manipulate"
mantis_mode = "manipulate_limits"
percent_past_limits = 0.10 # before: 0.075
# x:788 y:72, x:474 y:133
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.target_limits = 'upper'
_state_machine.userdata.cycle_counter = 1
_state_machine.userdata.stand_posture = None # calculated
_state_machine.userdata.offsets = {
'left_arm': dict(),
'right_arm': dict()
}
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
self._percent_past_limits = percent_past_limits
# Create STAND posture trajectory
_state_machine.userdata.stand_posture = AtlasFunctions.gen_stand_posture_trajectory(
)
# [/MANUAL_CREATE]
# x:222 y:281, x:349 y:167
_sm_determine_offsets_0 = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['cycle_counter', 'offsets'],
output_keys=['offsets'])
with _sm_determine_offsets_0:
# x:61 y:53
OperatableStateMachine.add(
'Get_Left_Joint_Positions',
CurrentJointPositionsState(planning_group="l_arm_group"),
transitions={
'retrieved': 'Determine_Closest_Limits_Left',
'failed': 'failed'
},
autonomy={
'retrieved': Autonomy.Off,
'failed': Autonomy.Low
},
remapping={'joint_positions': 'joint_positions'})
# x:319 y:54
OperatableStateMachine.add(
'Determine_Closest_Limits_Left',
CalculationState(calculation=self.get_closest_limits_left),
transitions={'done': 'Store_Offsets_Left'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'joint_positions',
'output_value': 'joint_limits'
})
# x:598 y:162
OperatableStateMachine.add(
'Get_Right_Joint_Positions',
CurrentJointPositionsState(planning_group="r_arm_group"),
transitions={
'retrieved': 'Determine_Closest_Limits_Right',
'failed': 'failed'
},
autonomy={
'retrieved': Autonomy.Off,
'failed': Autonomy.Low
},
remapping={'joint_positions': 'joint_positions'})
# x:584 y:275
OperatableStateMachine.add(
'Determine_Closest_Limits_Right',
CalculationState(calculation=self.get_closest_limits_right),
transitions={'done': 'Store_Offsets_Right'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'joint_positions',
'output_value': 'joint_limits'
})
# x:608 y:54
OperatableStateMachine.add(
'Store_Offsets_Left',
FlexibleCalculationState(
calculation=self.store_offsets_left,
input_keys=['limits', 'value', 'offsets', 'counter']),
transitions={'done': 'Get_Right_Joint_Positions'},
autonomy={'done': Autonomy.Off},
remapping={
'limits': 'joint_limits',
'value': 'joint_positions',
'offsets': 'offsets',
'counter': 'cycle_counter',
'output_value': 'offsets'
})
# x:340 y:274
OperatableStateMachine.add(
'Store_Offsets_Right',
FlexibleCalculationState(
calculation=self.store_offsets_right,
input_keys=['limits', 'value', 'offsets', 'counter']),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'limits': 'joint_limits',
'value': 'joint_positions',
'offsets': 'offsets',
'counter': 'cycle_counter',
'output_value': 'offsets'
})
# x:528 y:401, x:707 y:282
_sm_manipulate_limits_1 = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['cycle_counter', 'offsets'],
output_keys=['offsets', 'traj_past_limits'])
with _sm_manipulate_limits_1:
# x:100 y:156
OperatableStateMachine.add(
'Prevent_Runtime_Failure',
CalculationState(calculation=lambda x: dict()),
transitions={'done': 'Go_to_MANIPULATE_LIMITS'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'cycle_counter',
'output_value': 'traj_past_limits'
})
# x:387 y:55
OperatableStateMachine.add(
'Wait_for_Control_Mode_change',
WaitState(wait_time=1.0),
transitions={'done': 'Get_Left_Joint_Positions'},
autonomy={'done': Autonomy.Low})
# x:895 y:279
OperatableStateMachine.add(
'Gen_Traj_from_90%_to_110%',
CalculationState(calculation=self.gen_traj_past_limits),
transitions={'done': 'Go_to_110%_Joint_Limits'},
autonomy={'done': Autonomy.Low},
remapping={
'input_value': 'current_joint_values',
'output_value': 'traj_past_limits'
})
# x:893 y:391
OperatableStateMachine.add(
'Go_to_110%_Joint_Limits',
ExecuteTrajectoryBothArmsState(controllers=[
'left_arm_traj_controller', 'right_arm_traj_controller'
]),
transitions={
'done': 'Determine_Offsets',
'failed': 'Determine_Offsets'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.High
},
remapping={'trajectories': 'traj_past_limits'})
# x:651 y:385
OperatableStateMachine.add('Determine_Offsets',
_sm_determine_offsets_0,
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'cycle_counter': 'cycle_counter',
'offsets': 'offsets'
})
# x:648 y:54
OperatableStateMachine.add(
'Get_Left_Joint_Positions',
CurrentJointPositionsState(planning_group="l_arm_group"),
transitions={
'retrieved': 'Get_Right_Joint_Positions',
'failed': 'failed'
},
autonomy={
'retrieved': Autonomy.Off,
'failed': Autonomy.High
},
remapping={'joint_positions': 'joint_positions_left'})
# x:904 y:53
OperatableStateMachine.add(
'Get_Right_Joint_Positions',
CurrentJointPositionsState(planning_group="r_arm_group"),
transitions={
'retrieved': 'Generate_Joint_Positions_Struct',
'failed': 'failed'
},
autonomy={
'retrieved': Autonomy.Off,
'failed': Autonomy.High
},
remapping={'joint_positions': 'joint_positions_right'})
# x:886 y:168
OperatableStateMachine.add(
'Generate_Joint_Positions_Struct',
FlexibleCalculationState(calculation=lambda ik: {
'left_arm': ik[0],
'right_arm': ik[1]
},
input_keys=['left', 'right']),
transitions={'done': 'Gen_Traj_from_90%_to_110%'},
autonomy={'done': Autonomy.Off},
remapping={
'left': 'joint_positions_left',
'right': 'joint_positions_right',
'output_value': 'current_joint_values'
})
# x:92 y:55
OperatableStateMachine.add(
'Go_to_MANIPULATE_LIMITS',
ChangeControlModeActionState(target_mode=mantis_mode),
transitions={
'changed': 'Wait_for_Control_Mode_change',
'failed': 'failed'
},
autonomy={
'changed': Autonomy.Off,
'failed': Autonomy.High
})
# x:574 y:247, x:276 y:549
_sm_update_calibration_2 = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['offsets'])
with _sm_update_calibration_2:
# x:46 y:44
OperatableStateMachine.add(
'Process_Offsets',
CalculationState(calculation=self.process_offsets),
transitions={'done': 'Print_Offset_Info'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'offsets',
'output_value': 'offsets'
})
# x:227 y:45
OperatableStateMachine.add(
'Print_Offset_Info',
CalculationState(calculation=self.print_offset_info),
transitions={'done': 'Publish_Offsets'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'offsets',
'output_value': 'none'
})
# x:390 y:158
OperatableStateMachine.add(
'Ask_Perform_Update',
OperatorDecisionState(
outcomes=['update', 'no_update'],
hint=
"Do you want to apply the calculated offsets for calibration?",
suggestion=None),
transitions={
'update': 'Convert_Offset_Data',
'no_update': 'finished'
},
autonomy={
'update': Autonomy.Full,
'no_update': Autonomy.Full
})
# x:232 y:337
OperatableStateMachine.add(
'Update_Calibration',
UpdateJointCalibrationState(
joint_names=self._joint_names['left_arm'][0:4] +
self._joint_names['right_arm'][0:4]),
transitions={
'updated': 'Calibration_Successful',
'failed': 'Calibration_Failed'
},
autonomy={
'updated': Autonomy.Low,
'failed': Autonomy.High
},
remapping={'joint_offsets': 'offset_list'})
# x:241 y:242
OperatableStateMachine.add(
'Convert_Offset_Data',
CalculationState(calculation=lambda o: o['left_arm']['avg'] +
o['right_arm']['avg']),
transitions={'done': 'Update_Calibration'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'offsets',
'output_value': 'offset_list'
})
# x:522 y:337
OperatableStateMachine.add(
'Calibration_Successful',
LogState(text="Successfully updated calibration offsets.",
severity=Logger.REPORT_INFO),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:246 y:445
OperatableStateMachine.add(
'Calibration_Failed',
LogState(text="Failed to apply calibration offsets!",
severity=Logger.REPORT_ERROR),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:399 y:44
OperatableStateMachine.add(
'Publish_Offsets',
CalculationState(calculation=self.publish_offsets),
transitions={'done': 'Ask_Perform_Update'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'offsets',
'output_value': 'none'
})
# x:978 y:197, x:394 y:80
_sm_perform_checks_3 = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['cycle_counter', 'target_limits', 'offsets'],
output_keys=['cycle_counter', 'offsets'])
with _sm_perform_checks_3:
# x:105 y:74
OperatableStateMachine.add(
'Go_to_Intermediate_Mode',
ChangeControlModeActionState(target_mode=motion_mode),
transitions={
'changed': 'Gen_Traj_to_90%_Limits',
'failed': 'failed'
},
autonomy={
'changed': Autonomy.Off,
'failed': Autonomy.High
})
# x:653 y:274
OperatableStateMachine.add('Manipulate_Limits',
_sm_manipulate_limits_1,
transitions={
'finished':
'Gen_Traj_back_to_90%_Limits',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'cycle_counter': 'cycle_counter',
'offsets': 'offsets',
'traj_past_limits':
'traj_past_limits'
})
# x:903 y:78
OperatableStateMachine.add(
'Increment_Cycle_counter',
CalculationState(calculation=lambda counter: counter + 1),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'cycle_counter',
'output_value': 'cycle_counter'
})
# x:344 y:277
OperatableStateMachine.add(
'Move_to_90%_Joint_Limits',
MoveitStartingPointState(vel_scaling=0.3),
transitions={
'reached': 'Manipulate_Limits',
'failed': 'Move_to_90%_Joint_Limits'
},
autonomy={
'reached': Autonomy.Low,
'failed': Autonomy.Full
},
remapping={'trajectories': 'trajectories_90'})
# x:114 y:276
OperatableStateMachine.add(
'Gen_Traj_to_90%_Limits',
CalculationState(calculation=self.gen_traj_pre_limits),
transitions={'done': 'Move_to_90%_Joint_Limits'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'target_limits',
'output_value': 'trajectories_90'
})
# x:636 y:78
OperatableStateMachine.add(
'Go_back_to_90%_Joint_Limits',
ExecuteTrajectoryBothArmsState(controllers=[
'left_arm_traj_controller', 'right_arm_traj_controller'
]),
transitions={
'done': 'Increment_Cycle_counter',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.High
},
remapping={'trajectories': 'traj_back_to_90'})
# x:636 y:172
OperatableStateMachine.add(
'Gen_Traj_back_to_90%_Limits',
FlexibleCalculationState(
calculation=self.gen_traj_back_from_limits,
input_keys=['trajectories_90', 'traj_past_limits']),
transitions={'done': 'Go_back_to_90%_Joint_Limits'},
autonomy={'done': Autonomy.Off},
remapping={
'trajectories_90': 'trajectories_90',
'traj_past_limits': 'traj_past_limits',
'output_value': 'traj_back_to_90'
})
with _state_machine:
# x:110 y:52
OperatableStateMachine.add(
'Initial_Control_Mode',
ChangeControlModeActionState(target_mode=initial_mode),
transitions={
'changed': 'Perform_Checks',
'failed': 'failed'
},
autonomy={
'changed': Autonomy.High,
'failed': Autonomy.High
})
# x:712 y:317
OperatableStateMachine.add(
'Initial_Mode_before_exit',
ChangeControlModeActionState(target_mode=initial_mode),
transitions={
'changed': 'Update_Calibration',
'failed': 'failed'
},
autonomy={
'changed': Autonomy.Off,
'failed': Autonomy.High
})
# x:122 y:302
OperatableStateMachine.add('Perform_Checks',
_sm_perform_checks_3,
transitions={
'finished':
'Are_We_Done_Yet?',
'failed':
'Intermediate_Mode_before_exit'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'cycle_counter': 'cycle_counter',
'target_limits': 'target_limits',
'offsets': 'offsets'
})
# x:126 y:505
OperatableStateMachine.add(
'Are_We_Done_Yet?',
DecisionState(outcomes=["done", "more"],
conditions=lambda counter: "done"
if counter >= 2 else "more"),
transitions={
'done': 'Intermediate_Mode_before_exit',
'more': 'Setup_next_Cycle'
},
autonomy={
'done': Autonomy.Low,
'more': Autonomy.High
},
remapping={'input_value': 'cycle_counter'})
# x:15 y:404
OperatableStateMachine.add(
'Setup_next_Cycle',
CalculationState(calculation=lambda lim: 'lower'
if lim == 'upper' else 'upper'),
transitions={'done': 'Perform_Checks'},
autonomy={'done': Autonomy.Low},
remapping={
'input_value': 'target_limits',
'output_value': 'target_limits'
})
# x:725 y:186
OperatableStateMachine.add('Update_Calibration',
_sm_update_calibration_2,
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'offsets': 'offsets'})
# x:726 y:427
OperatableStateMachine.add(
'Move_to_Stand_Posture',
MoveitStartingPointState(vel_scaling=0.3),
transitions={
'reached': 'Initial_Mode_before_exit',
'failed': 'Move_to_Stand_Posture'
},
autonomy={
'reached': Autonomy.Off,
'failed': Autonomy.Full
},
remapping={'trajectories': 'stand_posture'})
# x:412 y:427
OperatableStateMachine.add(
'Intermediate_Mode_before_exit',
ChangeControlModeActionState(target_mode=motion_mode),
transitions={
'changed': 'Move_to_Stand_Posture',
'failed': 'failed'
},
autonomy={
'changed': Autonomy.Off,
'failed': Autonomy.High
})
return _state_machine
# Private functions can be added inside the following tags
# [MANUAL_FUNC]
def gen_traj_pre_limits(self, limits_side):
"""Create trajectories for going to 90 percent of joint limits (either upper or lower limits)"""
joint_config = {
'left_arm': self._joint_calib['left_arm'][limits_side],
'right_arm': self._joint_calib['right_arm'][limits_side]
}
return AtlasFunctions.gen_arm_trajectory_from_joint_configuration(
joint_config)
def _get_closest_limits(self, side, current_values):
"""
Selects the closest limit with respect to the current value (upper or lower bound).
"""
limits = self._joint_limits[side]
closest_limit = list()
for i in range(len(current_values)):
near_limit = 'upper' if abs(limits['upper'][i] - current_values[i]
) < abs(limits['lower'][i] -
current_values[i]) else 'lower'
closest_limit.append(limits[near_limit][i])
rospy.loginfo("Limit joint positions: %s" % str(closest_limit))
rospy.loginfo("Current joint positions: %s" % str(current_values))
return closest_limit
def get_closest_limits_left(self, current_values):
return self._get_closest_limits('left_arm', current_values)
def get_closest_limits_right(self, current_values):
return self._get_closest_limits('right_arm', current_values)
def gen_traj_past_limits(self, current_joint_values):
"""
Given all joint limits, generate a trajectory that takes the joints to 110%% percent past limits.
atlas_v5 update: Do not push the lower 3 joints (electric ones) path the limits.
"""
result = dict()
for arm in ['left_arm', 'right_arm']:
current_values = current_joint_values[arm]
arm_limits = self._get_closest_limits(arm, current_values)
arm_target = list()
arm_effort = list()
percentage = self._percent_past_limits
# Push the upper 4 joints against the limits
for i in range(0, 4):
near_limit = 'upper' if self._joint_limits[arm]['upper'][
i] == arm_limits[i] else 'lower'
limit_range = self._joint_limits[arm]['upper'][
i] - self._joint_limits[arm]['lower'][i]
offset_sign = 1 if near_limit is 'upper' else -1
arm_target.append(arm_limits[i] +
offset_sign * percentage * limit_range)
arm_effort.append(float(offset_sign))
# "Ignore" the lower 3 joints (electric motor ones)
for i in range(4, 7):
arm_target.append(current_values[i])
arm_effort.append(
0.0
) # Zero effort stands for not applying additional force
trajectory = JointTrajectory()
trajectory.joint_names = self._joint_names[arm]
point = JointTrajectoryPoint()
point.positions = arm_target
point.velocities = [0.0] * len(
arm_target) # David's controller expects zero velocities
point.effort = arm_effort
point.time_from_start = rospy.Duration.from_sec(2.5)
trajectory.points.append(point)
# rospy.loginfo("110%% joint positions for %s arm: %s" % (arm, str(arm_target[0:4]))) # Only report the relevant joints
result[arm] = trajectory
return result
def gen_traj_back_from_limits(self, input_keys):
"""The resulting trajectory points are the same as for going to 90%% of limits, but with the efforts set for David's controllers."""
traj_pre_limits = input_keys[0]
traj_past_limits = input_keys[1]
traj_back_to_90 = dict()
for arm in ['left_arm', 'right_arm']:
trajectory = traj_pre_limits[
arm] # Start with 90% of joint limits as the trajectory points
trajectory.points[0].effort = traj_past_limits[arm].points[
0].effort # Set efforts as per David's controllers
trajectory.points[0].time_from_start = rospy.Duration.from_sec(1.0)
# David's controller expects zero velocities
trajectory.points[0].velocities = [0.0] * len(
trajectory.points[0].positions)
traj_back_to_90[arm] = trajectory
return traj_back_to_90
def store_offsets(self, side, input_keys):
limits = input_keys[0][0:4] # Ignore the lower 3 joints
values = input_keys[1][0:4] # --//-- --//--
offsets = input_keys[2]
counter = input_keys[3]
offsets[side][counter] = [
limit - value for limit, value in zip(limits, values)
]
msg = JointTrajectory()
msg.joint_names = self._joint_names[side][
0:4] # Ignore the lower 3 joints
point = JointTrajectoryPoint()
point.positions = values
point.velocities = offsets[side][counter]
msg.points.append(point)
self._pub.publish(self._offset_topic, msg)
Logger.loginfo("Publishing %s arm offsets to %s" %
(side, self._offset_topic))
return offsets
def publish_offsets(self,
offsets,
arms=['left_arm', 'right_arm'],
current_values=[]):
for side in arms:
msg = JointTrajectory()
msg.joint_names = self._joint_names[side]
point = JointTrajectoryPoint()
point.positions = current_values
point.velocities = offsets[side]['avg']
msg.points.append(point)
self._pub.publish(self._offset_topic, msg)
Logger.loginfo("Publishing %s arm offsets to %s" %
(side, self._offset_topic))
def store_offsets_left(self, input_keys):
return self.store_offsets('left_arm', input_keys)
def store_offsets_right(self, input_keys):
return self.store_offsets('right_arm', input_keys)
def process_offsets(self, offsets):
for side in ['left_arm', 'right_arm']:
# transposes list of lists from iteration,joint to joint,iteration
iteration_values = map(list, zip(*offsets[side].values()))
# Calculate the average offset and the deviation from the average
offsets[side]['avg'] = [
sum(joint_entries) / float(len(joint_entries))
for joint_entries in iteration_values
]
offsets[side]['diff'] = [
max(map(lambda x: abs(x - avg),
joint_entries)) for joint_entries, avg in zip(
iteration_values, offsets[side]['avg'])
]
return offsets
def print_offset_info(self, offsets):
sides = ['left_arm', 'right_arm']
for side in sides:
Logger.loginfo("Joint order (%s): %s" %
(side, str(self._joint_names[side][0:4])))
rounded_offsets = [
round(offset, 3) for offset in offsets[side]['avg']
] # round due to comms_bridge
Logger.loginfo("Offsets (%s): %s" % (side, str(rounded_offsets)))
# Logger.loginfo("Max deviation from average (%s): %s" % (side, str(offsets[side]['diff'])))
pprint.pprint(offsets) # Pretty print to the "onboard" terminal
class WebsiteDummyModalState(EventState):
'''
State to demonstrate functionality of webUI. Constantely publishes to /robot_status and also calls the service_proxy to change the shown webpage to the current robot status
<= failed If service_proxy does not answer with success
<= done If service_proxy answers with success
'''
def __init__(self):
super(WebsiteDummyModalState,
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)
def execute(self, userdata):
self.current_status_pub.publish(self.status_topic, self.cmd_status)
if self._response.success == True and (rospy.Time.now() -
self._start_time).to_sec() > 10:
Logger.loginfo("-------------------------")
Logger.loginfo("successfull service_proxy: " +
str(self._response.success))
Logger.loginfo(self._response.message)
Logger.loginfo("data:" + self._response.data)
Logger.loginfo("-------------------------")
return 'done'
elif self._response.success == False:
return 'failed'
def on_enter(self, userdata):
self._start_time = rospy.Time.now()
Logger.loginfo("started website_dummy_state1!")
self.cmd_status = RobotStatus()
self.cmd_status.currentTask = '192863'
self.cmd_status.currentBehaviorStatus = ''
self.cmd_status.lastTask = '203056'
self.cmd_status.lastTaskResult = ''
self.cmd_status.nextTask = '216385'
self.cmd_status.allActions = ['296881', '296898', '297511']
self.cmd_status.indexCurrent = 2
self.cmd_status.actionResults = ['', 'failed']
self.cmd_status.batteryCapacity = 0.3
self.cmd_status.batteryVoltage = 0.0
self.cmd_status.listen = False
service_call = WebServiceProxyMsg()
service_call.command = "object"
self._response = self.client_website_proxy.call(service_call.command)
def on_exit(self, userdata):
Logger.loginfo("exit started website_dummy_state1!")
def on_start(self):
Logger.loginfo("started website_dummy_state1 ready!")
def on_stop(self):
Logger.loginfo("started website_dummy_state1 STOPPED!")
def scan_callback(self, data):
self.data = data
