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))
def test_publish_subscribe(self):
t1 = '/pubsub_1'
t2 = '/pubsub_2'
pub = ProxyPublisher({t1: String})
pub = ProxyPublisher({t2: String}, _latch=True)
sub = ProxySubscriberCached({t1: String})
self.assertTrue(pub.is_available(t1))
self.assertTrue(pub.wait_for_any(t1))
self.assertFalse(pub.wait_for_any(t2))
pub.publish(t1, String('1'))
pub.publish(t2, String('2'))
rospy.sleep(
.5) # make sure latched message is sent before subscriber is added
sub = ProxySubscriberCached({t2: String})
rospy.sleep(
.5) # make sure latched message can be received before checking
self.assertTrue(sub.has_msg(t1))
self.assertEqual(sub.get_last_msg(t1).data, '1')
sub.remove_last_msg(t1)
self.assertFalse(sub.has_msg(t1))
self.assertIsNone(sub.get_last_msg(t1))
self.assertTrue(sub.has_msg(t2))
self.assertEqual(sub.get_last_msg(t2).data, '2')
class GenericSub(EventState):
'''
GenericPubSub state subscribes to a given topic (Int32 message only) and will not move forward until a response is recieved.
This is meant to be a template to be modified to your needs it is not super useful in its current
state. Intended mostly for communicating with arduino
'''
def __init__(self, subtopic):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(GenericSub, self).__init__(outcomes=["completed", "failed"])
# Store state parameters for later use.
self._subtopic = subtopic
self._sub = ProxySubscriberCached({self._subtopic: Int32})
#rospy.init_node('reset_control', anonymous=True)
def execute(self, userdata):
#publish to arduino
#while(1):
if self._sub.has_msg(self._subtopic):
msg = self._sub.get_last_msg(self._subtopic)
print(msg)
# in case you want to make sure the same message is not processed twice:
self._sub.remove_last_msg(self._subtopic)
return "completed"
class ContinueButton(EventState):
'''
Reads on a topic to see for the continue button.
<= done Continue if button pressed.
'''
def __init__(self):
'''
Constructor
'''
super(ContinueButton, self).__init__(outcomes=['true', 'false'])
self._topic = "/continue_button"
self._connected = False
self._sub = ProxySubscriberCached({self._topic: Bool})
def execute(self, userdata):
Logger.loginfo('Waiting for the continue button')
if self._sub.has_msg(self._topic):
message = self._sub.get_last_msg(self._topic)
self._sub.remove_last_msg(self._topic)
if message.data:
return 'true'
else:
return 'false'
class DetectPersonState(EventState):
"""
Detects the nearest person and provides their pose.
-- wait_timeout float Time (seconds) to wait for a person before giving up.
#> person_pose PoseStamped Pose of the nearest person if one is detected, else None.
<= detected Detected a person.
<= not_detected No person detected, but time ran out.
"""
def __init__(self, wait_timeout):
super(DetectPersonState, self).__init__(outcomes=["detected", "not_detected"], output_keys=["person_pose"])
self._wait_timeout = rospy.Duration(wait_timeout)
self._topic = "/people_tracker/pose"
self._sub = ProxySubscriberCached({self._topic: PoseStamped})
self._start_waiting_time = None
def execute(self, userdata):
if rospy.Time.now() > self._start_waiting_time + self._wait_timeout:
userdata.person_pose = None
return "not_detected"
if self._sub.has_msg(self._topic):
userdata.person_pose = self._sub.get_last_msg(self._topic)
return "detected"
def on_enter(self, userdata):
self._sub.remove_last_msg(self._topic)
self._start_waiting_time = rospy.Time.now()
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 GetClosestEntity(EventState):
'''
return the closest entity from the list in parameter. If no list in parameter, return the closest entity from the topic /entities
># entityList object[] List of entities.
#> entityFound object The found entity.
<= done Indicates completion of the extraction.
<= not_found Indicates completion of the extraction.
'''
def __init__(self):
'''Constructor'''
super(GetClosestEntity, self).__init__(outcomes=['done', 'not_found'],
input_keys=['entityList'],
output_keys=['output_entity'])
self._sub = ProxySubscriberCached({'/entities': Entities})
self._subpos = ProxySubscriberCached({'/robot_pose': Pose})
self.mypose = None
def dist(self, entity1, entity2):
x = entity1.position.x - entity2.position.x
y = entity1.position.y - entity2.position.y
return math.sqrt(x * x + y * y)
def execute(self, userdata):
'''Execute this state'''
if self._subpos.has_msg('/robot_pose'):
self.mypose = self._subpos.get_last_msg('/robot_pose')
if len(userdata.entityList) == 0:
Logger.loginfo('va chercher la liste entities')
if self._sub.has_msg('/entities'):
myList = self._sub.get_last_msg('/entities')
self._sub.remove_last_msg('/entities')
myList = myList.entities
Logger.loginfo('a la liste entities')
else:
myList = userdata.entityList
Logger.loginfo('a la liste en parametre')
if len(myList) == 0:
Logger.loginfo('liste vide')
return "not_found"
min_dist = 99999
closestEntity = myList[0]
for entity in myList:
if self.dist(self.mypose, entity) < min_dist:
min_dist = self.dist(self.mypose, entity)
closestEntity = entity
userdata.output_entity = closestEntity
return 'done'
class hsr_GetWrench(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.
-- threshold float StartBottun(20.0>0), PutControl(-15.0<0), Ignore(0.0)
<= completed Completed
<= failed Failed
'''
def __init__(self, threshold=-15.0):
super(hsr_GetWrench, self).__init__(outcomes=['completed', 'failed'])
self.threshold = threshold
self._topic = '/hsrb/wrist_wrench/raw'
self.sub_wrench = ProxySubscriberCached({self._topic: WrenchStamped})
def execute(self, userdata):
if self.sub_wrench.has_msg(self._topic):
msg = self.sub_wrench.get_last_msg(self._topic)
self.sub_wrench.remove_last_msg(self._topic)
if self.threshold < 0.0 and msg.wrench.force.x < self.threshold:
print "Move on Gripper Open Phase."
return "completed"
if self.threshold > 0.0 and msg.wrench.force.x > self.threshold:
print "Start the Task."
return "completed"
if self.threshold == 0.0:
print "Ignore the this state"
return "completed"
else:
rospy.loginfo("Cannot subscribe the topic: %s", self._topic)
return "failed"
def on_enter(self, userdata):
pass
#self._sub_wrench = ProxySubscriberCached({self._topic: WrenchStamped})
def on_exit(self, userdata):
pass
def on_start(self):
pass
def on_stop(self):
pass
class Reset_Control_State_GR(EventState):
'''
Reset control takes in the trial information from Test control and on a succesful completion starts
the Data Control. If all trials are completed then it will loop back to Test control. Direction is
used for determining a successful and unsuccseful outcome for testing purposed but will need to be
replaced with a different measure of success once it becomes more fleshed out.
TODO: More complex information for trials, update info
-- direction int TEMPORARY: Determines a succesful (1) or unsuccesful (0) outcome for testing purposes
># number_of_trials Trial information (currently just an int)
<= continue All actions completed
<= failed Trial control failed to initialize or call something TODO: Proper error checking
<= completed All Trials have been succesfully completed, go back to Test control
'''
def __init__(self):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(Reset_Control_State_GR, self).__init__(outcomes = ["continue", "failed"], input_keys=["rotation"])
# Store state parameters for later use.
self._rotation = None
self._pub = ProxyPublisher({"reset_start": Int32})
self._sub = ProxySubscriberCached({"reset_complete": Int32})
#rospy.init_node('reset_control', anonymous=True)
def execute(self, userdata):
#publish to arduino
#while(1):
if self._sub.has_msg("reset_complete"):
msg = self._sub.get_last_msg("reset_complete")
print(msg)
# in case you want to make sure the same message is not processed twice:
self._sub.remove_last_msg("reset_complete")
return "continue"
def on_enter(self, userdata):
#self._rotation = 5
#Initializes class variable from userdata, has to be done outside of constructor
if(self._rotation is None and userdata.rotation is not None):
self._rotation = userdata.rotation
self._pub.publish("reset_start", self._rotation)
class hsr_SpeechRecognitionByHeadMic(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.
#> recognition String speech recognition
<= recognition recognition
'''
def __init__(self):
super(hsr_SpeechRecognitionByHeadMic,self).__init__(outcomes=['recognition'],output_keys=['recognition'])
self._topic = "/hsrb/voice/text"
self.Speech2Text_CB = ProxySubscriberCached({self._topic: RecognitionResult})
#self.led_cli = actionlib.SimpleActionClient('em_led_action', ledAction) # LED
self.goal = ledGoal()
p = subprocess.Popen("rosservice call /hsrb/voice/stop_recognition '{}'", shell=True)
def execute(self, userdata):
if self.Speech2Text_CB.has_msg(self._topic):
text = self.Speech2Text_CB.get_last_msg(self._topic)
result = text.sentences
rospy.loginfo('[Subscribe %s] "%s"', self._topic, result[0])
print result[0]
userdata.recognition = result[0]
self.Speech2Text_CB.remove_last_msg(self._topic)
return 'recognition'
def on_enter(self, userdata):
#self.led_cli.wait_for_server()
self.goal.mode = 2
self.goal.color = ColorRGBA(r=0.0, g=1.0, b=0.0, a=0.0)
self.goal.flash_interval = 5
#self.led_cli.send_goal(self.goal)
p = subprocess.Popen("rosservice call /hsrb/voice/start_recognition '{}'", shell=True)
def on_exit(self, userdata):
self.goal.mode = 1
self.goal.color = ColorRGBA(r=0.0, g=0.0, b=1.0, a=0.0)
self.goal.flash_interval = 0
#self.led_cli.send_goal(self.goal)
p = subprocess.Popen("rosservice call /hsrb/voice/stop_recognition '{}'", shell=True)
def on_start(self):
pass
def on_stop(self):
pass
class TurtlebotStatusState(EventState):
"""
The state monitors the Kobuki turtlebot bumper and cliff sensors and will
return an outcome if a one is activated.
-- bumper_topic string The topic that the bumper events are published
-- cliff_topic string The topic that the cliff events are published
<= bumper A bumper was activated
<= cliff The cliff sensor
"""
def __init__(self,
bumper_topic='mobile_base/events/bumper',
cliff_topic='mobile_base/events/cliff'):
super(TurtlebotStatusState,
self).__init__(outcomes=['bumper', 'cliff'])
self._bumper_topic = bumper_topic
self._cliff_topic = cliff_topic
self._bumper_sub = ProxySubscriberCached(
{self._bumper_topic: BumperEvent})
self._cliff_sub = ProxySubscriberCached(
{self._cliff_topic: CliffEvent})
self._return = None # Handle return in case outcome is blocked by low autonomy
def execute(self, userdata):
if self._bumper_sub.has_msg(self._bumper_topic):
sensor = self._bumper_sub.get_last_msg(self._bumper_topic)
self._bumper_sub.remove_last_msg(self._bumper_topic)
if sensor.state > 0:
Logger.logwarn('Bumper %d contact' % (sensor.bumper))
self._return = 'bumper'
if self._cliff_sub.has_msg(self._cliff_topic):
sensor = self._cliff_sub.get_last_msg(self._cliff_topic)
self._cliff_sub.remove_last_msg(self._cliff_topic)
if sensor.state > 0:
Logger.logwarn('Cliff alert')
self._return = 'cliff'
return self._return
def on_enter(self, userdata):
self._return = None # Clear the completion flag
class SpeakState(EventState):
"""
Genie TTS request
"""
def __init__(self):
super(SpeakState, self).__init__(outcomes=['done'],
input_keys=['tts_text'])
self.tts_topic = '/genie_tts'
self.state_topic = '/genie_state'
self.tts_pub = ProxyPublisher({self.tts_topic: String})
self.state_sub = ProxySubscriberCached({self.state_topic: UInt8})
def execute(self, userdata):
if self.state_sub.has_msg(self.state_topic):
# get genie state
state = self.state_sub.get_last_msg(self.state_topic)
self.state_sub.remove_last_msg(self.state_topic)
# non-speaking state
if state.data == 0:
return 'done'
def on_enter(self, userdata):
tts_text = String()
tts_text.data = userdata.tts_text
check_rate = rospy.Rate(10)
while self.state_sub.has_msg(self.state_topic):
# get genie state
state = self.state_sub.get_last_msg(self.state_topic)
self.state_sub.remove_last_msg(self.state_topic)
# non-speaking state
if state.data == 0:
break
check_rate.sleep()
# send tts request
self.tts_pub.publish(self.tts_topic, tts_text)
class hsr_SpeechRecognition(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.
-- topic string topic
#> recognition String speech recognition
<= recognition recognition
'''
def __init__(self, topic):
super(hsr_SpeechRecognition,
self).__init__(outcomes=['recognition'],
output_keys=['recognition'])
self._topic = topic
self.Speech2Text_CB = ProxySubscriberCached({self._topic: String})
# self.led_cli = actionlib.SimpleActionClient('em_led_action', ledAction)
# self.goal = ledGoal()
def execute(self, userdata):
if self.Speech2Text_CB.has_msg(self._topic):
text = self.Speech2Text_CB.get_last_msg(self._topic)
rospy.loginfo('[Subscribe %s] "%s"', self._topic, text.data)
userdata.recognition = text.data
self.Speech2Text_CB.remove_last_msg(self._topic)
return 'recognition'
def on_enter(self, userdata):
pass
# self.led_cli.wait_for_server()
# self.goal.mode = 2
# self.goal.color = ColorRGBA(r=0.0, g=1.0, b=0.0, a=0.0)
# self.goal.flash_interval = 5
# self.led_cli.send_goal(self.goal)
def on_exit(self, userdata):
pass
def on_start(self):
pass
def on_stop(self):
pass
class DetectPersonState(EventState):
'''
Detects the nearest person and provides their pose.
-- wait_timeout float Time (seconds) to wait for a person before giving up.
#> person_pose PoseStamped Pose of the nearest person if one is detected, else None.
<= detected Detected a person.
<= not_detected No person detected, but time ran out.
'''
def __init__(self, wait_timeout):
super(DetectPersonState, self).__init__(outcomes = ['detected', 'not_detected'],
output_keys = ['person_pose'])
self._wait_timeout = rospy.Duration(wait_timeout)
self._topic = '/people_tracker/pose'
self._sub = ProxySubscriberCached({self._topic: PoseStamped})
self._start_waiting_time = None
def execute(self, userdata):
if rospy.Time.now() > self._start_waiting_time + self._wait_timeout:
userdata.person_pose = None
print 'detect_person_state: did not detect any person'
return 'not_detected'
if self._sub.has_msg(self._topic):
userdata.person_pose = self._sub.get_last_msg(self._topic)
print 'detect_person_state: detected a person'
return 'detected'
def on_enter(self, userdata):
print 'detect_person_state: trying to detect a person'
self._sub.remove_last_msg(self._topic)
self._start_waiting_time = rospy.Time.now()
class DetectPersonState(EventState):
'''
Detects the nearest person and provides their pose.
-- wait_timeout float Time (seconds) to wait for a person before giving up.
#> person_pose PoseStamped Pose of the nearest person if one is detected, else None.
<= detected Detected a person.
<= not_detected No person detected, but time ran out.
'''
def __init__(self, wait_timeout):
super(DetectPersonState, self).__init__(outcomes = ['detected', 'not_detected'],
output_keys = ['person_pose'])
self._wait_timeout = rospy.Duration(wait_timeout)
self._topic = '/people_tracker/pose'
self._sub = ProxySubscriberCached({self._topic: PoseStamped})
self._start_waiting_time = None
def execute(self, userdata):
if rospy.Time.now() > self._start_waiting_time + self._wait_timeout:
userdata.person_pose = None
return 'not_detected'
if self._sub.has_msg(self._topic):
userdata.person_pose = self._sub.get_last_msg(self._topic)
return 'detected'
def on_enter(self, userdata):
self._sub.remove_last_msg(self._topic)
self._start_waiting_time = rospy.Time.now()
class Arm_Control_State_GR(EventState):
'''
Arm control takes in the trial information from Test control and on a succesful completion starts
the reset.
TODO: More complex information and better ways to call things also better description.
-- direction int TEMPORARY: Determines a succesful (1) or unsuccesful (0) outcome for testing purposes
># number_of_trials Trial information (currently just an int)
<= continue All actions completed
<= failed Trial control failed to initialize or call something TODO: Proper error checking
<= completed All Trials have been succesfully completed, go back to Test control
'''
def __init__(self):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(Arm_Control_State_GR, self).__init__(outcomes = ["continue", "failed"])
self._pub = ProxyPublisher({"sim2real/reset_status": Int32})
self._sub = ProxySubscriberCached({"sim2real/reset": Int32})
#rospy.init_node('reset_control', anonymous=True)
def execute(self, userdata):
if self._sub.has_msg("sim2real/reset"):
msg = self._sub.get_last_msg("sim2real/reset")
self._sub.remove_last_msg("sim2real/reset")
if(msg.data == 1):
return "continue"
def on_enter(self, userdata):
self._pub.publish("sim2real/reset_status", 1)
class TestSubState(EventState):
def __init__(self, topic):
'''Constructor'''
super(TestSubState,
self).__init__(outcomes=['received', 'unavailable'],
output_keys=['result'])
self._topic = topic
self._sub = ProxySubscriberCached({self._topic: String})
self._msg_counter = 0
self._timeout = rospy.Time.now() + rospy.Duration(1.5)
def execute(self, userdata):
if self._msg_counter == 0 and rospy.Time.now() > self._timeout:
return 'unavailable'
if self._sub.has_msg(self._topic):
msg = self._sub.get_last_msg(self._topic)
self._sub.remove_last_msg(self._topic)
userdata.result = msg.data
self._msg_counter += 1
if self._msg_counter == 3:
return 'received'
class GetEntityByID(EventState):
'''
Search for an entity by it's ID number
># ID int The ID of the entity
#> Entity object The entity found
<= found the entity has been found
<= not_found noting has been found
'''
def __init__(self):
'''
Constructor
'''
super(GetEntityByID, self).__init__(outcomes=['found', 'not_found'],
input_keys=['ID'],
output_keys=['Entity'])
self._sub = ProxySubscriberCached({'/entities': Entities})
self.message = None
def execute(self, userdata):
if self._sub.has_msg('/entities'):
Logger.loginfo('getting list of entities')
self.message = self._sub.get_last_msg('/entities')
self._sub.remove_last_msg('/entities')
if self.message is not None:
for entity in self.message.entities:
if entity.ID == userdata.ID:
userdata.Entity = entity
return 'found'
return 'not_found'
class GetPoseState(EventState):
"""
Grabs the most recent published PoseStamped.
-- topic String The topic to subscribe to
#> goal PointStamped The goal.
<= done Goal PoseStamped is available.
"""
def __init__(self, topic = 'move_base_simple/goal'):
"""Constructor"""
super(GetPoseState, self).__init__(outcomes=['done'], output_keys=['goal'])
self._topic = topic
self._sub = ProxySubscriberCached({self._topic: PoseStamped})
self._goal_pose = None
def execute(self, userdata):
if self._sub.has_msg(self._topic):
Logger.loginfo('%s Received new goal' % (self.name))
self._goal_pose = self._sub.get_last_msg(self._topic)
self._sub.remove_last_msg(self._topic)
if self._goal_pose is not None:
userdata.goal = self._goal_pose
return 'done'
def on_enter(self, userdata):
userdata.goal = None
self._goal_pose = None
if self._sub.has_msg(self._topic):
Logger.loginfo('%s Clearing prior goal' % (self.name))
self._sub.remove_last_msg(self._topic)
Logger.loghint('%s Input new 2D Nav goal (e.g. via RViz) ' % (self.name))
class GenericPubSub(EventState):
'''
GenericPubSub state publishes an integer (Int32) once to the topic given. It also subscribes
to a given topic (Int32 message only) and will not move forward until a response is recieved.
This is meant to be a template to be modified to your needs it is not super useful in its current
state.
'''
def __init__(self, pubtopic, subtopic, pubint):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(GenericPubSub, self).__init__(outcomes=["completed", "failed"])
# Store state parameters for later use.
self._rotation = None
self._pubtopic = pubtopic
self._subtopic = subtopic
self._pubint = pubint
self._pub = ProxyPublisher({self._pubtopic: Int32})
self._sub = ProxySubscriberCached({self._subtopic: Int32})
#rospy.init_node('reset_control', anonymous=True)
def execute(self, userdata):
#publish to arduino
#while(1):
if self._sub.has_msg(self._subtopic):
msg = self._sub.get_last_msg(self._subtopic)
print(msg)
# in case you want to make sure the same message is not processed twice:
self._sub.remove_last_msg(self._subtopic)
return "completed"
def on_enter(self, userdata):
#self._rotation = 5
#Initializes class variable from userdata, has to be done outside of constructor
self._pub.publish(self._pubtopic, self._pubint)
class TakeImageCamera(EventState):
'''
Return the image from the topic.
>= topic String the topic
<= image Image the image
<= received the image is receivec
<= unavailable can't subscribe
'''
def __init__(self, topic):
'''
Constructor
'''
super(TakeImageCamera,
self).__init__(outcomes=['received', 'unavailable'],
output_keys=['image'])
self._sub = ProxySubscriberCached({topic: Image})
self.topic = topic
self.message = None
def execute(self, userdata):
if self._sub.has_msg(self.topic):
Logger.loginfo('Has an image')
self.message = self._sub.get_last_msg(self.topic)
self._sub.remove_last_msg(self.topic)
if self.message is not None:
userdata.image = self.message
return 'received'
return 'unavailable'
class TestSubState(EventState):
def __init__(self, topic):
'''Constructor'''
super(TestSubState, self).__init__(outcomes=['received', 'unavailable'],
output_keys=['result'])
self._topic = topic
self._sub = ProxySubscriberCached({self._topic: String})
self._msg_counter = 0
self._timeout = rospy.Time.now() + rospy.Duration(1.5)
def execute(self, userdata):
if self._msg_counter == 0 and rospy.Time.now() > self._timeout:
return 'unavailable'
if self._sub.has_msg(self._topic):
msg = self._sub.get_last_msg(self._topic)
self._sub.remove_last_msg(self._topic)
userdata.result = msg.data
self._msg_counter += 1
if self._msg_counter == 3:
return 'received'
class GetCurrentJointValuesListState(EventState):
'''
Retrieves current values of specified joints.
Joints are specified by parameter list.
-- joint_names string[] List of desired joint names.
-- timeout double Timeout value (optional)
-- joint_states_topic string Optional name of joint states topic
(default: /joint_states)
#> joint_names string[] List of current joint names.
#> joint_values float[] List of current joint values.
<= retrieved Joint values are available.
<= timeout Joint values are not available.
'''
def __init__(self,
joint_names,
timeout=None,
joint_states_topic='/joint_states'):
'''
Constructor
'''
super(GetCurrentJointValuesListState,
self).__init__(outcomes=['retrieved', 'timeout'],
output_keys=['joint_names', 'joint_values'])
self.topic = joint_states_topic
self.sub = ProxySubscriberCached({self.topic: JointState})
self.joint_names = joint_names
self.joint_values = list()
if (timeout is not None) and (timeout > 0.0):
self.timeout_duration = rospy.Duration(timeout)
else:
self.timeout_duration = None
def execute(self, userdata):
if (self.return_code is not None):
# Handle blocked transition or error during on_enter
userdata.joint_names = self.joint_names
userdata.joint_values = self.joint_values
return self.return_code
while self.sub.has_buffered(self.topic):
msg = self.sub.get_from_buffer(self.topic)
for i in range(len(msg.name)):
if msg.name[i] in self.joint_names \
and self.joint_values[self.joint_names.index(msg.name[i])] is None:
self.joint_values[self.joint_names.index(
msg.name[i])] = msg.position[i]
if all(v is not None for v in self.joint_values):
userdata.joint_names = self.joint_names
userdata.joint_values = self.joint_values
self.return_code = 'retrieved'
return 'retrieved'
if (self.timeout_duration is not None and \
(rospy.Time.now()-self.start_time) > self.timeout_duration ):
Logger.logerr(
'Timeout %s - found %s of %s' %
(self.name, str(self.joint_values), str(self.joint_names)))
self.return_code = 'timeout'
return 'timeout'
def on_enter(self, userdata):
self.sub.enable_buffer(self.topic)
self.sub.remove_last_msg(self.topic, True)
self.joint_values = [None] * len(self.joint_names)
self.return_code = None
self.start_time = rospy.Time.now()
userdata.joint_names = self.joint_names
# Clear user data until we have valid data
userdata.joint_values = None
def on_exit(self, userdata):
self.sub.disable_buffer(self.topic)
class hsr_SpaCoTyGetDatasetPlus(EventState):
'''
This state is to detect bounding box and grasping points for objects in image. Output is detect result.
-- output_topic string output_topic is "/bounding_box_2d_monitor"
-- rgb_topic string rgb_topic is "/hsrb/head_rgbd_sensor/rgb/image_rect_color"
-- depth_topic string depth_topic is "/hsrb/head_rgbd_sensor/depth_registered/image_rect_raw"
-- camera_info_topic string camera_info_topic is "/hsrb/head_rgbd_sensor/rgb/camera_info"
-- yolo_yaml string load the yaml file for getting labels(default:yolov3.yaml)
-- resnet_model string select resnet model (default: resnet50)
-- save_image bool option saving image(default:True)
-- z_offset float offset about z coordinate (default:0.1 [m])
-- MEMO Please push "/spacoty/get_dataset"(False), if u wanna finished
># save_folder string save the dataset to save_folder directory(default:default)
<= completed transition leading to completion
<= failed transition leading to failure
'''
def __init__(
self,
output_topic="/bounding_box_2d_monitor",
rgb_topic="/hsrb/head_rgbd_sensor/rgb/image_rect_color",
depth_topic="/hsrb/head_rgbd_sensor/depth_registered/image_rect_raw",
camera_info_topic="/hsrb/head_rgbd_sensor/rgb/camera_info",
yolo_yaml="yolov3.yaml",
resnet_model="resnet50",
save_image=True,
z_offset=0.1,
MEMO="/spacoty/get_dataset"):
super(hsr_SpaCoTyGetDatasetPlus,
self).__init__(outcomes=['completed', 'failed'],
input_keys=['save_folder'])
# Create the action client when building the behavior.
# This will cause the behavior to wait for the client before starting execution
# and will trigger a timeout error if it is not available.
# Using the proxy client provides asynchronous access to the result and status
# and makes sure only one client is used, no matter how often this state is used in a behavior.
self._topic = {
"img": "bounding_box_2d",
"sw": "/spacoty/get_dataset",
"wrd": "/spacoty/place_name"
}
self._output_topic = output_topic
self._depth_topic = depth_topic
self._rgb_topic = rgb_topic
self._camera_info_topic = camera_info_topic
self._save_folder = "/root/HSR/catkin_ws/src/em_spco_tidy_up/training_data/"
self._save_image = save_image
self.camera_info = None
self.rgb_image = None
self.depth_image = None
self.z_offset = z_offset
# get object labels
# self._yolo_yaml_path = "/root/HSR/catkin_ws/src/hsr_launch/config/darknet_ros/"+yolo_yaml
# obj_yaml = rosparam.load_file(self._yolo_yaml_path)
# self.obj_class = obj_yaml[0][0]["yolo_model"]["detection_classes"]["names"]
self._yolo_yaml_path = "/root/HSR/catkin_ws/src/cv_detect_object/scripts/" + yolo_yaml
obj_yaml = rosparam.load_file(self._yolo_yaml_path)
self.obj_class = obj_yaml[0][0].values()
# select ResNet
if resnet_model[-2:] == '50':
self.resnet = chainer.links.ResNet50Layers()
elif resnet_model[-3:] == '101':
self.resnet = chainer.links.ResNet101Layers()
elif resnet_model[-3:] == '152':
self.resnet = chainer.links.ResNet152Layers()
else:
print(
"[ERROR] You must choice \'ResNet-50\', \'ResNet-101\' or \'ResNet-152\'."
)
exit(1)
# pass required clients as dict (topic: type)
self._client = ProxyActionClient(
{self._topic["img"]: hsr_common.msg.BoundingBox2DAction})
self.bridge = cv_bridge.CvBridge()
# Subscriber
self.sub_sw = ProxySubscriberCached({self._topic["sw"]: Bool})
self.sub_word = ProxySubscriberCached({self._topic["wrd"]: String})
# define the dimension
self._data_dim = {"pose": 3, "object": 2048, "word": 0}
# It may happen that the action client fails to send the action goal.
self._error = False
self._get_image_error = False
def execute(self, userdata):
# While this state is active, check if the action has been finished and evaluate the result.
if self.sub_sw.has_msg(self._topic["sw"]):
Logger.loginfo('Get the Switcher.')
self.finish_sw = self.sub_sw.get_last_msg(self._topic["sw"])
self.finish_sw = self.finish_sw.data
self.sub_sw.remove_last_msg(self._topic["sw"])
# Get image
#timeout = 1.0
rgb_msg = rospy.wait_for_message(
self._rgb_topic,
sensor_msgs.msg.Image #, timeout = timeout
)
depth_msg = rospy.wait_for_message(
self._depth_topic,
sensor_msgs.msg.Image #, timeout = timeout
)
camera_info_msg = rospy.wait_for_message(
self._camera_info_topic,
sensor_msgs.msg.CameraInfo #, timeout = timeout
)
self.rgb_image = self.bridge.imgmsg_to_cv2(rgb_msg, "bgr8")
self.depth_image = self.bridge.imgmsg_to_cv2(depth_msg, "16UC1")
self.camera_info = camera_info_msg
# Logger.loginfo("Got the image...")
if self.finish_sw == True and self._ignore == False:
# Create the goal.
# Logger.loginfo("Waiting image...")
# if self.rgb_image is not None:
# Logger.loginfo("Get rgb image!")
# sys.stdout.flush()
# else:
# Logger.logwarn(
# 'Failed to get RGB image!')
# self._get_image_error = True
# if self.depth_image is not None:
# Logger.loginfo("Get depth image!")
# sys.stdout.flush()
# else:
# Logger.logwarn(
# 'Failed to get Depth image!')
# self._get_image_error = True
# if self.camera_info is not None:
# Logger.loginfo("Get camera info!")
# sys.stdout.flush()
# else:
# Logger.logwarn(
# 'Failed to get Camera info!')
# self._get_image_error = True
self.start = timer()
goal = hsr_common.msg.BoundingBox2DGoal()
goal.image = self.bridge.cv2_to_imgmsg(self.rgb_image, "bgr8")
# Send the goal.
# make sure to reset the error state since a previous state execution might have failed
try:
self._client.send_goal(self._topic["img"], goal)
except Exception as e:
# Since a state failure not necessarily causes a behavior failure, it is recommended to only print warnings, not errors.
# Using a linebreak before appending the error log enables the operator to collapse details in the GUI.
Logger.logwarn(
'Failed to send the BoundingBox2D command:\n%s' % str(e))
self._error = True
# Check if the client failed to send the goal.
if self._get_image_error:
return 'failed'
elif self._error:
return 'failed'
# Check if the action has been finished
rospy.sleep(0.2)
if self._client.has_result(self._topic["img"]):
# Logger.loginfo('Success for getting the result of YOLO detecttion.')
result = self._client.get_result(self._topic["img"])
self.end = timer()
self.detect_sw = True
else:
self.detect_sw = False
if self.detect_sw == True:
# In this example, we also provide the amount of cleaned dishes as output key.
# userdata.detection = result
self.object_count = 0
rgb_image_init = copy(self.rgb_image)
for n, region in enumerate(result.regions):
hsv = cv2.cvtColor(
np.uint8([[[120 * (1 - result.scores[n]), 255, 255]]]),
cv2.COLOR_HSV2BGR)[0][0]
x0 = region.x_offset
y0 = region.y_offset
x1 = region.x_offset + region.width - 1
y1 = region.y_offset + region.height - 1
# save the object features
dst_im = rgb_image_init[int(y0):int(y0 + region.height),
int(x0):int(x0 + region.width)]
cv2.rectangle(self.rgb_image, (x0, y0), (x1, y1),
(int(hsv[0]), int(hsv[1]), int(hsv[2])), 2)
label_str = '%.2f: %s' % (result.scores[n],
result.names[n])
text_config = {
'text': label_str,
'fontFace': cv2.FONT_HERSHEY_PLAIN,
'fontScale': 1,
'thickness': 1,
}
size, baseline = cv2.getTextSize(**text_config)
cv2.rectangle(self.rgb_image, (x0, y0),
(x0 + size[0], y0 + size[1]),
(255, 255, 255), cv2.FILLED)
cv2.putText(self.rgb_image,
org=(x0, y0 + size[1]),
color=(255, 0, 0),
**text_config)
combinations = [
(x, y)
for x in range(int(x0 + region.width * 0.45 -
1), int(x0 + region.width * 0.55 -
1))
for y in range(int(y0 + region.height * 0.45 - 1),
int(y0 + region.height * 0.55 - 1))
]
grasping_z = 0
pixel_count = 0
for x, y in combinations:
if (self.depth_image.item(y, x)
== 0) or (self.depth_image.item(y, x)
== 66535):
pass
else:
grasping_z = grasping_z + self.depth_image.item(
y, x)
pixel_count = pixel_count + 1
if pixel_count != 0:
grasping_z = grasping_z / pixel_count
cx, cy, cz = self.get_direction(
self.camera_info, (x0 + x1) / 2, (y0 + y1) / 2)
pose = Pose()
pose.position.x = cx * grasping_z / 1000.0
pose.position.y = cy * grasping_z / 1000.0
pose.position.z = (cz * grasping_z / 1000.0)
pose.orientation.x = 0.0
pose.orientation.y = 0.0
pose.orientation.z = 0.0
pose.orientation.w = 0.0
try:
# transform and save the object poses
trans_pose = self.tf2_trans(pose)
trans_pose.position.z += self.z_offset
self.pose_data = np.append(self.pose_data, [
trans_pose.position.x, trans_pose.position.y,
trans_pose.position.z
])
# save the object features
dst_im = cv2.resize(dst_im, (224, 224),
interpolation=cv2.INTER_CUBIC)
dst_im_conv = chainer.links.model.vision.resnet.prepare(
dst_im)
dst_im_conv = dst_im_conv[np.newaxis]
resnet_vec = self.resnet.extract(dst_im_conv)
self.object_data = np.append(
self.object_data, resnet_vec['pool5'].data[0])
# count detection of object for word information
self.object_count += 1
Logger.loginfo(
"[Successfully]:Collect the information.")
except (tf2_ros.LookupException,
tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
Logger.loginfo(
"[ERROR_tf2_time]:ignore the information. (obj:%s, id:%d)"
% (result.names[n], len(self.pose_data)))
self.tf_buffer = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(
self.tf_buffer)
# Draw the region of using depth information
cv2.rectangle(self.rgb_image,
(int(x0 + region.width * 0.45 - 1),
int(y0 + region.height * 0.45 - 1)),
(int(x0 + region.width * 0.55 - 1),
int(y0 + region.height * 0.55 - 1)),
(grasping_z / 10000 * 255, grasping_z /
10000 * 255, grasping_z / 10000 * 255),
cv2.FILLED)
#cv2.imshow("Image window", cv_image)
# cv2.waitKey(3)
# Get the place names
pre_word_data = []
if self.sub_word.has_msg(self._topic["wrd"]):
msg = self.sub_word.get_last_msg(self._topic["wrd"])
self.sub_word.remove_last_msg(self._topic["wrd"])
pre_word_data = [0 for i in xrange(self._data_dim["word"])]
pre_word_data[self.word_list.index(msg.data)] += 1
for m in xrange(self.object_count):
self.word_data = np.append(self.word_data,
pre_word_data)
else:
pre_word_data = np.zeros(self._data_dim["word"] *
self.object_count)
self.word_data = np.append(self.word_data, pre_word_data)
try:
self.image_pub.publish(
self.bridge.cv2_to_imgmsg(self.rgb_image, "bgr8"))
if self._save_image:
img_name = self._save_folder + '/image/detect' + str(
self.save_image_count) + '.png'
cv2.imwrite(img_name, self.rgb_image)
Logger.loginfo('Save the image: %s' % (img_name))
self.save_image_count += 1
except cv_bridge.CvBridgeError as e:
Logger.logwarn(str(e))
# Based on the result, decide which outcome to trigger.
if not result is None:
Logger.loginfo('Finished detection. (%f [sec])' %
(self.end - self.start, ))
else:
return 'failed'
# If the action has not yet finished, no outcome will be returned and the state stays active.
elif self.finish_sw == False and self._ignore == False:
Logger.loginfo('Finished to get dataset.')
re_pose = self.pose_data.reshape(
len(self.pose_data) / self._data_dim["pose"],
self._data_dim["pose"])
re_object = self.object_data.reshape(
len(self.object_data) / self._data_dim["object"],
self._data_dim["object"])
re_word = self.word_data.reshape(
len(self.word_data) / self._data_dim["word"],
self._data_dim["word"])
np.savetxt(self._save_folder + "/pose.csv", re_pose)
np.savetxt(self._save_folder + "/object.csv", re_object)
np.savetxt(self._save_folder + "/word.csv", re_word)
Logger.loginfo("Save the dataset to %s" % (self._save_folder))
return 'completed'
self._ignore = False
def on_enter(self, userdata):
# When entering this state, we send the action goal once to let the robot start its work.
# As documented above, we get the specification of which dishwasher to use as input key.
# This enables a previous state to make this decision during runtime and provide the ID as its own output key.
self.image_pub = rospy.Publisher(self._output_topic,
sensor_msgs.msg.Image,
latch=True,
queue_size=10)
self._save_folder = self._save_folder + userdata.save_folder
if os.path.exists(self._save_folder) == True:
subprocess.Popen("rm -rf " + self._save_folder, shell=True)
subprocess.Popen("mkdir -p " + self._save_folder + "/image",
shell=True)
subprocess.Popen("cp " +
self._save_folder[:-len(userdata.save_folder)] +
"word_list_st" + userdata.save_folder[-1:] + ".csv " +
self._save_folder,
shell=True)
subprocess.Popen("mv " + self._save_folder + "/word_list_st" +
userdata.save_folder[-1:] + ".csv " +
self._save_folder + "/word_list.csv",
shell=True)
time.sleep(0.1)
self.pose_data = np.array([])
self.word_data = np.array([])
self.object_data = np.array([])
self.word_list = np.loadtxt(self._save_folder + "/word_list.csv",
dtype="unicode",
delimiter="\n")
self._data_dim.update({"word": len(self.word_list)})
self.save_image_count = 0
self.finish_sw = True
self.detect_sw = False
self._ignore = True
self.tf_buffer = tf2_ros.Buffer()
def on_exit(self, userdata):
# Make sure that the action is not running when leaving this state.
# A situation where the action would still be active is for example when the operator manually triggers an outcome.
if not self._client.has_result(self._topic["img"]):
self._client.cancel(self._topic["img"])
Logger.loginfo('Cancelled active action goal.')
def tf2_trans(self, before_pose):
# tf_buffer = tf2_ros.Buffer()
trans = self.tf_buffer.lookup_transform(
"map", "head_rgbd_sensor_rgb_frame", rospy.Time(0),
rospy.Duration(1.0)
) #rospy.Time(0), rospy.Duration(1.0)) #rospy.Time.now()) rospy.get_rostime()
pose = PoseStamped()
pose.pose = before_pose
transformed = tf2_geometry_msgs.do_transform_pose(pose, trans)
return transformed.pose
def get_direction(self, camera_info, px, py):
camera_model = PinholeCameraModel()
camera_model.fromCameraInfo(camera_info)
cx, cy, cz = camera_model.projectPixelTo3dRay((px, py))
return cx, cy, cz
class GoFowardState(EventState):
'''
Driving state for a Robco19. This state allows the robot to drive forward a certain distance
at a specified velocity/ speed with colision avoicance. Laser scan for obstacle detectionon on /scan_filtered topic.
-- speed float Speed at which to drive the robot
-- travel_dist float How far to drive the robot before leaving this state
-- obstacle_dist float Distance at which to determine blockage of robot path
># remaining_travel_dist_IN float Holds the remaining distance for the robot to travel
#> remaining_travel_dist_OUT float Holds the current distance travelled.
<= failed If behavior is unable to ready on time
<= done Example for a failure outcome.
'''
def __init__(self, speed, travel_dist, obstacle_dist):
#add input and output keys
super(GoFowardState,
self).__init__(outcomes=['failed', 'done'],
input_keys=['remaining_travel_dist_IN'],
output_keys=['remaining_travel_dist_OUT'])
self._start_time = None
#make distance_travelled an Instance variable
self.distance_travelled = 0.0
self.data = None
self._speed = speed
self._travel_dist = travel_dist
self._obstacle_dist = obstacle_dist
self.vel_topic = '/cmd_vel'
self.scan_topic = '/scan_filtered'
#create publisher passing it the vel_topic_name and msg_type
self.pub = ProxyPublisher({self.vel_topic: Twist})
#create subscriber
self.scan_sub = ProxySubscriberCached({self.scan_topic: LaserScan})
def execute(self, userdata):
if not self.cmd_pub:
return 'failed'
#run obstacle checks [index 0: left, 360: middle, 719: right]
if self.scan_sub.has_msg(self.scan_topic):
self.data = self.scan_sub.get_last_msg(self.scan_topic)
self.scan_sub.remove_last_msg(self.scan_topic)
Logger.loginfo('FWD obstacle distance is: %s' %
self.data.ranges[360])
if self.data.ranges[360] <= self._obstacle_dist:
#self.scan_sub.remove_last_msg(self.scan_topic)
self.data = None
#set the output key before transitioning
userdata.remaining_travel_dist_OUT = self._travel_dist - self.distance_travelled
return 'failed'
#measure distance travelled
elapsed_time = (rospy.Time.now() - self._start_time).to_sec()
self.distance_travelled = (elapsed_time) * self._speed
Logger.loginfo("Distance Travelled: %s" % self.distance_travelled)
if self.distance_travelled >= self._travel_dist:
return 'done'
#drive
self.pub.publish(self.vel_topic, self.cmd_pub)
def on_enter(self, userdata):
Logger.loginfo("Drive FWD STARTED!")
#set robot speed here
self.cmd_pub = Twist()
self.cmd_pub.linear.x = self._speed
self.cmd_pub.angular.z = 0.0
#check if the input key is set
if userdata.remaining_travel_dist_IN:
self._travel_dist = userdata.remaining_travel_dist_IN
Logger.loginfo("Remaining Distance to Travel: %sm" %
self._travel_dist)
#start the timer
self._start_time = rospy.Time.now()
def on_exit(self, userdata):
self.cmd_pub.linear.x = 0.0
self.pub.publish(self.vel_topic, self.cmd_pub)
#self.scan_sub.unsubscribe_topic(self.scan_topic)
Logger.loginfo("Drive FWD 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("Drive FWD STOPPED!")
self.cmd_pub.linear.x = 0.0
self.pub.publish(self.vel_topic, self.cmd_pub)
def scan_callback(self, data):
self.data = data
class GetPositionToPlaceOnTable(EventState):
"""
Renvoie la position d'un endroit sur la table pour poser un objet.
### InputKey
># angle msg.ranges
### OutputKey
<= done Angle de l'obstacle
"""
def __init__(self):
'''
Constructor
'''
super(GetPositionToPlaceOnTable,
self).__init__(outcomes=['done', 'not_found'],
input_keys=['distanceFromEdge'],
output_keys=['position'])
self.planeTopic = "/segment_table/plane"
self.planeSub = ProxySubscriberCached({self.planeTopic: PointCloud2})
self.robotposeTopic = "/robot_pose"
self.robotposeSub = ProxySubscriberCached({self.robotposeTopic: Pose})
self.plane = None
def dist(self, point1, point2):
x = point1.x - point2.x
y = point1.y - point2.y
return math.sqrt(x * x + y * y)
def execute(self, userdata):
'''
Execute this state
'''
# Get the latest robot pose
mypose = self.robotposeSub.get_last_msg(self.robotposeTopic)
if self.planeSub.has_msg(self.planeTopic):
Logger.loginfo('getting table point cloud')
self.plane = self.planeSub.get_last_msg(self.planeTopic)
self.planeSub.remove_last_msg(self.planeTopic)
if self.plane is not None and mypose is not None:
gen = point_cloud2.read_points(self.plane)
closest_point = Point()
closest_point.x = 0
closest_point.y = 0
closest_point.z = 0
min_dist = 99999
numberOfPoints = 0
sum_x = 0
sum_y = 0
sum_z = 0
# find the closest point and center
point = Point()
for p in gen:
numberOfPoints = numberOfPoints + 1
point.x = p[0]
point.y = p[1]
point.z = p[2]
sum_x += point.x
sum_y += point.y
sum_z += point.z
if self.dist(mypose.position, point) < min_dist:
min_dist = self.dist(mypose.position, point)
closest_point = point
center = Point()
center.x = sum_x / numberOfPoints
center.y = sum_y / numberOfPoints
center.z = sum_z / numberOfPoints
#find point to return
distanceClosestPointCenter = self.dist(closest_point, center)
if distanceClosestPointCenter < userdata.distanceFromEdge:
userdata.position = center
return "done"
else:
rapportProportionnel = userdata.distanceFromEdge / distanceClosestPointCenter
pointToReturn = Point()
pointToReturn.x = closest_point.x + (
center.x - closest_point.x) * rapportProportionnel
pointToReturn.y = closest_point.y + (
center.y - closest_point.y) * rapportProportionnel
pointToReturn.z = closest_point.z + (
center.z - closest_point.z) * rapportProportionnel
userdata.position = pointToReturn
return "done"
else:
if self.plane is None:
Logger.loginfo('plane is none')
if mypose is None:
Logger.loginfo('pose is none')
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 TwistSubState(EventState):
'''
Gets the latest message on the given topic and stores it to userdata.
-- topic string The topic on which should be listened.
-- blocking bool Blocks until a message is received.
-- clear bool Drops last message on this topic on enter
in order to only handle message received since this state is active.
#> message object Latest message on the given topic of the respective type.
<= received Message has been received and stored in userdata or state is not blocking.
<= unavailable The topic is not available when this state becomes actives.
'''
def __init__(self, topic, blocking=True, clear=False):
'''
Constructor
'''
super(TwistSubState,
self).__init__(outcomes=['received', 'unavailable'],
output_keys=['message'])
self._topic = topic
self._blocking = blocking
self._clear = clear
self._connected = False
self._value = Float32()
(msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic)
if msg_topic == self._topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub = ProxySubscriberCached({self._topic: msg_type})
self._connected = True
else:
Logger.logwarn(
'Topic %s for state %s not yet available.\nFound: %s\nWill try again when entering the state...'
% (self._topic, self.name, str(msg_topic)))
def execute(self, userdata):
'''
Execute this state
'''
if not self._connected:
return 'unavailable'
if self._sub.has_msg(self._topic) or not self._blocking:
userdata.message = self._sub.get_last_msg(self._topic)
self._value = self._sub.get_last_msg(self._topic)
self._sub.remove_last_msg(self._topic)
if (int(value.data) == int(7777.0)): #ball not found in image
return 'unavailable'
return 'received'
def on_enter(self, userdata):
if not self._connected:
(msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic)
if msg_topic == self._topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub = ProxySubscriberCached({self._topic: msg_type})
self._connected = True
Logger.loginfo(
'Successfully subscribed to previously unavailable topic %s'
% self._topic)
else:
Logger.logwarn('Topic %s still not available, giving up.' %
self._topic)
if self._connected and self._clear and self._sub.has_msg(self._topic):
self._sub.remove_last_msg(self._topic)
def _get_msg_from_path(self, msg_path):
msg_import = msg_path.split('/')
msg_module = '%s.msg' % (msg_import[0])
package = __import__(msg_module, fromlist=[msg_module])
clsmembers = inspect.getmembers(
package, lambda member: inspect.isclass(member) and member.
__module__.endswith(msg_import[1]))
return clsmembers[0][1]
class SubscriberState(EventState):
'''
Gets the latest message on the given topic and stores it to userdata.
-- topic string The topic on which should be listened.
-- blocking bool Blocks until a message is received.
-- clear bool Drops last message on this topic on enter
in order to only handle message received since this state is active.
#> message object Latest message on the given topic of the respective type.
<= received Message has been received and stored in userdata or state is not blocking.
<= unavailable The topic is not available when this state becomes actives.
'''
def __init__(self, topic, blocking = True, clear = False):
'''
Constructor
'''
super(SubscriberState, self).__init__(outcomes=['received', 'unavailable'],
output_keys=['message'])
self._topic = topic
self._blocking = blocking
self._clear = clear
self._connected = False
(msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic)
if msg_topic == self._topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub = ProxySubscriberCached({self._topic: msg_type})
self._connected = True
else:
Logger.logwarn('Topic %s for state %s not yet available.\nFound: %s\nWill try again when entering the state...' % (self._topic, self.name, str(msg_topic)))
def execute(self, userdata):
'''
Execute this state
'''
if not self._connected:
return 'unavailable'
if self._sub.has_msg(self._topic) or not self._blocking:
userdata.message = self._sub.get_last_msg(self._topic)
self._sub.remove_last_msg(self._topic)
return 'received'
def on_enter(self, userdata):
if not self._connected:
(msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic)
if msg_topic == self._topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub = ProxySubscriberCached({self._topic: msg_type})
self._connected = True
Logger.loginfo('Successfully subscribed to previously unavailable topic %s' % self._topic)
else:
Logger.logwarn('Topic %s still not available, giving up.' % self._topic)
if self._connected and self._clear and self._sub.has_msg(self._topic):
self._sub.remove_last_msg(self._topic)
def _get_msg_from_path(self, msg_path):
msg_import = msg_path.split('/')
msg_module = '%s.msg' % (msg_import[0])
package = __import__(msg_module, fromlist=[msg_module])
clsmembers = inspect.getmembers(package, lambda member: inspect.isclass(member) and member.__module__.endswith(msg_import[1]))
return clsmembers[0][1]
class WaitForMsgState(EventState):
'''
Check new messages on the given topic if they are satisfying the given function.
Whenever the function returns True the outcome will be 'successed'.
If the function have not been True during the specified time the outcome will be 'failed'.
Only message that are received since this state is active are processed.
-- topic string The topic on which should be listened.
-- wait_time float Amount of time to wait for the function to be valid.
-- function Function that check the comming msgs. The function should return bool value
-- input_keys string[] State machine variables that will are in the input.
-- output_keys string[] State machine variables that will be returned.
-- output_function Function that is called whenever calling function with incoming message was successful.
It should be used in the case you want to store output_key with message
<= successed The function processed a msg with valid result.
<= failed Timeout has been reached before the function was valid.
'''
@staticmethod
def default_output_function(message, userdata):
pass
def __init__(self, topic, wait_time, function, input_keys = [], output_keys = [], output_function = None):
'''
Constructor
'''
super(WaitForMsgState, self).__init__(outcomes=['successed', 'failed'], output_keys = output_keys, input_keys = input_keys)
self._topic = rospy.resolve_name(topic)
self._wait_time = wait_time
self._function = function
self._connected = False
if output_function is None:
self._output_function = WaitForMsgState.default_output_function
else:
self._output_function = output_function
(msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic)
if msg_topic == self._topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub = ProxySubscriberCached({self._topic: msg_type})
self._connected = True
else:
Logger.logwarn('[WaitForMsg]: Topic %s for state %s not yet available.\nFound: %s\nWill try again when entering the state...' % (self._topic, self.name, str(msg_topic)))
def execute(self, userdata):
'''
Execute this state
'''
if not self._connected:
return 'failed'
if self._sub.has_msg(self._topic):
message = self._sub.get_last_msg(self._topic)
self._sub.remove_last_msg(self._topic)
try:
if self._function(message):
self._output_function(message, userdata)
return 'successed'
else:
return
except Exception as e:
Logger.logerr('[WaitForMsg]: Failed to use function. Error is: %s' % str(e))
return 'failed'
if self._wait_time >= 0:
elapsed = rospy.get_rostime() - self._start_time;
if (elapsed.to_sec() > self._wait_time):
Logger.logerr('[WaitForMsg]: Condition was not satisfied in time limit %d s!'% self._wait_time)
return 'failed'
def on_enter(self, userdata):
self._start_time = rospy.get_rostime()
if not self._connected:
(msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic)
if msg_topic == self._topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub = ProxySubscriberCached({self._topic: msg_type})
self._connected = True
Logger.loginfo('[WaitForMsg]: Successfully subscribed to previously unavailable topic %s' % self._topic)
else:
Logger.logwarn('[WaitForMsg]: Topic %s still not available, giving up.' % self._topic)
# removed last msg
if self._connected and self._sub.has_msg(self._topic):
Logger.loginfo('[WaitForMsg]: Waiting for msg from topic %s' % self._topic)
self._sub.remove_last_msg(self._topic)
def _get_msg_from_path(self, msg_path):
msg_import = msg_path.split('/')
msg_module = '%s.msg' % (msg_import[0])
package = __import__(msg_module, fromlist=[msg_module])
clsmembers = inspect.getmembers(package, lambda member: inspect.isclass(member) and member.__module__.endswith(msg_import[1]))
return clsmembers[0][1]
class list_entities_near_point(EventState):
'''
Will list entities near a point, within a radius sorted by distance
-- radius float distance used to screen out farther entities.
#< name string name to compare entities with, can be used to screen found entities by name
#< position Point position used to compare distance, can be Point or Pose
#> found_entities object list of found entities
<= found entities are found
<= none_found no one is found
'''
def __init__(self, radius):
'''
Constructor
'''
super(list_entities_near_point, self).__init__(outcomes=['found', 'none_found'],
output_keys=['entity_list', 'number'],
input_keys=['name', 'position'])
self._sub = ProxySubscriberCached({'/entities': Entities})
self.radius = radius
self.message = None
self.position = None
def execute(self, userdata):
if self._sub.has_msg('/entities'):
Logger.loginfo('getting detected entities')
self.message = self._sub.get_last_msg('/entities')
self._sub.remove_last_msg('/entities')
if type(userdata.position) == Pose:
self.position = userdata.position.position
else:
self.position = userdata.position
if self.message is not None:
found_entities = self.list(userdata.name)
userdata.entity_list = found_entities
userdata.number = len(found_entities)
if len(found_entities) != 0:
return 'found'
else:
return 'none_found'
def list(self, name):
found_entities = []
wraps = []
for entity in self.message.entities:
x = entity.position.x - self.position.x
y = entity.position.y - self.position.y
z = entity.position.z - self.position.z
dist = (x**2 + y**2 + z**2)**0.5
#Logger.loginfo('dist: '+str(dist)+" name: "+entity.name)
if (name == "" or entity.name == name or entity.category == name) and dist < self.radius:
wrap = wrapper()
wrap.init(self.position, entity)
wraps.append(wrap)
wraps.sort(key=wrapper.key)
for wrap in wraps:
found_entities.append(wrap.entity)
return found_entities
class SetGripperState(EventState):
'''
Publishes a command for the gripper.
>= width float desired gripper width in meter.
>= effort float desired gripper effort.
<= object_size float result width.
<# object an object has been gripped
># no_object no object gripped
'''
def __init__(self, width, effort):
'''
Constructor
'''
super(SetGripperState, self).__init__(outcomes=['object', 'no_object'], output_keys=['object_size'])
self.width = width
self.effort = effort
self._connected = False
self._topic = '/sara_gripper_action_controller/gripper_cmd/goal'
self._topic_f = '/sara_gripper_action_controller/gripper_cmd/result'
self._pub = ProxyPublisher({self._topic: GripperCommandActionGoal})
(msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic_f)
if msg_topic == self._topic_f:
msg_type = self._get_msg_from_path(msg_path)
self._sub = ProxySubscriberCached({self._topic_f: msg_type})
self._connected = True
Logger.loginfo('connecting marker state to '+self._topic_f)
else:
Logger.logwarn('Topic %s for state %s not yet available.\nFound: %s\nWill try again when entering the state...' % (self._topic_f, self.name, str(msg_topic)))
def execute(self, userdata):
if not self._connected:
return 'no_object'
if self._sub.has_msg(self._topic_f):
message = self._sub.get_last_msg(self._topic_f)
size = message.result.position
self._sub.remove_last_msg(self._topic_f)
userdata.object_size = size
if abs( size-self.width) > 0.006:
return 'object'
else:
return 'no_object'
def on_enter(self, userdata):
msg = GripperCommandActionGoal()
msg.goal.command.position = self.width
msg.goal.command.max_effort = self.effort
self._pub.publish(self._topic, msg)
if not self._connected:
(msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic_f)
if msg_topic == self._topic_f:
msg_type = self._get_msg_from_path(msg_path)
self._sub = ProxySubscriberCached({self._topic_f: msg_type})
self._connected = True
Logger.loginfo('Successfully subscribed to previously unavailable topic %s' % self._topic_f)
else:
Logger.logwarn('Topic %s still not available, giving up.' % self._topic_f)
self._sub.remove_last_msg(self._topic_f)
def _get_msg_from_path(self, msg_path):
msg_import = msg_path.split('/')
msg_module = '%s.msg' % (msg_import[0])
package = __import__(msg_module, fromlist=[msg_module])
clsmembers = inspect.getmembers(package, lambda member: inspect.isclass(member) and member.__module__.endswith(msg_import[1]))
return clsmembers[0][1]
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 SaraSay(EventState):
"""
Make sara say something
-- sentence string What to say.
Can be a simple String or a lanmda function using input_keys.
(e.g., lambda x: "I found the "x[0] + " on the " + x[1] + "!").
-- input_keys string[] List of available input keys.
-- emotion int Set the facial expression.
(0=no changes, 1=happy, 2=sad, 3=straight mouth, 4=angry, 5=surprise, 6=blink, 7=party)
-- block Bool Should the robot finish it's sentence before continuing?
># input_keys object[] Input(s) to the sentence as a list of userdata.
The individual inputs can be accessed as list elements (see lambda expression example).
<= done what's said is said
"""
def __init__(self, sentence, input_keys=[], emotion=0, block=True):
"""Constructor"""
super(SaraSay, self).__init__(outcomes=['done'], input_keys=input_keys)
if not (isinstance(sentence, types.LambdaType) and sentence.__name__
== "<lambda>") and len(input_keys) is not 0:
raise ValueError(
"In state " + type(self).__name__ + " saying \"" + sentence +
"\", you need to define a lambda function for sentence.")
# Get parameters
self.msg = say()
self.sentence = sentence
self.emotion = UInt8()
self.emotion.data = emotion
self.block = block
# Set topics
self.emotion_topic = "sara_face/Emotion"
self.say_topic = "/say"
self.sayServiceTopic = "/wm_say"
self.emotionPub = ProxyPublisher({self.emotion_topic: UInt8})
# Prepare wm_tts
if self.block:
self.sayClient = ProxyServiceCaller(
{self.sayServiceTopic: say_service})
else:
self.sayPub = ProxyPublisher({self.say_topic: say})
# Initialise the face sub
self.emotionSub = ProxySubscriberCached({self.emotion_topic: UInt8})
self.lastEmotion = None
def execute(self, userdata):
if self.block:
if self.sayClient.is_available(self.sayServiceTopic):
try:
# Call the say service
srvSay = say_serviceRequest()
srvSay.say.sentence = self.msg.sentence
srvSay.say.emotion = self.msg.emotion
self._response = self.sayClient.call(
self.sayServiceTopic, srvSay)
except rospy.ServiceException as exc:
Logger.logwarn("Sara say did not work: \n" + str(exc))
return 'done'
else:
Logger.logwarn(
"wm_tts service is not available. Remaining trials: " +
str(self.maxBlockCount))
# Count the number of trials before continuing on life
self.maxBlockCount -= 1
if self.maxBlockCount <= 0:
return 'done'
else:
return 'done'
def on_enter(self, userdata):
if self.emotion.data is not 0:
# Save the previous emotion so we can place it back later
if self.block and self.emotionSub.has_msg(self.emotion_topic):
self.lastEmotion = self.emotionSub.get_last_msg(
self.emotion_topic)
self.emotionSub.remove_last_msg(self.emotion_topic)
# Set the emotion
self.emotionPub.publish(self.emotion_topic, self.emotion)
# Set the message according to the lambda function if needed.
if isinstance(
self.sentence,
types.LambdaType) and self.sentence.__name__ == "<lambda>":
self.msg.sentence = self.sentence(
map(lambda key: userdata[key],
list(reversed(list(self._input_keys)))))
else:
self.msg.sentence = self.sentence
Logger.loginfo('Sara is saying: "' + str(self.msg.sentence) + '".')
# Say the thing if not blocking
if not self.block:
# publish
self.sayPub.publish(self.say_topic, self.msg)
return "done"
# Set the maximum tries before continuing
self.maxBlockCount = 30
def on_exit(self, userdata):
if self.block and self.emotion.data is not 0:
# Put the original emotion back
if self.lastEmotion:
self.emotionPub.publish(self.emotion_topic, self.lastEmotion)
class 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 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 TestOnboard(unittest.TestCase):
def __init__(self, name):
super(TestOnboard, self).__init__(name)
self.sub = ProxySubscriberCached({
'flexbe/status': BEStatus,
'flexbe/log': BehaviorLog
})
self.rate = rospy.Rate(100)
# make sure that behaviors can be imported
data_folder = os.path.dirname(os.path.realpath(__file__))
sys.path.insert(0, data_folder)
# run onboard and add custom test behaviors to onboard lib
self.onboard = FlexbeOnboard()
self.lib = self.onboard._behavior_lib
self.lib._add_behavior_manifests(data_folder)
def assertStatus(self, expected, timeout):
""" Assert that the expected onboard status is received before the timeout. """
for i in range(int(timeout * 100)):
self.rate.sleep()
if self.sub.has_msg('flexbe/status'):
break
else:
raise AssertionError('Did not receive a status as required.')
msg = self.sub.get_last_msg('flexbe/status')
self.sub.remove_last_msg('flexbe/status')
self.assertEqual(msg.code, expected)
return msg
def test_onboard_behaviors(self):
behavior_pub = rospy.Publisher('flexbe/start_behavior',
BehaviorSelection,
queue_size=1)
rospy.sleep(0.5) # wait for publisher
# wait for the initial status message
self.assertStatus(BEStatus.READY, 1)
# send simple behavior request without checksum
be_id, _ = self.lib.find_behavior("Test Behavior Log")
request = BehaviorSelection()
request.behavior_id = be_id
request.autonomy_level = 255
behavior_pub.publish(request)
self.assertStatus(BEStatus.ERROR, 2)
# send valid simple behavior request
with open(self.lib.get_sourcecode_filepath(be_id)) as f:
request.behavior_checksum = zlib.adler32(f.read())
behavior_pub.publish(request)
self.assertStatus(BEStatus.STARTED, 1)
self.assertStatus(BEStatus.FINISHED, 3)
self.assertIn('data', self.sub.get_last_msg('flexbe/log').text)
# send valid complex behavior request
be_id, _ = self.lib.find_behavior("Test Behavior Complex")
request = BehaviorSelection()
request.behavior_id = be_id
request.autonomy_level = 255
request.arg_keys = ['param']
request.arg_values = ['value_2']
request.input_keys = ['data']
request.input_values = ['2']
with open(self.lib.get_sourcecode_filepath(be_id)) as f:
content = f.read()
modifications = [('flexbe_INVALID', 'flexbe_core'),
('raise ValueError("TODO: Remove!")', '')]
for replace, by in modifications:
index = content.index(replace)
request.modifications.append(
BehaviorModification(index, index + len(replace), by))
for replace, by in modifications:
content = content.replace(replace, by)
request.behavior_checksum = zlib.adler32(content)
behavior_pub.publish(request)
self.assertStatus(BEStatus.STARTED, 1)
result = self.assertStatus(BEStatus.FINISHED, 3)
self.assertEqual(result.args[0], 'finished')
self.assertIn('value_2', self.sub.get_last_msg('flexbe/log').text)
# send the same behavior with different parameters
request.arg_keys = ['param', 'invalid']
request.arg_values = ['value_1', 'should be ignored']
request.input_keys = []
request.input_values = []
behavior_pub.publish(request)
self.assertStatus(BEStatus.STARTED, 1)
result = self.assertStatus(BEStatus.FINISHED, 3)
self.assertEqual(result.args[0], 'failed')
self.assertIn('value_1', self.sub.get_last_msg('flexbe/log').text)
