class GetPipePose(EventState):
'''
># centerpose PoseStamped() Pose of centered pipe.
#> pipepose PoseStamped() Pose of desired pipe.
'''
TOP_LEFT = 1
TOP_RIGHT = 2
CENTER = 3
DOWN_LEFT = 4
DOWN_RIGHT = 5
def __init__(self, choice):
'''
Constructor
'''
super(GetPipePose, self).__init__(outcomes=['succeeded'], input_keys = ['centerpose'], output_keys = ['pipepose'])
self._choice = choice
self._tf = ProxyTransformListener().listener()
self._succeeded = False
def execute(self, userdata):
'''
Execute this state
'''
if self._succeeded:
return 'succeeded'
def on_enter(self, userdata):
tempPose = PoseStamped()
tempPose = self._tf.transformPose('base_link', userdata.centerpose)
if self._choice == GetPipePose.TOP_LEFT:
tempPose.pose.position.y = tempPose.pose.position.y + 0.105
tempPose.pose.position.z = tempPose.pose.position.z + 0.105
if self._choice == GetPipePose.TOP_RIGHT:
tempPose.pose.position.y = tempPose.pose.position.y - 0.105
tempPose.pose.position.z = tempPose.pose.position.z + 0.105
if self._choice == GetPipePose.DOWN_RIGHT:
tempPose.pose.position.y = tempPose.pose.position.y - 0.105
tempPose.pose.position.z = tempPose.pose.position.z - 0.105
if self._choice == GetPipePose.DOWN_LEFT:
tempPose.pose.position.y = tempPose.pose.position.y + 0.105
tempPose.pose.position.z = tempPose.pose.position.z - 0.105
userdata.pipepose = tempPose
self._succeeded = True
class ObjectDetectionMonitor(EventState):
'''
Receive list of detected objects as cob_object_detection_msgs/DetectionArray messages and check if there is an object
which matches given (label, type) pair. Based on present of this object classify situation in `no_detections` state,
`have_detections` (there is objects but none of them matches give (label, type)),
`detection_matches` (desired object found). In the last case its pose bublished on given topic.
-- detection_topic string DetectionArray message topic.
-- label string Object label. Use '*' to match all labals.
-- type string Object type. Use '*' to match all types.
-- detection_period float Parameteres: detection_period (s), position_tolerance (m), orientation_tolerance (rad).
-- exit_states string[] Stop monitoring and return outcome if detected situation in this list.
-- pose_topic string Topic for publishing matched object pose (may be Empty).
-- pose_frame_id string Frame in which pose should be published.
#> pose object Object pose before exit.
<= no_detections Nothing have been detected for detection_period.
<= have_detections Objects were detected, but not of them matches (label, type)
<= detecton_matches Object presents and it is moving. `pose` key contains last pose.
<= failure Runtime error.
'''
def __init__(self,
detection_topic='detection',
label='*',
type='*',
exit_states=['no_detections'],
pose_topic='',
pose_frame_id='odom_combined',
detection_period=1.0):
super(ObjectDetectionMonitor, self).__init__(outcomes=[
'no_detections', 'have_detections', 'detection_matches', 'failure'
],
output_keys=['pose'])
# store state parameter for later use.
self._detection_topic = detection_topic
self._label = label
self._type = type
self._pose_topic = pose_topic
self._frame_id = pose_frame_id
self._exit_states = exit_states
self._detection_period = detection_period
# setup proxies
self._detection_subscriber = ProxySubscriberCached(
{self._detection_topic: DetectionArray})
self._detection_subscriber.enable_buffer(self._detection_topic)
if self._pose_topic:
self._pose_publisher = ProxyPublisher(
{self._pose_topic: PoseStamped})
else:
self._pose_publisher = None
self._tf_listener = ProxyTransformListener().listener()
# check parameters
if not isinstance(
self._frame_id,
str): # or not self._tf_listener.frameExists(self._frame_id):
raise ValueError(
'ObjectDetectionMonitor: %s is not a valid frame' %
self._frame_id)
if not isinstance(self._label, str) or not isinstance(self._type, str):
raise ValueError(
'ObjectDetectionMonitor: label and type parameters must be string.'
)
if not isinstance(self._detection_period,
float) or self._detection_period < 0.0:
raise ValueError(
'ObjectDetectionMonitor: detection_period must be float.')
if not isinstance(self._exit_states, list) or not all([
state
in ['no_detections', 'have_detections', 'detection_matches']
for state in self._exit_states
]):
raise ValueError(
'ObjectDetectionMonitor: incorrect list of exit states.')
# pose buffers
self._pose = None
self._last_detection_stamp = None
self._last_match_stamp = None
self._last_match_id = None
def on_enter(self, userdata):
# reset pose buffers
self._pose = None
stamp = rospy.Time.now()
self._last_detection_stamp = stamp
self._last_match_stamp = stamp
self._last_match_id = None
Logger.loginfo('ObjectDetectionMonitor: start monitoring.')
def execute(self, userdata):
# list of found matches
have_detections = False
best_match = None
match = None
# check if new message is available
while self._detection_subscriber.has_buffered(self._detection_topic):
# get detections msg
detections_msg = self._detection_subscriber.get_from_buffer(
self._detection_topic)
# check if have detections
if len(detections_msg.detections) > 0:
have_detections = True
# process detections
for detection in detections_msg.detections:
# check match
is_match = True
if self._label != '*' and detection.label != self._label:
is_match = False
if self._type != '*' and detection.type != self._type:
is_match = False
# register match
if is_match:
match = detection
if match.id == self._last_match_id:
best_match = match
if best_match:
match = best_match
# result
stamp = rospy.Time.now()
if not have_detections:
# no object detected
# check if detection preiod exceeded
if (stamp - self._last_detection_stamp
).to_sec() > self._detection_period:
# no detections state
if 'no_detections' in self._exit_states:
Logger.loginfo('ObjectDetectionMonitor: no detections.')
return 'no_detections'
elif not match:
# object detected but it is not match
self._last_detection_stamp = stamp
# check if detection perod exceeded
if (stamp - self._last_detection_stamp
).to_sec() > self._detection_period:
# have_detections state
if 'have_detections' in self._exit_states:
Logger.loginfo('ObjectDetectionMonitor: detection.')
return 'have_detections'
else:
# appropriate match is found
self._last_match_stamp = stamp
self._last_match_id = match.id
# publish
if self._pose_publisher:
# transform to frame_id
try:
# transform
pose_stamped = PoseStamped(
pose=match.pose,
header=Header(frame_id=match.header.frame_id))
self._pose = self._tf_listener.transformPose(
self._frame_id, pose_stamped)
except tf.Exception as e:
Logger.logwarn(
'Unable to transform from %s to %s' %
(match.pose.header.frame_id, self._frame_id))
return 'failure'
# publish
self._pose_publisher.publish(self._pose_topic, self._pose)
# match_detection state
if 'detection_matches' in self._exit_states:
Logger.loginfo('ObjectDetectionMonitor: match.')
return 'detection_matches'
return None
def on_exit(self, userdata):
userdata.pose = self._pose
class DetectPipesSM(Behavior):
'''
Position as required and try different ways to detect the pipe positions
'''
def __init__(self):
super(DetectPipesSM, self).__init__()
self.name = 'Detect Pipes'
# parameters of this behavior
self.add_parameter('pose_offset', 0.3)
self.add_parameter('close_offset', 0.1)
# references to used behaviors
# Additional initialization code can be added inside the following tags
# [MANUAL_INIT]
self._tf = ProxyTransformListener().listener()
# [/MANUAL_INIT]
# Behavior comments:
# O 98 480
# Run in full autonomy to skip looking, run in high to decide
def create(self):
joints_arm_with_gripper = ['arm_joint_%d'%i for i in range(5)]
srdf = "hector_tracker_robot_moveit_config/config/taurob_tracker.srdf"
percept_class = 'pipes'
move_group = "arm_with_gripper_group"
move_group_no_eef = "arm_group"
# x:268 y:162, x:472 y:46
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.object_pose = None
_state_machine.userdata.object_type = percept_class
_state_machine.userdata.test_pregrasp = [0, 1.57, 0.77, 0.38, 0]
_state_machine.userdata.index = -1
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:365, x:569 y:352, x:45 y:183
_sm_gripper_action_0 = OperatableStateMachine(outcomes=['finished', 'failed', 'back'])
with _sm_gripper_action_0:
# x:168 y:53
OperatableStateMachine.add('Decide_Close_Gripper',
OperatorDecisionState(outcomes=['close_gripper', 'move_back'], hint="Grasp pipe?", suggestion='close_gripper'),
transitions={'close_gripper': 'Close_Gripper', 'move_back': 'back'},
autonomy={'close_gripper': Autonomy.Full, 'move_back': Autonomy.Full})
# x:221 y:173
OperatableStateMachine.add('Close_Gripper',
GripperState(action=0.8, duration=2.0),
transitions={'done': 'Manipulate_Pipe', 'failed': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.High})
# x:229 y:391
OperatableStateMachine.add('Decide_Open_Gripper',
OperatorDecisionState(outcomes=['open_gripper', 'move_back'], hint="Open gripper or pull pipe?", suggestion='open_gripper'),
transitions={'open_gripper': 'Open_Gripper', 'move_back': 'back'},
autonomy={'open_gripper': Autonomy.High, 'move_back': Autonomy.Full})
# x:241 y:290
OperatableStateMachine.add('Manipulate_Pipe',
LogState(text="Perform desired pipe manipulation", severity=Logger.REPORT_HINT),
transitions={'done': 'Decide_Open_Gripper'},
autonomy={'done': Autonomy.High})
# x:215 y:511
OperatableStateMachine.add('Open_Gripper',
GripperState(action=GripperState.OPEN, duration=1.0),
transitions={'done': 'finished', 'failed': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.High})
# x:153 y:384, x:388 y:221
_sm_move_back_out_1 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['pipes_out_pose'])
with _sm_move_back_out_1:
# x:118 y:87
OperatableStateMachine.add('Get_Cartesian_Path_Back',
CalculateCartesianPathState(move_group=move_group, ignore_collisions=True),
transitions={'planned': 'Execute_Cartesian_Path_Back', 'failed': 'failed'},
autonomy={'planned': Autonomy.Low, 'failed': Autonomy.High},
remapping={'eef_pose': 'pipes_out_pose', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'})
# x:116 y:216
OperatableStateMachine.add('Execute_Cartesian_Path_Back',
MoveitExecuteTrajectoryState(),
transitions={'done': 'finished', 'failed': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.High},
remapping={'joint_trajectory': 'joint_trajectory'})
# x:230 y:517, x:550 y:229
_sm_move_to_close_pose_2 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['pipes_pose'])
with _sm_move_to_close_pose_2:
# x:97 y:43
OperatableStateMachine.add('Move_Pose_Cartesian',
CalculationState(calculation=lambda p: self.update_pose(p, self.close_offset)),
transitions={'done': 'Visualize_Pipes_Close_Pose'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'pipes_pose', 'output_value': 'pipes_close_pose'})
# x:113 y:140
OperatableStateMachine.add('Visualize_Pipes_Close_Pose',
PublishPoseState(topic='/pipes/close_pose'),
transitions={'done': 'Get_Cartesian_Path'},
autonomy={'done': Autonomy.Low},
remapping={'pose': 'pipes_close_pose'})
# x:147 y:371
OperatableStateMachine.add('Execute_Cartesian_Path',
MoveitExecuteTrajectoryState(),
transitions={'done': 'finished', 'failed': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.High},
remapping={'joint_trajectory': 'joint_trajectory'})
# x:121 y:260
OperatableStateMachine.add('Get_Cartesian_Path',
CalculateCartesianPathState(move_group=move_group, ignore_collisions=True),
transitions={'planned': 'Execute_Cartesian_Path', 'failed': 'failed'},
autonomy={'planned': Autonomy.Low, 'failed': Autonomy.High},
remapping={'eef_pose': 'pipes_close_pose', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'})
# x:119 y:516, x:358 y:275
_sm_move_to_out_pose_3 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['pipes_pose'], output_keys=['pipes_out_pose'])
with _sm_move_to_out_pose_3:
# x:83 y:35
OperatableStateMachine.add('Move_Pose_Out',
CalculationState(calculation=lambda p: self.update_pose(p, self.pose_offset)),
transitions={'done': 'Visualize_Pipes_Out_Pose'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'pipes_pose', 'output_value': 'pipes_out_pose'})
# x:85 y:307
OperatableStateMachine.add('Move_To_Joints',
MoveitToJointsState(move_group=move_group, joint_names=joints_arm_with_gripper, action_topic='/move_group'),
transitions={'reached': 'Correct_Gripper_Rotation', 'planning_failed': 'failed', 'control_failed': 'failed'},
autonomy={'reached': Autonomy.Low, 'planning_failed': Autonomy.High, 'control_failed': Autonomy.High},
remapping={'joint_config': 'joint_config'})
# x:77 y:118
OperatableStateMachine.add('Visualize_Pipes_Out_Pose',
PublishPoseState(topic='/pipes/out_pose'),
transitions={'done': 'Get_Joint_Target'},
autonomy={'done': Autonomy.Low},
remapping={'pose': 'pipes_out_pose'})
# x:83 y:211
OperatableStateMachine.add('Get_Joint_Target',
CalculateIKState(move_group=move_group, ignore_collisions=False),
transitions={'planned': 'Move_To_Joints', 'failed': 'failed'},
autonomy={'planned': Autonomy.Low, 'failed': Autonomy.High},
remapping={'eef_pose': 'pipes_out_pose', 'joint_config': 'joint_config'})
# x:71 y:408
OperatableStateMachine.add('Correct_Gripper_Rotation',
GripperRollState(rotation=GripperRollState.HORIZONTAL, duration=1.0),
transitions={'done': 'finished', 'failed': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.High})
# x:272 y:264, x:55 y:574
_sm_eef_to_detected_4 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['pipes_pose'], output_keys=['pipes_pose'])
with _sm_eef_to_detected_4:
# x:58 y:35
OperatableStateMachine.add('Move_To_Out_Pose',
_sm_move_to_out_pose_3,
transitions={'finished': 'Decide_Move_Close', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'pipes_pose': 'pipes_pose', 'pipes_out_pose': 'pipes_out_pose'})
# x:568 y:38
OperatableStateMachine.add('Move_To_Close_Pose',
_sm_move_to_close_pose_2,
transitions={'finished': 'Gripper_Action', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'pipes_pose': 'pipes_pose'})
# x:175 y:448
OperatableStateMachine.add('Move_Back_Out',
_sm_move_back_out_1,
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'pipes_out_pose': 'pipes_out_pose'})
# x:326 y:34
OperatableStateMachine.add('Decide_Move_Close',
OperatorDecisionState(outcomes=['close', 'end'], hint="Move closer to pipe?", suggestion='close'),
transitions={'close': 'Move_To_Close_Pose', 'end': 'finished'},
autonomy={'close': Autonomy.High, 'end': Autonomy.Full})
# x:626 y:219
OperatableStateMachine.add('Gripper_Action',
_sm_gripper_action_0,
transitions={'finished': 'Decide_Move_Back', 'failed': 'failed', 'back': 'Move_Back_Out'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'back': Autonomy.Inherit})
# x:408 y:452
OperatableStateMachine.add('Decide_Move_Back',
OperatorDecisionState(outcomes=['move_back', 'end'], hint="Move back from pipe?", suggestion='move_back'),
transitions={'move_back': 'Move_Back_Out', 'end': 'finished'},
autonomy={'move_back': Autonomy.High, 'end': Autonomy.Full})
# x:30 y:365, x:130 y:365
_sm_select_pipe_5 = OperatableStateMachine(outcomes=['finished', 'next'], input_keys=['index', 'object_pose', 'object_data'], output_keys=['index', 'pipe_pose'])
with _sm_select_pipe_5:
# x:98 y:78
OperatableStateMachine.add('Increment_Index',
CalculationState(calculation=lambda x: x+1),
transitions={'done': 'Select_Pipe'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'index', 'output_value': 'index'})
# x:102 y:178
OperatableStateMachine.add('Select_Pipe',
SelectPipePose(),
transitions={'selected': 'next', 'out_of_range': 'finished'},
autonomy={'selected': Autonomy.Off, 'out_of_range': Autonomy.Off},
remapping={'object_pose': 'object_pose', 'object_data': 'object_data', 'pose_choice': 'index', 'pipe_pose': 'pipe_pose'})
# x:427 y:433, x:453 y:131
_sm_get_pipes_6 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['object_pose'], output_keys=['object_pose', 'object_data'])
with _sm_get_pipes_6:
# x:51 y:28
OperatableStateMachine.add('Get_Observation_Arm_Config',
GetJointsFromSrdfState(config_name="observation_pose", srdf_file=srdf, move_group="", robot_name=""),
transitions={'retrieved': 'Move_To_Observation_Pose', 'file_error': 'failed'},
autonomy={'retrieved': Autonomy.Off, 'file_error': Autonomy.Off},
remapping={'joint_values': 'joints_observation_pose'})
# x:53 y:111
OperatableStateMachine.add('Move_To_Observation_Pose',
MoveitToJointsState(move_group="arm_group", joint_names=joints_arm_with_gripper[0:4], action_topic='/move_group'),
transitions={'reached': 'Detect_Pipes', 'planning_failed': 'failed', 'control_failed': 'failed'},
autonomy={'reached': Autonomy.Low, 'planning_failed': Autonomy.High, 'control_failed': Autonomy.High},
remapping={'joint_config': 'joints_observation_pose'})
# x:67 y:303
OperatableStateMachine.add('Get_Pipes_Pose',
MonitorPerceptState(required_support=0, percept_class='pipes', track_percepts=False),
transitions={'detected': 'Visualize_Pipes_Pose'},
autonomy={'detected': Autonomy.Off},
remapping={'object_id': 'object_id', 'object_pose': 'object_pose', 'object_data': 'object_data'})
# x:59 y:396
OperatableStateMachine.add('Visualize_Pipes_Pose',
PublishPoseState(topic='/pipes/center_pose'),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Low},
remapping={'pose': 'object_pose'})
# x:72 y:210
OperatableStateMachine.add('Detect_Pipes',
PipeDetectionState(max_attempts=100),
transitions={'found': 'Get_Pipes_Pose', 'unknown': 'failed'},
autonomy={'found': Autonomy.Low, 'unknown': Autonomy.High})
with _state_machine:
# x:94 y:72
OperatableStateMachine.add('Get_Pipes',
_sm_get_pipes_6,
transitions={'finished': 'Select_Pipe', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'object_pose': 'object_pose', 'object_data': 'object_data'})
# x:492 y:328
OperatableStateMachine.add('Move_To_Pregrasp',
MoveitToJointsState(move_group="arm_group", joint_names=joints_arm_with_gripper[0:4], action_topic='/move_group'),
transitions={'reached': 'EEF_To_Detected', 'planning_failed': 'failed', 'control_failed': 'failed'},
autonomy={'reached': Autonomy.Low, 'planning_failed': Autonomy.High, 'control_failed': Autonomy.High},
remapping={'joint_config': 'test_pregrasp'})
# x:87 y:328
OperatableStateMachine.add('Decide_Look_At',
OperatorDecisionState(outcomes=['focus', 'skip'], hint="Should sensor head focus pipes?", suggestion='skip'),
transitions={'focus': 'Look_At_Target', 'skip': 'Move_To_Pregrasp'},
autonomy={'focus': Autonomy.Full, 'skip': Autonomy.High})
# x:300 y:428
OperatableStateMachine.add('Look_At_Target',
LookAtWaypoint(),
transitions={'reached': 'Move_To_Pregrasp', 'failed': 'failed'},
autonomy={'reached': Autonomy.High, 'failed': Autonomy.Full},
remapping={'waypoint': 'pipes_pose'})
# x:94 y:222
OperatableStateMachine.add('Select_Pipe',
_sm_select_pipe_5,
transitions={'finished': 'finished', 'next': 'Decide_Look_At'},
autonomy={'finished': Autonomy.Inherit, 'next': Autonomy.Inherit},
remapping={'index': 'index', 'object_pose': 'object_pose', 'object_data': 'object_data', 'pipe_pose': 'pipes_pose'})
# x:783 y:322
OperatableStateMachine.add('EEF_To_Detected',
_sm_eef_to_detected_4,
transitions={'finished': 'Select_Pipe', 'failed': 'Select_Pipe'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'pipes_pose': 'pipes_pose'})
return _state_machine
# Private functions can be added inside the following tags
# [MANUAL_FUNC]
def update_pose(self, pose, offset):
new_pose = self._tf.transformPose('base_link', pose)
new_pose.pose.position.x -= offset
new_pose.pose.orientation.x = 0
new_pose.pose.orientation.y = 0
new_pose.pose.orientation.z = 0
new_pose.pose.orientation.w = 1
return new_pose
class RandPoseGenerator(EventState):
'''
Periodically generate geometry_msgs/PoseStamped messge in given parallelepiped.
Frames for parallelepiped is defined and the frame in which message is published can be different.
-- topic string Topic where PoseStamped message should be published.
-- duration float How long this state will be executed (seconds).
-- interval float[2] Array of floats, maximal and minimal interval between movements.
-- minXYZ float[3] Max values for x, y, z coordinates.
-- maxXYZ float[3] Min values for x, y, z coordinates.
-- frame_xyz string Coordinate frame of parallelepiped.
-- frame_out string Frame in which pose is publised.
<= done Finished.
<= failed Failed to activate FollowJointState controller.
'''
def __init__(self,
topic='motion/controller/look_at/in_pose_ref',
duration=120,
interval=[3, 5],
maxXYZ=[1, 0.3, 0.5],
minXYZ=[1.0, -0.3, 0.0],
frame_xyz='base_link',
frame_out='odom_combined'):
super(RandPoseGenerator, self).__init__(outcomes=['done', 'failure'])
# Store topic parameter for later use.
if not isinstance(topic, str):
raise ValueError("Topic name must be string.")
if not isinstance(duration, (int, float)) or duration <= 0:
raise ValueError("Duration must be positive number.")
if len(interval) != 2 or any([
not isinstance(t, (int, float)) for t in interval
]) or any([t < 0 for t in interval]) or interval[0] > interval[1]:
raise ValueError("Interval must represent interval of time.")
if len(minXYZ) != 3 or any(
[not isinstance(v, (int, float)) for v in minXYZ]):
raise ValueError("minXYZ: list of three numbers was expected.")
if len(maxXYZ) != 3 or any(
[not isinstance(v, (int, float)) for v in maxXYZ]):
raise ValueError("maxXYZ: list of three numbers was expected.")
if not isinstance(frame_xyz, str) or not isinstance(frame_out, str):
raise ValueError("Frame names must be string.")
self._topic = topic
self._duration = Duration.from_sec(duration)
self._interval = interval
self._minXYZ = minXYZ
self._maxXYZ = maxXYZ
self._frame_in = frame_xyz
self._frame_out = frame_out
# create proxies
self._publisher = ProxyPublisher({self._topic: PoseStamped})
self._tf_listener = ProxyTransformListener().listener()
self._tf_listener.waitForTransform(self._frame_out, self._frame_in,
rospy.Time(), rospy.Duration(1))
if not self._tf_listener.canTransform(self._frame_out, self._frame_in,
rospy.Time(0)):
raise ValueError(
"Unable to perform transform between frames %s and %s." %
(self._frame_out, self._frame_in))
# state
self._start_timestamp = None
self._movement_timestamp = None
self._movement_duration = Duration()
# error in enter hook
self._error = False
def on_enter(self, userdata):
self._error = False
# set start timestamp
self._start_timestamp = Time.now()
self._movement_timestamp = Time.now()
Logger.loginfo('Start random pose generation, topic `%s`.' %
self._topic)
def execute(self, userdata):
if self._error:
return 'failure'
time_now = Time.now()
# check if time elasped
if time_now - self._start_timestamp > self._duration:
return 'done'
# check if we have tosend new point
if time_now - self._movement_timestamp > self._movement_duration:
# determine new interval
self._movement_timestamp = time_now
self._movement_duration = Duration.from_sec(
random.uniform(*self._interval))
# form message
msg = PoseStamped()
msg.header.frame_id = self._frame_in
msg.header.stamp = Time(0.0)
# random position
msg.pose.position.x = random.uniform(self._minXYZ[0],
self._maxXYZ[0])
msg.pose.position.y = random.uniform(self._minXYZ[1],
self._maxXYZ[1])
msg.pose.position.z = random.uniform(self._minXYZ[2],
self._maxXYZ[2])
# transform to output frame
try:
# transform
self._pose = self._tf_listener.transformPose(
self._frame_out, msg)
except tf.Exception as e:
Logger.logwarn('Unable to transform from %s to %s: %s' %
(self._frame_in, self._frame_out, e))
return 'failure'
# publish pose
msg.header.stamp = self._movement_timestamp
self._publisher.publish(self._topic, msg)
def on_exit(self, userdata):
Logger.loginfo('Done random pose generation.')
class BrohoofSM(Behavior):
'''
Move leg1 up to specified target, perform greeting, wait and pt leg down.
Robot is assumed to be standing on four legs.
'''
def __init__(self):
super(BrohoofSM, self).__init__()
self.name = 'Brohoof'
# parameters of this behavior
self.add_parameter('wait_time', 5)
self.add_parameter('neck_angle', 0.25)
self.add_parameter('brohoof_cone', 0.78)
self.add_parameter('brohoof_upper_limit', 0.30)
# references to used behaviors
# Additional initialization code can be added inside the following tags
# [MANUAL_INIT]
self._tf = ProxyTransformListener().listener()
# [/MANUAL_INIT]
# Behavior comments:
def create(self):
# x:228 y:396, x:225 y:240, x:1010 y:659
_state_machine = OperatableStateMachine(outcomes=['finished', 'unreachable', 'failed'], input_keys=['pose'])
_state_machine.userdata.pose = PoseStamped(Header(frame_id = 'base_link'), Pose(Point(0.4, 0.0, -0.05), Quaternion(0, 0, 0, 1)))
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:118 y:85
OperatableStateMachine.add('CheckHandPosition',
DecisionState(outcomes=['good','bad'], conditions=self.checkHandPosition),
transitions={'good': 'GetHeadPose', 'bad': 'unreachable'},
autonomy={'good': Autonomy.Off, 'bad': Autonomy.Off},
remapping={'input_value': 'pose'})
# x:890 y:253
OperatableStateMachine.add('SayHello',
TextCommandState(type='voice/play_wav', command='hello_im_sweetie_bot_friedship_programms', topic='control'),
transitions={'done': 'Wait'},
autonomy={'done': Autonomy.Off})
# x:663 y:69
OperatableStateMachine.add('RaiseHead',
MoveitToJointsState(move_group='head', joint_names=['joint51', 'joint52', 'joint53', 'joint54'], action_topic='move_group'),
transitions={'reached': 'RaiseLegProgram', 'planning_failed': 'failed', 'control_failed': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'joint_config': 'head_pose'})
# x:309 y:66
OperatableStateMachine.add('GetHeadPose',
GetJointValuesState(joints=['joint51', 'joint52', 'joint53', 'joint54']),
transitions={'retrieved': 'CalcRaisedHeadPose'},
autonomy={'retrieved': Autonomy.Off},
remapping={'joint_values': 'head_pose'})
# x:485 y:73
OperatableStateMachine.add('CalcRaisedHeadPose',
CalculationState(calculation=self.calculateHeadPoseFunc),
transitions={'done': 'RaiseHead'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'head_pose', 'output_value': 'head_pose'})
# x:891 y:65
OperatableStateMachine.add('RaiseLegProgram',
ExecuteJointTrajectory(action_topic='motion/controller/joint_trajectory', trajectory_param='brohoof_begin', trajectory_ns='/saved_msgs/joint_trajectory'),
transitions={'success': 'SayHello', 'partial_movement': 'failed', 'invalid_pose': 'failed', 'failure': 'failed'},
autonomy={'success': Autonomy.Off, 'partial_movement': Autonomy.Off, 'invalid_pose': Autonomy.Off, 'failure': Autonomy.Off},
remapping={'result': 'result'})
# x:510 y:396
OperatableStateMachine.add('ReturnLegProgrammed',
ExecuteJointTrajectory(action_topic='motion/controller/joint_trajectory', trajectory_param='brohoof_end', trajectory_ns='/saved_msgs/joint_trajectory'),
transitions={'success': 'finished', 'partial_movement': 'failed', 'invalid_pose': 'failed', 'failure': 'failed'},
autonomy={'success': Autonomy.Off, 'partial_movement': Autonomy.Off, 'invalid_pose': Autonomy.Off, 'failure': Autonomy.Off},
remapping={'result': 'result'})
# x:851 y:399
OperatableStateMachine.add('Wait',
WaitState(wait_time=self.wait_time),
transitions={'done': 'ReturnLegProgrammed'},
autonomy={'done': Autonomy.Off})
return _state_machine
# Private functions can be added inside the following tags
# [MANUAL_FUNC]
def calculateHeadPoseFunc(self, joints):
joints = list(joints)
joints[0] = 0.0
joints[2] = self.neck_angle
return joints
def checkHandPosition(self, input_pose):
'''
Check if hand is in good position for brohoof. Return 'good' or 'bad'.
'''
# check if frame header presents
if not input_pose.header.frame_id:
Logger.logerr('checkHandPosition: Header is missing.')
return 'bad'
# convert ot base_link frame
try:
output_pose = self._tf.transformPose('base_link', PoseStamped(Header(frame_id = input_pose.header.frame_id), input_pose.pose))
except tf.Exception as e:
Logger.logerr('calculateHeadPoseFunc: Cannot transform from `%s` to `base_link` frame.' % input_pose.header.frame_id)
return 'bad'
pos = output_pose.pose.position
Logger.loginfo('calculateHeadPoseFunc: object pos: %s' % str(pos))
# Check if boundarie are good
if pos.z > self.brohoof_upper_limit or pos.z < -0.15:
# too high or to low
return 'bad'
if pos.x < 0.25:
# too close
return 'bad'
# Move to shoulder1 point
pos.x -= 0.080
pos.y -= 0.037
pos.z += 0.027
# Check if angle is out of cone
if abs(math.atan2(pos.y,pos.x)) > self.brohoof_cone:
# out of cone
return 'bad'
return 'good'
