def __init__(self, hand, force_threshold=1.5):
'''Constructor'''
super(WaitForForceState,
self).__init__(outcomes=['success', 'failure'])
self._action_name = rospy.get_name()
self._prefix = "meka_roscontrol"
self._client = {}
self._movement_finished = {}
self.force_variation = {}
self.force_bias = {}
self.previous_force = {}
self._r = rospy.Rate(100)
self._threshold = force_threshold
# store latest planning scene
self.current_planning_scene = None
self.force_variation['left_arm'] = numpy.array([0.0, 0.0, 0.0])
self.force_variation['right_arm'] = numpy.array([0.0, 0.0, 0.0])
self.force_bias['left_arm'] = numpy.array([0.0, 0.0, 0.0])
self.force_bias['right_arm'] = numpy.array([0.0, 0.0, 0.0])
self.previous_force['left_arm'] = numpy.array([0.0, 0.0, 0.0])
self.previous_force['right_arm'] = numpy.array([0.0, 0.0, 0.0])
self._current_val = 0
self._group = hand + '_arm'
self.sub_left = ProxySubscriberCached(
{'/meka_ros_pub/m3loadx6_ma30_l0/wrench': WrenchStamped})
self.sub_right = ProxySubscriberCached(
{'/meka_ros_pub/m3loadx6_ma29_l0/wrench': WrenchStamped})
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')
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",
save_image=True,
z_offset=0.1,
MEMO="/spacoty/get_dataset"):
super(hsr_SpaCoTyGetDataset,
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()
# 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": len(self.obj_class), "word": 0}
# It may happen that the action client fails to send the action goal.
self._error = False
self._get_image_error = False
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 __init__(self):
# See example_state.py for basic explanations.
super(WonderlandAddUpdatePeople, self).__init__(outcomes=['done'])
self.url = "http://wonderland:8000/api/people/"
self._sub = ProxySubscriberCached({'/entities': Entities})
self._topic = "/robot_pose"
self._subscriber_pos = ProxySubscriberCached({self._topic: Pose})
self.my_pose = None
self.message = None
def __init__(self):
'''
Constructor
'''
super(GetEmptyChair, self).__init__(outcomes=['done', 'nothing_found'],
output_keys=['output_entity'])
self._sub = ProxySubscriberCached({'/entities': Entities})
self._subpos = ProxySubscriberCached({'/robot_pose': Pose})
self.mypose = None
self.message = None
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 __init__(self):
"""
Constructor
"""
super(WaitForOpenDoorState, self).__init__(outcomes=['done'])
self._topic = '/laser/srd_front/scan'
self._sub = ProxySubscriberCached({self._topic: LaserScan})
def __init__(self):
'''Constructor'''
super(GetLaserscanState, self).__init__(outcomes=['done'],
output_keys=['laserscan'])
self._scan_topic = "/multisense/lidar_scan"
self._sub = ProxySubscriberCached({self._scan_topic: LaserScan})
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.ref_frame = userdata.ref_frame
self._topic = userdata.camera_topic
self._camera_frame = userdata.camera_frame
self._connected = False
self._failed = False
self._start_time = rospy.get_rostime()
# Subscribe to the topic for the logical camera
(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)))
# Get transform between camera and robot_base
try:
self._transform = self._tf_buffer.lookup_transform(
self.ref_frame, self._camera_frame, rospy.Time(0),
rospy.Duration(1.0))
except Exception as e:
Logger.logwarn('Could not transform pose: ' + str(e))
self._failed = True
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 __init__(self):
"""Constructor"""
super(KeepLookingAt, self).__init__(outcomes=['failed'],
input_keys=['ID'])
self.entities_topic = "/entities"
self._sub = ProxySubscriberCached({self.entities_topic: Entities})
self.pubp = rospy.Publisher("/sara_head_pitch_controller/command", Float64, queue_size=1)
self.puby = rospy.Publisher("/sara_head_yaw_controller/command", Float64, queue_size=1)
# Reference from and reference to
self.service = get_directionRequest()
self.service.reference = "head_xtion_link"
self.service.origine = "base_link"
self.Entity = None
self.serviceName = '/get_direction'
Logger.loginfo('waiting for service '+str(self.serviceName))
self.serv = rospy.ServiceProxy(self.serviceName, get_direction)
self._active = True
try:
self.serv.wait_for_service(1)
except:
self._active = False
def execute(self, userdata):
# This method is called periodically while the state is active.
# Main purpose is to check state conditions and trigger a corresponding outcome.
# If no outcome is returned, the state will stay active.
for i in [0, 1, 2, 3, 4, 5]:
self._topic = "/ariac/logical_camera_" + str(i)
self._start_time = rospy.Time.now()
(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})
elapsed = rospy.get_rostime() - self._start_time
while (elapsed.to_sec() < self._wait):
elapsed = rospy.get_rostime() - self._start_time
if self._sub.has_msg(self._topic):
message = self._sub.get_last_msg(self._topic)
for model in message.models:
if model.type == userdata.part_type:
userdata.bin = "bingr" + str(i) + "PreGrasp"
userdata.camera_topic = "/ariac/logical_camera_" + str(
i)
userdata.camera_frame = "logical_camera_" + str(
i) + "_frame"
Logger.loginfo("bingr" + str(i) + "PreGrasp")
userdata.ref_frame = "torso_main"
return 'continue'
Logger.loginfo("part_type not found")
return 'failed'
def __init__(self, *args, **kwargs):
super(RosState, self).__init__(*args, **kwargs)
self._rate = rospy.Rate(10)
self._is_controlled = False
self._pub = ProxyPublisher()
self._sub = ProxySubscriberCached()
def __init__(self,
target_time,
velocity,
rotation_rate,
cmd_topic='/create_node/cmd_vel',
sensor_topic='/create_node/sensor_state'):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(CreateTimedTwistState,
self).__init__(outcomes=['done', 'failed'])
# Store state parameter for later use.
self._target_time = rospy.Duration(target_time)
self._twist = TwistStamped()
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._done = None # Track the outcome so we can detect if transition is blocked
self._sensor_topic = sensor_topic
self._cmd_topic = cmd_topic
self._sensor_sub = ProxySubscriberCached(
{self._sensor_topic: CreateSensorState})
self._pub = ProxyPublisher({self._cmd_topic: TwistStamped})
def __init__(self):
'''Constructor'''
super(GetCameraImageState, self).__init__(outcomes=['done'],
output_keys=['camera_img'])
self._img_topic = "/multisense/camera/left/image_rect_color"
self._sub = ProxySubscriberCached({self._img_topic: Image})
def __init__(self,
target_time,
distance,
cmd_topic='/cmd_vel',
odometry_topic='/odom'):
super(MoveDistanceState, self).__init__(outcomes=['done', 'failed'])
self._target_time = target_time
self._distance = distance
self._velocity = (distance / target_time)
self._twist = TwistStamped()
self._twist.twist.linear.x = self._velocity
self._twist.twist.angular.z = 0
# 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._cmd_topic = cmd_topic
self._pub = ProxyPublisher({self._cmd_topic: TwistStamped})
self._odom_topic = odometry_topic
self._odom_sub = ProxySubscriberCached({self._odom_topic: Odometry})
self._starting_odom = None
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})
def __init__(self, ref_frame, camera_topic, camera_frame):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(DetectPartCameraState,
self).__init__(outcomes=['continue', 'failed'],
output_keys=['pose'])
self.ref_frame = ref_frame
self._topic = camera_topic
self._camera_frame = camera_frame
self._connected = False
self._failed = False
# tf to transfor the object pose
self._tf_buffer = tf2_ros.Buffer(
rospy.Duration(10.0)) #tf buffer length
self._tf_listener = tf2_ros.TransformListener(self._tf_buffer)
# Subscribe to the topic for the logical camera
(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 __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 __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 __init__(self, frontality_level, distance_max=10):
'''
Constructor
'''
super(list_entities_by_name,
self).__init__(outcomes=['found', 'none_found'],
output_keys=['entity_list', 'number'],
input_keys=['name'])
self._sub = ProxySubscriberCached({'/entities': Entities})
self._topic = "/robot_pose"
self._subpos = ProxySubscriberCached({self._topic: Pose})
self.frontality_level = frontality_level
self.mypose = None
self.message = None
self.distance_max = distance_max
def __init__(self, distance=1, ReplanPeriod=0.5):
"""Constructor"""
super(SaraFollow, self).__init__(outcomes=['failed'],
input_keys=['ID'])
# Prepare the action client for move_base
self._action_topic = "/move_base"
self._client = ProxyActionClient({self._action_topic: MoveBaseAction})
# Initialise the subscribers
self.entities_topic = "/entities"
self._pose_topic = "/robot_pose"
self._sub = ProxySubscriberCached({
self._pose_topic: Pose,
self.entities_topic: Entities
})
# Get the parameters
self.targetDistance = distance
self.ReplanPeriod = ReplanPeriod
# Initialise the status variables
self.MyPose = None
self.lastPlanTime = 0
def __init__(self, outcomes = ['unknown'], pose_ns = 'saved_msgs/joint_state', tolerance = 0.17, joint_topic = "joint_states", timeout = 1.0):
# Process supplied pose list. Note it must be processed befor superclass constructor is called,
# beacuse it changes 'outcomes' list due to side effect.
# check if 'unknown' outcome is present
if 'unknown' not in outcomes:
raise RuntimeError, '"unknown" should presents in state outcomes'
# remove 'unknown' while preserving items order
poses_names = [ outcome for outcome in outcomes if outcome != 'unknown' ]
# Load poses from paramere server. Pose is loaded in form dict(joint name -> position),
# but they are stored as a list of tuples (pose_name, pose_dict) to preserve order.
self._poses = [ (name, self.load_joint_state(pose_ns, name)) for name in poses_names ]
# Call superclass constructor.
super(CheckJointState, self).__init__(outcomes = outcomes)
# Subscribe to the joint topic.
self._joint_topic = joint_topic
self._pose_subscriber = ProxySubscriberCached({ self._joint_topic: JointState })
# Store topic parameter for later use.
self._tolerance = tolerance
self._timeout = Duration.from_sec(timeout)
# timestamp
self._timestamp = None
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 __init__(self, topic):
super(StorePointcloudState, self).__init__(outcomes = ['done'],
output_keys = ['pointcloud'])
self._sub = ProxySubscriberCached({topic: PointCloud2})
self._pcl_topic = topic
def test_lockable_state(self):
state = self._create()
fb_topic = 'flexbe/command_feedback'
sub = ProxySubscriberCached({fb_topic: CommandFeedback})
rospy.sleep(0.2) # wait for pub/sub
# lock and unlock as commanded, return outcome after unlock
state._sub._callback(String('/subject'), 'flexbe/command/lock')
state.result = 'done'
outcome = self._execute(state)
self.assertIsNone(outcome)
self.assertTrue(state._locked)
self.assertMessage(
sub, fb_topic,
CommandFeedback(command='lock', args=['/subject', '/subject']))
state.result = None
state._sub._callback(String('/subject'), 'flexbe/command/unlock')
outcome = self._execute(state)
self.assertEqual(outcome, 'done')
self.assertMessage(
sub, fb_topic,
CommandFeedback(command='unlock', args=['/subject', '/subject']))
# lock and unlock without target
state._sub._callback(String(''), 'flexbe/command/lock')
state.result = 'done'
outcome = self._execute(state)
self.assertIsNone(outcome)
self.assertMessage(
sub, fb_topic,
CommandFeedback(command='lock', args=['/subject', '/subject']))
state._sub._callback(String(''), 'flexbe/command/unlock')
outcome = self._execute(state)
self.assertEqual(outcome, 'done')
self.assertMessage(
sub, fb_topic,
CommandFeedback(command='unlock', args=['/subject', '/subject']))
# reject invalid lock command
state._sub._callback(String('/invalid'), 'flexbe/command/lock')
outcome = self._execute(state)
self.assertEqual(outcome, 'done')
self.assertMessage(
sub, fb_topic,
CommandFeedback(command='lock', args=['/invalid', '']))
# reject generic unlock command when not locked
state._sub._callback(String(''), 'flexbe/command/unlock')
self._execute(state)
self.assertMessage(sub, fb_topic,
CommandFeedback(command='unlock', args=['', '']))
# do not transition out of locked container
state.parent._locked = True
outcome = self._execute(state)
self.assertIsNone(outcome)
state.parent._locked = False
state.result = None
outcome = self._execute(state)
self.assertEqual(outcome, 'done')
def on_enter(self, userdata):
self._planning_failed = False
self._control_failed = False
self._success = False
self._joint_config = userdata.joint_values
self._joint_names = userdata.joint_names
self._sub = ProxySubscriberCached({self._position_topic: JointState})
self._current_position = []
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
# Action Initialization
action_goal = MoveGroupGoal()
action_goal.request.group_name = self._move_group
action_goal.request.allowed_planning_time = 1.0
goal_constraints = Constraints()
for i in range(len(self._joint_names)):
goal_constraints.joint_constraints.append(
JointConstraint(joint_name=self._joint_names[i],
position=self._joint_config[i],
weight=1.0))
action_goal.request.goal_constraints.append(goal_constraints)
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn('Failed to send action goal for group: %s\n%s' %
(self._move_group, str(e)))
self._planning_failed = True
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 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.
# One of the outcomes declared above.
for i in [1,2,3,4,5,6]:
self._topic = "/ariac/logical_camera_stock"+str(i)
self._start_time = rospy.Time.now()
(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})
elapsed = rospy.get_rostime() - self._start_time;
while (elapsed.to_sec() < self._wait):
elapsed = rospy.get_rostime() - self._start_time;
if self._sub.has_msg(self._topic):
message = self._sub.get_last_msg(self._topic)
for model in message.models:
if model.type == userdata.part_type:
if userdata.arm_id == "Right_Arm":
userdata.gantry_pos = "Gantry_Part"+str(i)+"_R"
elif userdata.arm_id == "Left_Arm":
userdata.gantry_pos = "Gantry_Part"+str(i)+"_L"
userdata.camera_topic = "/ariac/logical_camera_stock"+str(i)
userdata.camera_frame = "logical_camera_stock"+str(i)+"_frame"
Logger.loginfo("Gantry_Part"+str(i))
if i < 5:
return 'bin'
else:
return 'shelf'
Logger.loginfo("part_type not found")
return 'not_found'
