class GetTemplatePoseState(EventState):
'''
Requests the pose of a template from the template server.
># template_id int ID of the template to get the pose for.
#> grasp PoseStamped The current pose of the template.
<= done Pose data is available.
<= failed Failed to get pose information.
'''
def __init__(self):
'''Constructor'''
super(GetTemplatePoseState,
self).__init__(outcomes=['done', 'failed'],
input_keys=['template_id'],
output_keys=['template_pose'])
self._service_topic = "/template_info"
self._srv = ProxyServiceCaller(
{self._service_topic: GetTemplateStateAndTypeInfo})
self._srv_result = None
self._failed = False
def execute(self, userdata):
if self._failed or self._srv_result is None:
return 'failed'
try:
userdata.template_pose = self._srv_result.template_state_information.pose
except Exception as e:
Logger.logwarn(
'Failed to retrieve pose information from service result:\n%s'
% str(e))
return 'failed'
return 'done'
def on_enter(self, userdata):
try:
self._srv_result = self._srv.call(
self._service_topic,
GetTemplateStateAndTypeInfoRequest(
userdata.template_id,
0)) # We don't care about the hand side
except Exception as e:
Logger.logwarn('Failed to send service call:\n%s' % str(e))
self._failed = True
class GetTemplatePoseState(EventState):
'''
Requests the pose of a template from the template server.
># template_id int ID of the template to get the pose for.
#> grasp PoseStamped The current pose of the template.
<= done Pose data is available.
<= failed Failed to get pose information.
'''
def __init__(self):
'''Constructor'''
super(GetTemplatePoseState, self).__init__(outcomes = ['done', 'failed'],
input_keys = ['template_id'],
output_keys = ['template_pose'])
self._service_topic = "/template_info"
self._srv = ProxyServiceCaller({self._service_topic: GetTemplateStateAndTypeInfo})
self._srv_result = None
self._failed = False
def execute(self, userdata):
if self._failed or self._srv_result is None:
return 'failed'
try:
userdata.template_pose = self._srv_result.template_state_information.pose
except Exception as e:
Logger.logwarn('Failed to retrieve pose information from service result:\n%s' % str(e))
return 'failed'
return 'done'
def on_enter(self, userdata):
try:
self._srv_result = self._srv.call(self._service_topic, GetTemplateStateAndTypeInfoRequest(userdata.template_id, 0)) # We don't care about the hand side
except Exception as e:
Logger.logwarn('Failed to send service call:\n%s' % str(e))
self._failed = True
class CurrentJointPositionsState(EventState):
'''
Retrieves the current positions of the joints of the specified planning group.
-- planning_group string Name of the planning group.
#> joint_positions float[] Current joint values. Length of the list depends on the number of joints of the planning group.
<= retrieved Successfully determined the current joint values.
<= failed Failed to send service call or got no result.
'''
def __init__(self, planning_group):
'''
Constructor
'''
super(CurrentJointPositionsState,
self).__init__(outcomes=['retrieved', 'failed'],
output_keys=['joint_positions'])
self._srv_topic = '/flor/planning/upper_body/get_group_state'
self._srv = ProxyServiceCaller(
{self._srv_topic: GetCurrentPlanningGroupState})
self._planning_group = planning_group
self._srv_result = None
self._failed = False
def execute(self, userdata):
'''
Execute this state
'''
if self._failed:
return 'failed'
if self._srv_result is not None:
userdata.joint_positions = self._srv_result.position
if len(self._srv_result.position) == 0:
Logger.logwarn("Lookup of joint positions of group %s failed" %
self._planning_group)
self._failed = True
return 'failed'
else:
return 'retrieved'
def on_enter(self, userdata):
self._failed = False
try:
self._srv_result = self._srv.call(
self._srv_topic,
GetCurrentPlanningGroupStateRequest(self._planning_group))
except Exception as e:
Logger.logwarn('Could not retrieve current joint positions!')
rospy.logwarn(str(e))
self._failed = True
class QueryJointPositionsState(EventState):
'''
Retrieves the current positions of the joints of the specified planning group.
-- side string One of left, right
-- controller string Specifify the trajectory controller suffix
Example: 'traj_hybrid_controller'
#> joint_config dict Trajectory joint positions at current time.
joint_names string[] : joint_values[]
<= retrieved Successfully determined the current joint values.
<= failed Failed to send service call or got no result.
'''
TRAJ_CONTROLLER = 'traj_controller'
HYBRID_CONTROLLER = 'traj_hybrid_controller'
def __init__(self, side, controller = 'traj_controller'):
'''
Constructor
'''
super(QueryJointPositionsState, self).__init__(outcomes = ['retrieved', 'failed'],
output_keys = ['joint_config'])
if not rospy.has_param("behavior/joint_controllers_name"):
Logger.logerr("Need to specify parameter behavior/joint_controllers_name at the parameter server")
return
controller_namespace = rospy.get_param("behavior/joint_controllers_name")
self._srv_topic = '/' + controller_namespace + '/' + side + '_arm_' + \
controller + '/query_state'
self._srv = ProxyServiceCaller({self._srv_topic: QueryTrajectoryState})
self._srv_result = None
self._failed = False
def execute(self, userdata):
'''
Execute this state
'''
if self._failed:
return 'failed'
else:
return 'retrieved'
def on_enter(self, userdata):
self._failed = False
try:
self._srv_result = self._srv.call(
self._srv_topic,
QueryTrajectoryStateRequest(time = rospy.Time.now()))
joint_names = self._srv_result.name
joint_values = list(self._srv_result.position) # cast tuple to list
userdata.joint_config = {'joint_names' : joint_names,
'joint_values': joint_values}
except Exception as e:
Logger.logwarn('Could not retrieve current joint positions!\n%s' % str(e))
self._failed = True
class ServiceRotateRelativeToGoToState(EventState):
'''
State for calling goto service in MRS system.
-- set_yaw_service_topic string service topic for setting yaw.
-- control_manager_diagnostics_topic string service topic for control diagnostics.
># goal_tracker_point mrs_msgs.srv.ReferenceStampedSrvRequest Target position in Reference style
<= successed Whenever the calling for successful.
<= failed When the calling failed.
'''
def __init__(self, set_yaw_service_topic,
control_manager_diagnostics_topic, odometry_topic):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ServiceRotateRelativeToGoToState,
self).__init__(input_keys=[
'goal_tracker_point', 'frame_id', 'yaw_relative_to_goal'
],
output_keys=['desired_yaw'],
outcomes=['successed', 'failed'])
# Store state parameter for later use.
self._set_yaw_service_topic = rospy.resolve_name(set_yaw_service_topic)
self._control_manager_diagnostics_topic = rospy.resolve_name(
control_manager_diagnostics_topic)
self._odometry_topic = rospy.resolve_name(odometry_topic)
# Create proxy service client
self._srv = ProxyServiceCaller(
{self._set_yaw_service_topic: mrs_msgs.srv.Vec1})
(msg_path, msg_topic,
fn) = rostopic.get_topic_type(self._control_manager_diagnostics_topic)
if msg_topic == self._control_manager_diagnostics_topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub_cont_diag = ProxySubscriberCached(
{self._control_manager_diagnostics_topic: msg_type})
self._connected = True
Logger.loginfo(
'[ServiceRotateRelativeToGoToState]: Successfully subscribed to topic %s'
% self._control_manager_diagnostics_topic)
(msg_path_odom, msg_topic_odom,
fn_odom) = rostopic.get_topic_type(self._odometry_topic)
if msg_topic_odom == self._odometry_topic:
msg_type_odom = self._get_msg_from_path(msg_path_odom)
self._sub_odom = ProxySubscriberCached(
{self._odometry_topic: msg_type_odom})
self._connected_odom = True
Logger.loginfo(
'[ServiceRotateRelativeToGoToState]: Successfully subscribed to topic %s'
% self._odometry_topic)
self._desired_yaw = 0
self._sended = False
self._failed = 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.
if not self._connected:
Logger.logerr(
'[ServiceRotateRelativeToGoToState]: not connected to %s' %
self._control_manager_diagnostics_topic)
return 'failed'
if not self._connected_odom:
Logger.logerr(
'[ServiceRotateRelativeToGoToState]: not connected to %s' %
self._odometry_topic)
return 'failed'
diff_time = rospy.get_time() - self._start_time
if self._sub_cont_diag.has_msg(self._control_manager_diagnostics_topic
) and self._sub_odom.has_msg(
self._odometry_topic):
Logger.loginfo(
'[ServiceRotateRelativeToGoToState]: has_diagnostics msg at time %s'
% (str(diff_time)))
message = self._sub_cont_diag.get_last_msg(
self._control_manager_diagnostics_topic)
message_odom = self._sub_odom.get_last_msg(self._odometry_topic)
currentPosition = message_odom.pose.pose.position
target_position = userdata.goal_tracker_point.position
self._desired_yaw = math.atan2(
target_position.y - currentPosition.y,
target_position.x - currentPosition.x)
self._sub_cont_diag.remove_last_msg(
self._control_manager_diagnostics_topic)
self._sub_odom.remove_last_msg(self._odometry_topic)
rospy.loginfo_throttle(
2,
'[ServiceRotateRelativeToGoToState]: currentPosition %s %s' %
(str(currentPosition.x), str(currentPosition.y)))
rospy.loginfo_throttle(
2,
'[ServiceRotateRelativeToGoToState]: target_position %s %s' %
(str(target_position.x), str(target_position.y)))
rospy.loginfo_throttle(
2, '[ServiceRotateRelativeToGoToState]: self._desired_yaw %s' %
(str(self._desired_yaw)))
if not self._sended:
# Check if the ProxyServiceCaller has been registered
if not self._srv.is_available(self._set_yaw_service_topic):
Logger.logerr(
'[ServiceRotateRelativeToGoToState]: Topic \'{}\' not yet registered!'
.format(self._set_yaw_service_topic))
self._failed = True
return
try:
service_request = mrs_msgs.srv.Vec1Request()
service_request.goal = self._desired_yaw
service_result = self._srv.call(
self._set_yaw_service_topic, service_request)
Logger.loginfo(
'[ServiceRotateRelativeToGoToState]: sended desired yaw'
)
if not service_result.success:
self._failed = True
Logger.logwarn(
'[ServiceRotateRelativeToGoToState]: Calling \'{}\' was not successful: {} for reference position (x,y,z) = ({},{},{})'
.format(self._service_topic,
str(service_result.message),
service_request.goal))
else:
self._sended = True
self._start_time = rospy.get_time()
userdata.desired_yaw = self._desired_yaw
Logger.loginfo(
'[ServiceRotateRelativeToGoToState]: call result {}'
.format(str(service_result.message)))
except Exception as e:
Logger.logerr(
'[ServiceRotateRelativeToGoToState]: Failed to call \'{}\' service request: {}'
.format(self._set_yaw_service_topic, str(e)))
self._failed = True
diff_time = rospy.get_time() - self._start_time
if diff_time >= 1.0 and not message.tracker_status.have_goal and self._sended:
Logger.loginfo(
'[ServiceRotateRelativeToGoToState]: Successfully ended following of trajectory %s'
% self._control_manager_diagnostics_topic)
return 'successed'
else:
return
if self._failed:
return 'failed'
def on_enter(self, userdata):
# This method is called when the state becomes active, i.e. a transition from another state to this one is taken.
# It is primarily used to start actions which are associated with this state.
self._start_time = rospy.get_time()
if not self._connected:
(msg_path, msg_topic, fn) = rostopic.get_topic_type(
self._control_manager_diagnostics_topic)
if msg_topic == self._control_manager_diagnostics_topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub_cont_diag = ProxySubscriberCached(
{self._control_manager_diagnostics_topic: msg_type})
self._connected = True
Logger.loginfo(
'[ServiceRotateRelativeToGoToState]: Successfully subscribed to previously unavailable topic %s'
% self._control_manager_diagnostics_topic)
else:
Logger.logwarn(
'[ServiceRotateRelativeToGoToState]: Topic %s still not available, giving up.'
% self._control_manager_diagnostics_topic)
if not self._connected_odom:
(msg_path_odom, msg_topic_odom,
fn_odom) = rostopic.get_topic_type(self._odometry_topic)
if msg_topic_odom == self._odometry_topic:
msg_type_odom = self._get_msg_from_path(msg_path_odom)
self._sub_odom = ProxySubscriberCached(
{self._odometry_topic: msg_type_odom})
self._connected_odom = True
Logger.loginfo(
'[ServiceRotateRelativeToGoToState]: Successfully subscribed to topic %s'
% self._odometry_topic)
else:
Logger.logwarn(
'[ServiceRotateRelativeToGoToState]: Topic %s still not available, giving up.'
% self._odometry_topic)
if self._connected and self._sub_cont_diag.has_msg(
self._control_manager_diagnostics_topic):
Logger.loginfo(
'[ServiceRotateRelativeToGoToState]: Waiting for msg from topic %s'
% self._control_manager_diagnostics_topic)
self._sub_cont_diag.remove_last_msg(
self._control_manager_diagnostics_topic)
if self._connected_odom and self._sub_odom.has_msg(
self._odometry_topic):
Logger.loginfo(
'[ServiceRotateRelativeToGoToState]: Waiting for msg from topic %s'
% self._odometry_topic)
self._sub_odom.remove_last_msg(self._odometry_topic)
self._failed = False
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 ServiceSetWallDefinitionAndConstraintsIntoPlanKeeper(EventState):
'''
State for saving the plans for robots in Wall challenge in MBZIRC 2020 into plan_keeper node.
-- service_topic string Service_topic_name.
#> brick_rules Binary rules of bricks.
#> brick_positions Array of brick positions in the wall.
#> brick_ids Array of brick ids ordered same as brick_positions.
<= successed Whenever the calling for successful.
<= failed When the calling failed.
'''
def __init__(self, plan_keeper_wall_definition_in_service_topic,
plan_keeper_building_rules_in_service_topic):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ServiceSetWallDefinitionAndConstraintsIntoPlanKeeper,
self).__init__(
input_keys=['brick_rules', 'brick_positions', 'brick_ids'],
outcomes=['successed', 'failed'])
# Store state parameter for later use.
self._plan_keeper_wall_definition_in_service_topic = rospy.resolve_name(
plan_keeper_wall_definition_in_service_topic)
self._plan_keeper_building_rules_in_service_topic = rospy.resolve_name(
plan_keeper_building_rules_in_service_topic)
# Create proxy service client
self._srv_wall_definition = ProxyServiceCaller({
self._plan_keeper_wall_definition_in_service_topic:
plan_keeper.srv.SetWallDefinition
})
self._srv_building_rules = ProxyServiceCaller({
self._plan_keeper_building_rules_in_service_topic:
plan_keeper.srv.SetBuildingRules
})
self._failed = 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.
if self._failed:
return 'failed'
else:
return 'successed'
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._failed = False
# Check if the ProxyServiceCaller has been registered for setting wall definition
if not self._srv_wall_definition.is_available(
self._plan_keeper_wall_definition_in_service_topic):
Logger.logerr(
'ProxyServiceCaller: Topic \'{}\' not yet registered!'.format(
self._plan_keeper_wall_definition_in_service_topic))
self._failed = True
return
try:
service_request = plan_keeper.srv.SetWallDefinitionRequest()
service_request.brick_positions = userdata.brick_positions
service_request.brick_ids = userdata.brick_ids
service_result = self._srv_wall_definition.call(
self._plan_keeper_wall_definition_in_service_topic,
service_request)
if not service_result.success:
self._failed = True
Logger.logwarn('Calling \'{}\' was not successful: {}'.format(
self._plan_keeper_wall_definition_in_service_topic,
str(service_result.message)))
else:
Logger.loginfo(service_result.message)
except Exception as e:
Logger.logerr('Failed to call \'{}\' service request: {}'.format(
self._plan_keeper_wall_definition_in_service_topic, str(e)))
self._failed = True
# Check if the ProxyServiceCaller has been registered for setting constraints
if not self._srv_building_rules.is_available(
self._plan_keeper_building_rules_in_service_topic):
Logger.logerr(
'ProxyServiceCaller: Topic \'{}\' not yet registered!'.format(
self._plan_keeper_building_rules_in_service_topic))
self._failed = True
return
try:
service_request = plan_keeper.srv.SetBuildingRulesRequest()
service_request.brick_rules = userdata.brick_rules
service_result = self._srv_building_rules.call(
self._plan_keeper_building_rules_in_service_topic,
service_request)
if not service_result.success:
self._failed = True
Logger.logwarn('Calling \'{}\' was not successful: {}'.format(
self._plan_keeper_building_rules_in_service_topic,
str(service_result.message)))
else:
Logger.loginfo(service_result.message)
except Exception as e:
Logger.logerr('Failed to call \'{}\' service request: {}'.format(
self._plan_keeper_building_rules_in_service_topic, str(e)))
self._failed = True
class SpiralAroundPointState(EventState):
'''
State for calling goto service in MRS system.
-- service_topic string Service_topic_name.
># goal_tracker_point mrs_msgs.srv.ReferenceStampedSrvRequest Target position in Reference style
># frame_id string frame id of targetreference
<= successed Whenever the calling for successful.
<= failed When the calling failed.
'''
def __init__(self, service_topic, brick_estimation_closest_wall_topic):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(SpiralAroundPointState, self).__init__(
input_keys=['start_spiral_tracker_point', 'frame_id'],
outcomes=['successed', 'failed', 'over_time'])
# Store state parameter for later use.
self._service_topic = rospy.resolve_name(service_topic)
self._closest_wall_topic = rospy.resolve_name(
brick_estimation_closest_wall_topic)
# Create proxy service client
self._srv = ProxyServiceCaller(
{self._service_topic: mrs_msgs.srv.ReferenceStampedSrv})
self._sub = rospy.Subscriber(self._closest_wall_topic,
mbzirc_msgs.msg.MbzircBrick,
self.nearest_wall_callback)
self._start_point = None
self._spiral_points = None
self._failed = False
self.received = False
def nearest_wall_callback(self, message):
#rospy.loginfo_throttle(5,"[SpiralAroundPointState]: received nearest wall %s"%(str(message.header.stamp)))
#rospy.loginfo("rospy.get_rostime() %s"%(str(rospy.get_rostime())))
#rospy.loginfo("message.header.stamp %s"%(str(message.header.stamp)))
#rospy.loginfo("diff %s"%(str(rospy.get_rostime()-message.header.stamp)))
if rospy.get_rostime() - message.header.stamp < rospy.Duration(
secs=TIME_DURATION):
rospy.loginfo_throttle(
0.5,
"[SpiralAroundPointState]: received nearest wall reacently %s"
% (str(message.header.stamp)))
self.received = True
def execute(self, userdata):
# This method is called periodically while the state is active.
# Main purpose is to check state conditions and trigger a corresponding outcome.
# If no outcome is returned, the state will stay active.
if self.received:
rospy.loginfo(
"[SpiralAroundPointState]: successfully ended with detection")
return 'successed'
diff_time = rospy.get_time() - self._start_time
if diff_time > TIME_DURATION:
rospy.loginfo(
"[SpiralAroundPointState]: spiral ended with over_time without detection"
)
return 'over_time'
time_portion = diff_time / TIME_DURATION
spiral_idx = int(len(self._spiral_points) * time_portion)
spiral_idx = min(spiral_idx, len(self._spiral_points) - 1)
spiral_idx = max(spiral_idx, 0)
current_target = self._spiral_points[spiral_idx]
# Check if the ProxyServiceCaller has been registered
if not self._srv.is_available(self._service_topic):
Logger.logerr(
'[SpiralAroundPointState]: Topic \'{}\' not yet registered!'.
format(self._service_topic))
self._failed = True
return
try:
service_request = mrs_msgs.srv.ReferenceStampedSrvRequest()
service_request.header = std_msgs.msg.Header()
service_request.header.stamp = rospy.Time.now()
service_request.header.seq = 0
service_request.header.frame_id = userdata.frame_id
service_request.reference.position.x = current_target[0]
service_request.reference.position.y = current_target[1]
service_request.reference.position.z = userdata.start_spiral_tracker_point.z
service_request.reference.yaw = userdata.start_spiral_tracker_point.yaw
service_result = self._srv.call(self._service_topic,
service_request)
if not service_result.success:
self._failed = True
Logger.logwarn(
'[SpiralAroundPointState]: {} Calling \'{}\' was not successful: {} for reference position (x,y,z) = ({},{},{})'
.format(time_portion, self._service_topic,
str(service_result.message),
service_request.reference.position.x,
service_request.reference.position.y,
service_request.reference.position.z))
else:
Logger.loginfo('[SpiralAroundPointState]: {}'.format(
str(service_result.message)))
except Exception as e:
Logger.logerr(
'[SpiralAroundPointState]: Failed to call \'{}\' service request: {}'
.format(self._service_topic, str(e)))
self._failed = True
if self._failed:
return 'failed'
def on_enter(self, userdata):
# This method is called when the state becomes active, i.e. a transition from another state to this one is taken.
# It is primarily used to start actions which are associated with this state.
self._start_point = userdata.start_spiral_tracker_point
start_direction = self._start_point.yaw - math.pi / 2.0
self._spiral_points = self.get_spiral_points(
[self._start_point.x, self._start_point.y], START_RADIUS,
MAX_RADIUS, RADIUS_STEP, start_direction, ROT_STEP, NUM_ROTATIONS)
self._start_time = rospy.get_time()
self._failed = False
self.received = False
def get_spiral_points(self, pos, start_radius, max_radius, radius_step,
start_direction, rot_step, num_rotations):
rot = start_direction
radius = start_radius
#speed = len_step/duration_s
num_points = (2.0 * math.pi * num_rotations) / rot_step
spiral_points = []
if PLOT:
plt.plot([pos[0]], [pos[0]], 'og')
for i in range(int(num_points)):
radius += radius_step
radius = min(radius, max_radius)
radius = max(radius, start_radius)
rot += rot_step
posx = pos[0] + radius * math.cos(rot)
posy = pos[1] + radius * math.sin(rot)
spiral_points.append([posx, posy])
if PLOT:
plt.plot([posx], [posy], '.r')
if PLOT:
plt.axis('equal')
plt.show()
return spiral_points
class GetPoseInFrameState(EventState):
'''
Transforms the given pose from its current frame to a target frame.
-- target_frame string The frame to which the pose will be transformed
For example, 'utorso', 'world', etc.
># pose_in PoseStamped The pose to be transformed to a target frame
Its current frame is included in the message
#> pose_out PoseStamped The same pose but in the target ref frame
<= done The pose was transformed successfully.
<= failed Failed to transform pose to target frame.
'''
def __init__(self, target_frame):
'''Constructor'''
super(GetPoseInFrameState, self).__init__(outcomes=['done', 'failed'],
input_keys=['pose_in'],
output_keys=['pose_out'])
self._target_frame = target_frame
# Set up service for requesting the transformation
self._service_topic = '/worldmodel_main/get_pose_in_frame_service'
self._srv = ProxyServiceCaller({self._service_topic: GetPoseInFrame})
self._srv_result = None
self._failed = False
self._done = False
def execute(self, userdata):
if self._failed:
return 'failed'
if self._done:
return 'done'
error = self._srv_result.error_code # message is empty if success
if error:
Logger.logwarn('Pose transformation failed because: %s' % error)
self._failed = True
return 'failed'
else:
userdata.pose_out = self._srv_result.transformed_pose
return 'done'
def on_enter(self, userdata):
self._failed = False
self._done = False
# Send the transformation request
try:
Logger.loginfo(
'Requesting transformation from frame %s to %s' %
(userdata.pose_in.header.frame_id, self._target_frame))
request = GetPoseInFrameRequest(pose=userdata.pose_in,
target_frame=self._target_frame)
self._srv_result = self._srv.call(self._service_topic, request)
except Exception as e:
Logger.logwarn('Failed to send service request:\n%s' % str(e))
self._failed = True
return
class AttachObjectState(EventState):
'''
Requests a grasp for a template from the template server.
># template_id int ID of the template to attach.
># hand_side string Hand side to which the template
should be attached {left, right}
#> template_pose PoseStamped Pose of the attached template in the
reference frame of the wrist (r/l_hand)
<= done Template has been attached.
<= failed Failed to attach template.
'''
def __init__(self):
'''Constructor'''
super(AttachObjectState, self).__init__(outcomes = ['done', 'failed'],
input_keys = ['template_id', 'hand_side'],
output_keys = ['template_pose'])
# Set up service for attaching object template
self._service_topic = "/stitch_object_template"
self._srv = ProxyServiceCaller({
self._service_topic: SetAttachedObjectTemplate})
# Set up subscribers for listening to the latest wrist poses
self._wrist_pose_topics = dict()
self._wrist_pose_topics['left'] = '/flor/l_arm_current_pose'
self._wrist_pose_topics['right'] = '/flor/r_arm_current_pose'
self._sub = ProxySubscriberCached({
self._wrist_pose_topics['left']: PoseStamped,
self._wrist_pose_topics['right']: PoseStamped})
self._sub.make_persistant(self._wrist_pose_topics['left'])
self._sub.make_persistant(self._wrist_pose_topics['right'])
self._failed = False
def execute(self, userdata):
if self._failed:
return 'failed'
return 'done'
def on_enter(self, userdata):
self._failed = False
# The frame ID to which the object template will be attached to
if userdata.hand_side == 'left':
frame_id = "l_hand"
elif userdata.hand_side == 'right':
frame_id = "r_hand"
else:
Logger.logwarn('Unexpected value of hand side: %s Expected {left, right}' % str(userdata.hand_side))
self._failed = True
return
# Get the current wrist pose, which is required by the stitching service
try:
wrist_pose_topic = self._wrist_pose_topics[userdata.hand_side]
current_wrist_pose = self._sub.get_last_msg(wrist_pose_topic)
# Set the PoseStamped message's frame ID to the one
# that the object template should be attached to:
current_wrist_pose.header.frame_id = frame_id
except Exception as e:
Logger.logwarn('Failed to get the latest hand pose:\n%s' % str(e))
self._failed = True
return
# Send the stiching request and write response to userdata
try:
request = SetAttachedObjectTemplateRequest(template_id = userdata.template_id,
pose = current_wrist_pose)
self._srv_result = self._srv.call(self._service_topic, request)
# The result contains the template's pose, mass, and center of mass
userdata.template_pose = self._srv_result.template_pose
except Exception as e:
Logger.logwarn('Failed to send service call:\n%s' % str(e))
self._failed = True
return
class GetTemplateAffordanceState(EventState):
'''
Requests affordance information for a template from the template server.
-- identifier string Name to identify the requested affordance.
># template_id int ID of the template to manipulate.
># hand_side string Hand side for which the state will ask the template server for affordances {left, right, both}
#> affordance Affordance A message as defined in vigir_object_template_msgs containing all required information.
<= done Affordance data is available.
<= failed Failed to get affordance information.
<= not_available The requested preference is not available (index too high).
'''
def __init__(self, identifier):
'''Constructor'''
super(GetTemplateAffordanceState,
self).__init__(outcomes=['done', 'failed', 'not_available'],
input_keys=['template_id', 'hand_side'],
output_keys=['affordance'])
self._service_topic = "/template_info"
self._srv = ProxyServiceCaller(
{self._service_topic: GetTemplateStateAndTypeInfo})
self._srv_result = None
self._identifier = identifier
self._hand_side = None
self._failed = False
self._done = False
def execute(self, userdata):
if self._failed or self._srv_result is None:
return 'failed'
if self._done:
return 'done'
try:
choices = self._srv_result.template_type_information.affordances
if not self._identifier in [a.name for a in choices]:
return 'not_available'
chosen_affordance = next(a for a in choices
if a.name == self._identifier)
userdata.affordance = chosen_affordance
Logger.loginfo('Using grasp with ID: %s' %
str(chosen_affordance.id))
except Exception as e:
Logger.logwarn(
'Failed to retrieve affordance information from service result:\n%s'
% str(e))
self._failed = True
return 'failed'
self._done = True
return 'done'
def on_enter(self, userdata):
self._failed = False
self._done = False
self._srv_result = None
if userdata.hand_side == 'left':
self._hand_side = GetTemplateStateAndTypeInfoRequest.LEFT_HAND
elif userdata.hand_side == 'right':
self._hand_side = GetTemplateStateAndTypeInfoRequest.RIGHT_HAND
elif userdata.hand_side == 'both':
self._hand_side = GetTemplateStateAndTypeInfoRequest.BOTH_HANDS
else:
Logger.logwarn(
'Unexpected value of hand side: %s Expected {left, right, both}'
% str(userdata.hand_side))
try:
self._srv_result = self._srv.call(
self._service_topic,
GetTemplateStateAndTypeInfoRequest(userdata.template_id,
self._hand_side))
except Exception as e:
Logger.logwarn('Failed to send service call:\n%s' % str(e))
self._failed = True
class AddGazeboModel(EventState):
'''
Call a service given a name.
-- model_name string Name of the calling service
># gazebo_model_pose Pose gaezbo_model_pose
<= done Affordance data is available.
<= failed Failed to get affordance information.
'''
BOX_MALE = 'box_male'
BOX_FEMALE = 'box_female'
def __init__(self, model_name):
'''Constructor'''
super(AddGazeboModel, self).__init__(outcomes = ['done', 'failed'],
input_keys = ['gazebo_model_pose'])
self._srv_result = None
self._service_name = '/gazebo/spawn_urdf_model'
self._failed = False
self._done = False
self._srv = ProxyServiceCaller({self._service_name:SpawnModel})
self._model_reference_frame = 'base'
self._gazebo_model_pose = None
self._model_name = model_name
def execute(self, userdata):
if self._failed or self._srv_result is None:
return 'failed'
if self._done:
return 'done'
self._done = True
return 'done'
def on_enter(self, userdata):
self._gazebo_model_pose = userdata.gazebo_model_pose
if self._gazebo_model_pose is not None:
if self._model_name == "box_male":
# Get male box Path
model_path = rospkg.RosPack().get_path('birl_baxter_description') + "/urdf/box/"
# Load male box SDF
self._model_xml = ''
with open(model_path + "box_male/robots/box_male.URDF", "r") as box_male_file:
self._model_xml = box_male_file.read().replace('\n', '')
try:
self._srv_result = self._srv.call(self._service_name,
SpawnModelRequest(self._model_name, self._model_xml, "/",
self._gazebo_model_pose, self._model_reference_frame))
rospy.loginfo('loading male box succesfully')
except Exception, e:
rospy.logerr("Spawn URDF service call failed: {0}".format(e))
self._failed = True
elif self._model_name == "box_female":
# get path
model_path = rospkg.RosPack().get_path('birl_baxter_description') + "/urdf/box/"
# Load female box URDF
box_female_xml = ''
with open(model_path + "box_female/robots/box_female.URDF", "r") as box_female_file:
box_female_xml = box_female_file.read().replace('\n', '')
rospy.wait_for_service('/gazebo/spawn_urdf_model')
try:
self._srv_result = self._srv.call(self._service_name,
SpawnModelRequest(self._model_name, self._model_xml, "/",
self._gazebo_model_pose, self._model_reference_frame))
except Exception, e:
rospy.logerr("Spawn URDF service call failed: {0}".format(e))
self._failed = True
else:
class ServiceSetBrickDetectionTypeAndWallLayer(EventState):
'''
State for saving the plans for robots in Wall challenge in MBZIRC 2020 into plan_keeper node.
-- service_topic_see_type string Service topic name for seting see WALL/BRICK..
-- service_topic_layer string Service topic name for setting layer of wall
># see_type what to see
># wall_layer in which layer we are placing
<= successed Whenever the calling for successful.
<= failed When the calling failed.
'''
BRICK_SEE_EVERYTHING = 0
BRICK_SEE_RED = 1
BRICK_SEE_GREEN = 2
BRICK_SEE_BLUE = 3
BRICK_SEE_ORANGE = 4
BRICK_SEE_WALL = 8
BRICK_SEE_WALL_HAVING_RED = 9
BRICK_SEE_WALL_HAVING_GREEN = 10
BRICK_SEE_WALL_HAVING_BLUE = 11
LAYER_0 = 0
LAYER_1 = 1
def __init__(self, service_topic_see_type, service_topic_layer):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ServiceSetBrickDetectionTypeAndWallLayer,
self).__init__(input_keys=['see_type', 'wall_layer'],
outcomes=['successed', 'failed'])
# Store state parameter for later use.
self._service_topic_see_type = rospy.resolve_name(
service_topic_see_type)
self._service_topic_layer = rospy.resolve_name(service_topic_layer)
# Create proxy service client
wait_time = 30
rospy.loginfo("waiting for service " + self._service_topic_see_type)
rospy.wait_for_service(self._service_topic_see_type, wait_time)
self._srv_detection_type = ProxyServiceCaller(
{self._service_topic_see_type: mbzirc_msgs.srv.DetectionType})
self._srv_layer = ProxyServiceCaller(
{self._service_topic_layer: mbzirc_msgs.srv.DetectionType})
self._failed = 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.
if self._failed:
return 'failed'
else:
return 'successed'
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._failed = False
# Check if the ProxyServiceCaller has been registered
if not self._srv_detection_type.is_available(
self._service_topic_see_type):
Logger.logerr(
'[ServiceSetBrickDetectionType]: Topic \'{}\' not yet registered!'
.format(self._service_topic_see_type))
self._failed = True
return
try:
service_request_see_type = mbzirc_msgs.srv.DetectionTypeRequest()
service_request_see_type.type = userdata.see_type
Logger.loginfo(
'[ServiceSetBrickDetectionType]: setting see type %s' %
(str(service_request_see_type.type)))
service_result_see_type = self._srv_detection_type.call(
self._service_topic_see_type, service_request_see_type)
if not service_result_see_type.success:
self._failed = True
Logger.logwarn(
'[ServiceSetBrickDetectionType]: Calling \'{}\' was not successful: {}'
.format(self._service_topic_see_type,
str(service_result_see_type.message)))
else:
Logger.loginfo(service_result_see_type.message)
except Exception as e:
Logger.logerr(
'[ServiceSetBrickDetectionType]:Failed to call \'{}\' service request: {}'
.format(self._service_topic_see_type, str(e)))
self._failed = True
#part for setting layer
if not self._srv_layer.is_available(self._service_topic_layer):
Logger.logerr(
'[ServiceSetBrickDetectionType]: Topic \'{}\' not yet registered!'
.format(self._service_topic_layer))
self._failed = True
return
try:
service_request_layer = mbzirc_msgs.srv.DetectionTypeRequest()
service_request_layer.type = userdata.wall_layer
Logger.loginfo(
'[ServiceSetBrickDetectionType]: setting slayer to %s' %
(str(service_request_see_type.type)))
service_result_layer = self._srv_layer.call(
self._service_topic_layer, service_request_layer)
if not service_result_layer.success:
self._failed = True
Logger.logwarn(
'[ServiceSetBrickDetectionType]: Calling \'{}\' was not successful: {}'
.format(self._service_topic_layer,
str(service_result_layer.message)))
else:
Logger.loginfo(service_result_layer.message)
except Exception as e:
Logger.logerr(
'[ServiceSetBrickDetectionType]:Failed to call \'{}\' service request: {}'
.format(self._service_topic_layer, str(e)))
self._failed = True
class AttachObjectState(EventState):
'''
Requests a grasp for a template from the template server.
># template_id int ID of the template to attach.
># hand_side string Hand side to which the template
should be attached {left, right}
#> template_pose PoseStamped Pose of the attached template in the
reference frame of the wrist (r/l_hand)
<= done Template has been attached.
<= failed Failed to attach template.
'''
def __init__(self):
'''Constructor'''
super(AttachObjectState,
self).__init__(outcomes=['done', 'failed'],
input_keys=['template_id', 'hand_side'],
output_keys=['template_pose'])
# Set up service for attaching object template
self._service_topic = "/stitch_object_template"
self._srv = ProxyServiceCaller(
{self._service_topic: SetAttachedObjectTemplate})
# Set up subscribers for listening to the latest wrist poses
self._wrist_pose_topics = dict()
self._wrist_pose_topics['left'] = '/flor/l_arm_current_pose'
self._wrist_pose_topics['right'] = '/flor/r_arm_current_pose'
self._sub = ProxySubscriberCached({
self._wrist_pose_topics['left']:
PoseStamped,
self._wrist_pose_topics['right']:
PoseStamped
})
self._sub.make_persistant(self._wrist_pose_topics['left'])
self._sub.make_persistant(self._wrist_pose_topics['right'])
self._failed = False
def execute(self, userdata):
if self._failed:
return 'failed'
return 'done'
def on_enter(self, userdata):
self._failed = False
# The frame ID to which the object template will be attached to
if userdata.hand_side == 'left':
frame_id = "l_hand"
elif userdata.hand_side == 'right':
frame_id = "r_hand"
else:
Logger.logwarn(
'Unexpected value of hand side: %s Expected {left, right}' %
str(userdata.hand_side))
self._failed = True
return
# Get the current wrist pose, which is required by the stitching service
try:
wrist_pose_topic = self._wrist_pose_topics[userdata.hand_side]
current_wrist_pose = self._sub.get_last_msg(wrist_pose_topic)
# Set the PoseStamped message's frame ID to the one
# that the object template should be attached to:
current_wrist_pose.header.frame_id = frame_id
except Exception as e:
Logger.logwarn('Failed to get the latest hand pose:\n%s' % str(e))
self._failed = True
return
# Send the stiching request and write response to userdata
try:
request = SetAttachedObjectTemplateRequest(
template_id=userdata.template_id, pose=current_wrist_pose)
self._srv_result = self._srv.call(self._service_topic, request)
# The result contains the template's pose, mass, and center of mass
userdata.template_pose = self._srv_result.template_pose
except Exception as e:
Logger.logwarn('Failed to send service call:\n%s' % str(e))
self._failed = True
return
class CallBirlService(EventState):
'''
Call a service given a name.
-- service_name string Name of the calling service
<= done Affordance data is available.
<= failed Failed to get affordance information.
'''
def __init__(self, service_name):
'''Constructor'''
super(CallBirlService, self).__init__(outcomes = ['done', 'failed'])
self._srv_result = None
self._service_name = service_name
self._hand_side = None
self._failed = False
self._done = False
self._srv = ProxyServiceCaller({self._service_name:Add_Gazebo_Model})
self._model_reference_frame = None
self._pose = None
self._model_name = None
def execute(self, userdata):
if self._failed or self._srv_result is None:
return 'failed'
if self._done:
return 'done'
self._done = True
return 'done'
def on_enter(self, userdata):
self._failed = False
self._done = False
self._srv_result = None
self._pose = Pose()
self._pose.position.x = 0.6
self._pose.position.y = 0
self._pose.position.z = -0.115
self._pose.orientation.x = 0
self._pose.orientation.y = 0
self._pose.orientation.z = 0
self._pose.orientation.w = 1
self._model_reference_frame = String()
self._model_reference_frame.data = "base"
self._model_name = String()
self._model_name.data = "box_female"
try:
self._srv_result = self._srv.call(self._service_name, Add_Gazebo_ModelRequest(self._model_name,
self._pose,
self._model_reference_frame))
except Exception as e:
Logger.logwarn('Failed to send service call:\n%s' % str(e))
self._failed = True
class GetTemplateUsabilityState(EventState):
'''
Requests a grasp for a template from the template server.
-- usability_name string Name of the template usability (e.g. 'tip')
-- usability_id int ID of the template usability.
Set to 100 if requesting by name instead.
># template_id int ID of the template of interest
#> usability_pose PoseStamped Pose of the template's usability in the
reference frame of the wrist (r/l_hand)
<= done The usability has been retrieved.
<= failed Failed to get the template's usability.
'''
def __init__(self, usability_name='', usability_id=100):
'''Constructor'''
super(GetTemplateUsabilityState,
self).__init__(outcomes=['done', 'failed'],
input_keys=['template_id'],
output_keys=['usability_pose'])
# Set up service for attaching object template
self._service_topic = "/usability_pose"
self._srv = ProxyServiceCaller(
{self._service_topic: GetUsabilityInWristFrame})
self._usability_id = usability_id
self._usability_name = usability_name
self._failed = False
def execute(self, userdata):
'''Move along people, nothing to see here!'''
if self._failed:
return 'failed'
else:
return 'done'
def on_enter(self, userdata):
'''Send the usability request and write response to userdata.'''
self._failed = False
if self._usability_id != 100 and self._usability_name != '':
Logger.logwarn(
'Not clear if you are requesting usability by ID (%d) + \
or name (%s)' % (self._usability_id, self._usability_name))
try:
request = GetUsabilityInWristFrameRequest(
template_id=userdata.template_id,
usability_id=self._usability_id,
usability_name=self._usability_name)
self._srv_result = self._srv.call(self._service_topic, request)
userdata.usability_pose = self._srv_result.wrist_usability
print request, self._srv_result.wrist_usability # debug
except Exception as e:
Logger.logwarn('Failed to send service call:\n%s' % str(e))
self._failed = True
return
class QueryJointPositionsState(EventState):
'''
Retrieves the current positions of the joints of the specified planning group.
-- side string One of left, right
-- controller string Specifify the trajectory controller suffix
Example: 'traj_hybrid_controller'
#> joint_config dict Trajectory joint positions at current time.
joint_names string[] : joint_values[]
<= retrieved Successfully determined the current joint values.
<= failed Failed to send service call or got no result.
'''
TRAJ_CONTROLLER = 'traj_controller'
HYBRID_CONTROLLER = 'traj_hybrid_controller'
def __init__(self, side, controller='traj_controller'):
'''
Constructor
'''
super(QueryJointPositionsState,
self).__init__(outcomes=['retrieved', 'failed'],
output_keys=['joint_config'])
if not rospy.has_param("behavior/joint_controllers_name"):
Logger.logerr(
"Need to specify parameter behavior/joint_controllers_name at the parameter server"
)
return
controller_namespace = rospy.get_param(
"behavior/joint_controllers_name")
self._srv_topic = '/' + controller_namespace + '/' + side + '_arm_' + \
controller + '/query_state'
self._srv = ProxyServiceCaller({self._srv_topic: QueryTrajectoryState})
self._srv_result = None
self._failed = False
def execute(self, userdata):
'''
Execute this state
'''
if self._failed:
return 'failed'
else:
return 'retrieved'
def on_enter(self, userdata):
self._failed = False
try:
self._srv_result = self._srv.call(
self._srv_topic,
QueryTrajectoryStateRequest(time=rospy.Time.now()))
joint_names = self._srv_result.name
joint_values = list(
self._srv_result.position) # cast tuple to list
userdata.joint_config = {
'joint_names': joint_names,
'joint_values': joint_values
}
except Exception as e:
Logger.logwarn('Could not retrieve current joint positions!\n%s' %
str(e))
self._failed = True
class ServiceGetWallBrickPositionsState(EventState):
'''
State for calling std_srvs/Trigger type of services.
-- service_topic string Service_topic_name.
<= finished Whenever the calling for successful.
<= failed When the calling failed.
'''
def __init__(self, service_topic):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ServiceGetWallBrickPositionsState,
self).__init__(outcomes=['finished', 'failed'],
output_keys=['mapped_objects'])
# Store state parameter for later use.
self._service_topic = rospy.resolve_name(service_topic)
# Create proxy service client
self._srv = ProxyServiceCaller(
{self._service_topic: brick_mapping.srv.GetMappedArenaObjects})
self._failed = 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.
if self._failed:
return 'failed'
else:
return 'finished'
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._failed = False
# Check if the ProxyServiceCaller has been registered
if not self._srv.is_available(self._service_topic):
Logger.logerr(
'[ServiceGetWallBrickPositionsState]: Topic \'{}\' not yet registered!'
.format(self._service_topic))
self._failed = True
return
try:
service_request = brick_mapping.srv.GetMappedArenaObjectsRequest()
service_result = self._srv.call(self._service_topic,
service_request)
Logger.loginfo(
'[ServiceGetWallBrickPositionsState]: got mapped_objects from mapping'
)
Logger.loginfo(str(service_result.mapped_objects))
userdata.mapped_objects = service_result.mapped_objects
if not service_result.success:
self._failed = True
Logger.logwarn(
'[ServiceGetWallBrickPositionsState]: Calling \'{}\' was not successful: {}'
.format(self._service_topic, str(service_result.message)))
else:
Logger.loginfo('[ServiceGetWallBrickPositionsState]: ' +
str(service_result.message))
except Exception as e:
Logger.logerr(
'[ServiceGetWallBrickPositionsState]: Failed to call \'{}\' service request: {}'
.format(self._service_topic, str(e)))
self._failed = True
class GetTemplatePregraspState(EventState):
'''
Requests pregrasp information for a template from the template server.
># template_id int ID of the template to grasp.
># hand_side string Hand side for which the state will ask the template server for pregrasp poses {left, right, both}
># preference int Index of the preferred pregrasp.
#> pre_grasp PoseStamped Target endeffector pose for pregrasp.
<= done Pregrasp data is available.
<= failed Failed to get pregrasp information.
<= not_available The requested preference is not available (index too high).
'''
def __init__(self):
'''Constructor'''
super(GetTemplatePregraspState, self).__init__(outcomes = ['done', 'failed', 'not_available'],
input_keys = ['template_id', 'hand_side', 'preference'],
output_keys = ['pre_grasp'])
self._service_topic = "/instantiated_grasp_info"
self._srv = ProxyServiceCaller({self._service_topic: GetInstantiatedGraspInfo})
self._srv_result = None
self._hand_side = None
self._failed = False
self._done = False
self._not_available = False
def execute(self, userdata):
if self._failed or self._srv_result is None:
return 'failed'
if self._done:
return 'done'
if self._not_available:
return 'not_available'
try:
options = self._srv_result.pre_grasp_information.grasps
if len(options) <= userdata.preference:
Logger.logwarn('The option with index %d is not available. There are %d options total.' % (userdata.preference, len(options)))
self._not_available = True
return 'not_available'
chosen_pregrasp = options[userdata.preference]
userdata.pre_grasp = chosen_pregrasp.grasp_pose
Logger.loginfo('Using grasp with ID: %s' % str(chosen_pregrasp.id))
except Exception as e:
Logger.logwarn('Failed to retrieve grasp information from service result:\n%s' % str(e))
self._failed = True
return 'failed'
self._done = True
return 'done'
def on_enter(self, userdata):
self._failed = False
self._done = False
self._not_available = False
if userdata.hand_side == 'left':
self._hand_side = GetInstantiatedGraspInfoRequest.LEFT_HAND
elif userdata.hand_side == 'right':
self._hand_side = GetInstantiatedGraspInfoRequest.RIGHT_HAND
elif userdata.hand_side == 'both':
self._hand_side = GetInstantiatedGraspInfoRequest.BOTH_HANDS
else:
Logger.logwarn('Unexpected value of hand side: %s Expected {left, right, both}' % str(userdata.hand_side))
try:
self._srv_result = self._srv.call(self._service_topic, GetInstantiatedGraspInfoRequest(int(userdata.template_id), self._hand_side))
except Exception as e:
Logger.logwarn('Failed to send service call:\n%s' % str(e))
self._failed = True
class ServiceScanArena(EventState):
'''
State for scanning arena in Wall challenge in MBZIRC 2020.
-- service_topic string Service_topic_name.
># scanning_altitude int16 Time that remains to finish the challenge.
<= successed Whenever the calling for successful.
<= failed When the calling failed.
'''
def __init__(self, service_topic):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ServiceScanArena, self).__init__(
input_keys=[
'scanning_robots', 'scanning_altitude', 'scanning_speed',
'scanning_max_acc', 'scanning_turning_speed', 'robot_id'
],
output_keys=['scanning_trajectory', 'scanning_trajectory_start'],
outcomes=['successed', 'failed'])
# Store state parameter for later use.
self._service_topic = rospy.resolve_name(service_topic)
# Create proxy service client
self._srv = ProxyServiceCaller(
{self._service_topic: arena_scan_planner.srv.ArenaScanTrajectory})
self._failed = 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.
if self._failed:
return 'failed'
else:
return 'successed'
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._failed = False
# Check if the ProxyServiceCaller has been registered
if not self._srv.is_available(self._service_topic):
Logger.logerr(
'[ServiceScanArena]: Topic \'{}\' not yet registered!'.format(
self._service_topic))
self._failed = True
return
try:
Logger.loginfo('[ServiceScanArena]: create service_request for ' +
self._service_topic)
service_request = arena_scan_planner.srv.ArenaScanTrajectoryRequest(
)
service_request.speed = userdata.scanning_speed
service_request.acc = userdata.scanning_max_acc
service_request.turning_speed = userdata.scanning_turning_speed
service_request.altitude = userdata.scanning_altitude
service_request.initial_speed = 0
service_request.loop = False
service_request.arena_part_id = min(userdata.robot_id + 1,
userdata.scanning_robots)
service_request.num_robots = userdata.scanning_robots
service_result = self._srv.call(self._service_topic,
service_request)
sweep_path = service_result.sweep_path
userdata.scanning_trajectory = sweep_path
userdata.scanning_trajectory_start = sweep_path[0]
Logger.logwarn('"[ServiceScanArena]: sweep path has %d points' %
(len(sweep_path)))
Logger.logwarn(
'"[ServiceScanArena]: sweep path start is at %f %f %f %f' %
(sweep_path[0].position.x, sweep_path[0].position.y,
sweep_path[0].position.z, sweep_path[0].heading))
if not service_result.ok:
self._failed = True
Logger.logwarn(
'"[ServiceScanArena]: Calling \'{}\' was not successful: {}'
.format(self._service_topic, str(service_result.message)))
else:
Logger.loginfo(service_result.message)
except Exception as e:
Logger.logerr(
'"[ServiceScanArena]: Failed to call \'{}\' service request: {}'
.format(self._service_topic, str(e)))
self._failed = True
class ServiceGetNextBrickInPlan(EventState):
'''
State for getting a plan for next brick in Wall challenge in MBZIRC 2020.
-- service_topic string Service_topic_name.
-- flying_altitude float Altitude when the robot will move.
#> grasp_position TrackerPoint Position with bricks.
#> placing_position TrackerPoint Position where to place brick after grasping.
#> wall_position TrackerPoint Position where the robot should go before calling dropping.
#> brick_type int8 Type of the brick.
#> brick_id int16 Id of the brick.
<= successed Whenever the calling for successful.
<= failed When the calling failed.
<= finished When plan is finished
'''
def __init__(self, service_topic, flying_altitude):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ServiceGetNextBrickInPlan,
self).__init__(output_keys=[
'grasp_position', 'placing_position', 'drop_wait_position',
'next_to_wall_wait_position', 'brick_type',
'brick_see_wall_typed', 'brick_id', 'wall_layer'
],
outcomes=['successed', 'finished', 'failed'])
# Store state parameter for later use.
self._service_topic = rospy.resolve_name(service_topic)
self._robot_name = rospy.get_namespace()[
1:-1] # removing backslash from beggining and end
self._flying_altitude = flying_altitude
# Create proxy service client
self._srv = ProxyServiceCaller(
{self._service_topic: plan_keeper.srv.BrickPlan})
self._failed = False
self._finished = 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.
if self._finished:
return 'finished'
if self._failed:
return 'failed'
else:
return 'successed'
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._failed = False
self._finished = False
# Check if the ProxyServiceCaller has been registered
if not self._srv.is_available(self._service_topic):
Logger.logerr(
'ProxyServiceCaller: Topic \'{}\' not yet registered!'.format(
self._service_topic))
self._failed = True
return
try:
service_request = plan_keeper.srv.BrickPlanRequest()
service_request.robot_name = self._robot_name
service_result = self._srv.call(self._service_topic,
service_request)
self._finished = service_result.finished
if not service_result.success:
self._failed = True
Logger.logwarn('Calling \'{}\' was not successful: {}'.format(
self._service_topic, str(service_result.message)))
else:
Logger.loginfo('[ServiceGetNextBrickInPlan]: {}'.format(
service_result.message))
service_result.grasp_position.position.z = self._flying_altitude
userdata.grasp_position = service_result.grasp_position
userdata.placing_position = service_result.drop_position
userdata.brick_type = service_result.brick_type
userdata.brick_id = service_result.brick_id
userdata.wall_layer = service_result.wall_layer
userdata.brick_see_wall_typed = service_result.brick_see_wall_typed
userdata.next_to_wall_wait_position = service_result.next_to_wall_wait_position
drop_wait_position = copy.deepcopy(
service_result.drop_wait_position)
drop_wait_position.position.z = self._flying_altitude
userdata.drop_wait_position = drop_wait_position
except Exception as e:
Logger.logerr('Failed to call \'{}\' service request: {}'.format(
self._service_topic, str(e)))
self._failed = True
class ServiceGetExistingCtopPlan(EventState):
'''
State for getting an existing plan (planned by some other drone) for robots in Wall challenge in MBZIRC 2020.
-- service_topic string Service_topic_name.
#> plans Plans how to divide robots in challenge.
<= successed Whenever the calling for successful.
<= failed When the calling failed.
'''
def __init__(self, service_topic):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ServiceGetExistingCtopPlan,
self).__init__(input_keys=[],
output_keys=['plans'],
outcomes=['successed', 'failed'])
# Store state parameter for later use.
self._service_topic = rospy.resolve_name(service_topic)
# Create proxy service client
self._srv = ProxyServiceCaller(
{self._service_topic: ctop_planner.srv.GetExistingPlanCTOP})
self._failed = 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.
if self._failed:
return 'failed'
else:
return 'successed'
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._failed = False
# Check if the ProxyServiceCaller has been registered
if not self._srv.is_available(self._service_topic):
Logger.logerr(
'ProxyServiceCaller: Topic \'{}\' not yet registered!'.format(
self._service_topic))
self._failed = True
return
try:
service_request = ctop_planner.srv.GetExistingPlanCTOPRequest()
service_result = self._srv.call(self._service_topic,
service_request)
if not service_result.success:
self._failed = True
Logger.logwarn('Calling \'{}\' was not successful: {}'.format(
self._service_topic, str(service_result.message)))
else:
Logger.loginfo(service_result.message)
userdata.plans = service_result.plans
except Exception as e:
Logger.logerr('Failed to call \'{}\' service request: {}'.format(
self._service_topic, str(e)))
self._failed = True
class ServiceGetCtopPlan(EventState):
'''
State for getting a plan for robots in Wall challenge in MBZIRC 2020.
-- service_topic string Service_topic_name.
># num_robots int16 Number of robots for the planner.
># remaining_time_s int16 Time that remains to finish the challenge.
># max_planning_time_s int16 Time until we want to finish the challenge.
># times_finish_actual_s int16[] Time until the robots will be available.
># builded_brick_ids int16[] Array of already placed bricks.
#> plans Plans how to divide robots in challenge.
<= successed Whenever the calling for successful.
<= failed When the calling failed.
'''
def __init__(self, service_topic):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ServiceGetCtopPlan,
self).__init__(input_keys=[
'num_robots', 'remaining_time_s', 'max_planning_time_s',
'times_finish_actual_s', 'builded_brick_ids'
],
output_keys=['plans'],
outcomes=['successed', 'failed'])
# Store state parameter for later use.
self._service_topic = rospy.resolve_name(service_topic)
# Create proxy service client
self._srv = ProxyServiceCaller(
{self._service_topic: ctop_planner.srv.PlanCTOP})
self._failed = 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.
if self._failed:
return 'failed'
else:
return 'successed'
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._failed = False
# Check if the ProxyServiceCaller has been registered
if not self._srv.is_available(self._service_topic):
Logger.logerr(
'ProxyServiceCaller: Topic \'{}\' not yet registered!'.format(
self._service_topic))
self._failed = True
return
try:
service_request = ctop_planner.srv.PlanCTOPRequest()
service_request.num_robots = userdata.num_robots
service_request.remaining_time_s = userdata.remaining_time_s
service_request.max_planning_time_s = userdata.max_planning_time_s
service_request.times_finish_actual_s = userdata.times_finish_actual_s
service_request.builded_brick_ids = userdata.builded_brick_ids
service_result = self._srv.call(self._service_topic,
service_request)
if not service_result.success:
self._failed = True
Logger.logwarn('Calling \'{}\' was not successful: {}'.format(
self._service_topic, str(service_result.message)))
else:
Logger.loginfo(service_result.message)
userdata.plans = service_result.plans
except Exception as e:
Logger.logerr('Failed to call \'{}\' service request: {}'.format(
self._service_topic, str(e)))
self._failed = True
class CurrentJointPositionsState(EventState):
'''
Retrieves the current positions of the joints of the specified planning group.
-- planning_group string Name of the planning group.
#> joint_positions float[] Current joint values. Length of the list depends on the number of joints of the planning group.
<= retrieved Successfully determined the current joint values.
<= failed Failed to send service call or got no result.
'''
def __init__(self, planning_group):
'''
Constructor
'''
super(CurrentJointPositionsState, self).__init__(outcomes=['retrieved', 'failed'],
output_keys=['joint_positions'])
self._srv_topic = '/flor/planning/upper_body/get_group_state'
self._srv = ProxyServiceCaller({self._srv_topic: GetCurrentPlanningGroupState})
self._planning_group = planning_group
self._srv_result = None
self._failed = False
def execute(self, userdata):
'''
Execute this state
'''
if self._failed:
return 'failed'
if self._srv_result is not None:
userdata.joint_positions = self._srv_result.position
if len(self._srv_result.position) == 0:
Logger.logwarn("Lookup of joint positions of group %s failed" % self._planning_group)
self._failed = True
return 'failed'
else:
return 'retrieved'
def on_enter(self, userdata):
self._failed = False
try:
self._srv_result = self._srv.call(
self._srv_topic,
GetCurrentPlanningGroupStateRequest(self._planning_group))
except Exception as e:
Logger.logwarn('Could not retrieve current joint positions!')
rospy.logwarn(str(e))
self._failed = True
class ServiceGoToAltitudeState(EventState):
'''
State for calling goto_altitude service in MRS system.
-- service_topic string Service_topic_name.
-- control_manager_diagnostics_topic string control_manager_diagnostics_topic.
># goal double Target altitude.
<= successed Whenever the calling for successful.
<= failed When the calling failed.
'''
def __init__(self, service_topic, control_manager_diagnostics_topic):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ServiceGoToAltitudeState, self).__init__(input_keys=['goal'],
outcomes = ['successed', 'failed'])
# Store state parameter for later use.
self._service_topic = rospy.resolve_name(service_topic)
self._control_manager_diagnostics_topic = rospy.resolve_name(control_manager_diagnostics_topic)
# Create proxy service client
self._srv = ProxyServiceCaller({self._service_topic: mrs_msgs.srv.Vec1})
(msg_path, msg_topic, fn) = rostopic.get_topic_type(self._control_manager_diagnostics_topic)
if msg_topic == self._control_manager_diagnostics_topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub_cont_diag = ProxySubscriberCached({self._control_manager_diagnostics_topic: msg_type})
self._connected = True
Logger.loginfo('[ServiceGoToAltitudeState]: Successfully subscribed to topic %s' % self._control_manager_diagnostics_topic)
self._failed = 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.
if not self._connected:
Logger.logerr('[ServiceGoToAltitudeState]: not connected to %s' % self._control_manager_diagnostics_topic)
return 'failed'
diff_time = rospy.get_time() - self._start_time
if self._sub_cont_diag.has_msg(self._control_manager_diagnostics_topic):
#Logger.loginfo('[ServiceGoToAltitudeState]: has diagnostics msg at time %s' % (str(diff_time)))
message = self._sub_cont_diag.get_last_msg(self._control_manager_diagnostics_topic)
self._sub_cont_diag.remove_last_msg(self._control_manager_diagnostics_topic)
if diff_time >= 1.0 and not message.tracker_status.have_goal:
Logger.loginfo('[ServiceGoToAltitudeState]: Successfully ended following of trajectory %s' % self._control_manager_diagnostics_topic)
return 'successed'
else:
return
if self._failed:
return 'failed'
def on_enter(self, userdata):
# This method is called when the state becomes active, i.e. a transition from another state to this one is taken.
# It is primarily used to start actions which are associated with this state.
self._start_time = rospy.get_time()
if not self._connected:
(msg_path, msg_topic, fn) = rostopic.get_topic_type(self._control_manager_diagnostics_topic)
if msg_topic == self._control_manager_diagnostics_topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub_cont_diag = ProxySubscriberCached({self._control_manager_diagnostics_topic: msg_type})
self._connected = True
Logger.loginfo('[ServiceGoToAltitudeState]: Successfully subscribed to previously unavailable topic %s' % self._control_manager_diagnostics_topic)
else:
Logger.logwarn('[ServiceGoToAltitudeState]: Topic %s still not available, giving up.' % self._control_manager_diagnostics_topic)
if self._connected and self._sub_cont_diag.has_msg(self._control_manager_diagnostics_topic):
Logger.loginfo('[ServiceGoToAltitudeState]: Waiting for msg from topic %s' % self._control_manager_diagnostics_topic)
self._sub_cont_diag.remove_last_msg(self._control_manager_diagnostics_topic)
self._failed = False
# Check if the ProxyServiceCaller has been registered
if not self._srv.is_available(self._service_topic):
Logger.logerr('[ServiceGoToAltitudeState]: ProxyServiceCaller: Topic \'{}\' not yet registered!'.format(self._service_topic))
self._failed = True
return
try:
service_request = mrs_msgs.srv.Vec1Request()
service_request.goal = userdata.goal
Logger.loginfo('[ServiceGoToAltitudeState]: calling goto_altitude %f'%(userdata.goal))
service_result = self._srv.call(self._service_topic, service_request)
if not service_result.success:
self._failed = True
Logger.logwarn('[ServiceGoToAltitudeState]: Calling \'{}\' was not successful: {}'.format(self._service_topic, str(service_result.message)))
else:
Logger.loginfo('[ServiceGoToAltitudeState]: {}'.format(str(service_result.message)))
except Exception as e:
Logger.logerr('[ServiceGoToAltitudeState]: Failed to call \'{}\' service request: {}'.format(self._service_topic, str(e)))
self._failed = True
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 ServiceFollowTrajectory(EventState):
'''
State for start trajectory following
-- service_topic_follow string Service_topic_name to start following
-- service_topic_set_trajectory string Service_topic_name to fill trajectory
># scanning_trajectory Trajectory that is followed
<= successed Whenever the calling for successful.
<= failed When the calling failed.
'''
def __init__(self, service_topic_follow,service_topic_set_trajectory,control_manager_diagnostics_topic):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ServiceFollowTrajectory, self).__init__(input_keys=['scanning_trajectory','frame_id'],
outcomes = ['successed', 'failed'])
# Store state parameter for later use.
self._service_topic_follow = rospy.resolve_name(service_topic_follow)
self._service_topic_set_trajectory = rospy.resolve_name(service_topic_set_trajectory)
self._control_manager_diagnostics_topic = rospy.resolve_name(control_manager_diagnostics_topic)
# Create proxy service client
self._srv_follow = ProxyServiceCaller({self._service_topic_follow: std_srvs.srv.Trigger})
self._srv_set_trajectory = ProxyServiceCaller({self._service_topic_set_trajectory: mrs_msgs.srv.TrajectoryReferenceSrv})
(msg_path, msg_topic, fn) = rostopic.get_topic_type(self._control_manager_diagnostics_topic)
if msg_topic == self._control_manager_diagnostics_topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub_cont_diag = ProxySubscriberCached({self._control_manager_diagnostics_topic: msg_type})
self._connected = True
Logger.loginfo('[ServiceFollowTrajectory]: Successfully subscribed to topic %s' % self._control_manager_diagnostics_topic)
self._failed = 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.
#Logger.logerr('[ServiceFollowTrajectory]: execute')
if not self._connected:
Logger.logerr('[ServiceFollowTrajectory]: not connected to %s' % self._control_manager_diagnostics_topic)
return 'failed'
diff_time = rospy.get_time() - self._start_time
if self._sub_cont_diag.has_msg(self._control_manager_diagnostics_topic):
rospy.loginfo_throttle(5,'[ServiceFollowTrajectory]: has diagnostics msg at time %s' % (str(rospy.get_time())))
message = self._sub_cont_diag.get_last_msg(self._control_manager_diagnostics_topic)
self._sub_cont_diag.remove_last_msg(self._control_manager_diagnostics_topic)
if diff_time >= 1.0 and not message.tracker_status.tracking_trajectory:
Logger.loginfo('[ServiceFollowTrajectory]: Successfully ended following of trajectory %s' % self._control_manager_diagnostics_topic)
return 'successed'
else:
return
if self._failed or diff_time >= len(userdata.scanning_trajectory) * 0.2 * 1.2:
Logger.logerr('Failed follow trajectory \'{}\' within time {} s out of {} s'.format(self._service_topic_follow, str(diff_time),len(userdata.scanning_trajectory) * 0.2 * 1.2))
return 'failed'
def on_enter(self, userdata):
# This method is called when the state becomes active, i.e. a transition from another state to this one is taken.
# It is primarily used to start actions which are associated with this state.
#Logger.logerr('[ServiceFollowTrajectory]: enter')
self._start_time = rospy.get_time()
if not self._connected:
(msg_path, msg_topic, fn) = rostopic.get_topic_type(self._control_manager_diagnostics_topic)
if msg_topic == self._control_manager_diagnostics_topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub_cont_diag = ProxySubscriberCached({self._control_manager_diagnostics_topic: msg_type})
self._connected = True
Logger.loginfo('[ServiceFollowTrajectory]: Successfully subscribed to previously unavailable topic %s' % self._control_manager_diagnostics_topic)
else:
Logger.logwarn('[ServiceFollowTrajectory]: Topic %s still not available, giving up.' % self._control_manager_diagnostics_topic)
if self._connected and self._sub_cont_diag.has_msg(self._control_manager_diagnostics_topic):
Logger.loginfo('[ServiceFollowTrajectory]: Waiting for msg from topic %s' % self._control_manager_diagnostics_topic)
self._sub_cont_diag.remove_last_msg(self._control_manager_diagnostics_topic)
self._failed = False
# Check if the ProxyServiceCaller has been registered
if not self._srv_follow.is_available(self._service_topic_follow):
Logger.logerr('[ServiceFollowTrajectory]: Topic \'{}\' not yet registered!'.format(self._service_topic_follow))
self._failed = True
return
if not self._srv_set_trajectory.is_available(self._service_topic_set_trajectory):
Logger.logerr('[ServiceFollowTrajectory]: Topic \'{}\' not yet registered!'.format(self._service_topic_set_trajectory))
self._failed = True
return
try:
service_request_set_trajectory = mrs_msgs.srv.TrajectoryReferenceSrvRequest()#TrackerTrajectorySrvRequest()
trajectory_msg = mrs_msgs.msg.TrajectoryReference()
trajectory_msg.header = std_msgs.msg.Header()
trajectory_msg.header.stamp = rospy.Time.now()
trajectory_msg.header.seq = 0
trajectory_msg.header.frame_id = userdata.frame_id
trajectory_msg.use_heading = True
trajectory_msg.fly_now = True
trajectory_msg.loop = False
trajectory_msg.points = userdata.scanning_trajectory
service_request_set_trajectory.trajectory = trajectory_msg
service_result_set_trajectory = self._srv_set_trajectory.call(self._service_topic_set_trajectory,service_request_set_trajectory)
Logger.logwarn('[ServiceFollowTrajectory]: Called \'{}\' in ServiceFollowTrajectory set trejectory'.format(self._service_topic_set_trajectory))
if not service_result_set_trajectory.success:
self._failed = True
Logger.logwarn('[ServiceFollowTrajectory]: Calling \'{}\' was not successful'.format(self._service_topic_set_trajectory))
else:
Logger.loginfo('[ServiceFollowTrajectory]: Calling \'{}\' was successful'.format(self._service_topic_set_trajectory))
except Exception as e:
Logger.logerr('Failed to call \'{}\' service request: {}'.format(self._service_topic_follow, str(e)))
self._failed = True
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 GetPoseInFrameState(EventState):
'''
Transforms the given pose from its current frame to a target frame.
-- target_frame string The frame to which the pose will be transformed
For example, 'utorso', 'world', etc.
># pose_in PoseStamped The pose to be transformed to a target frame
Its current frame is included in the message
#> pose_out PoseStamped The same pose but in the target ref frame
<= done The pose was transformed successfully.
<= failed Failed to transform pose to target frame.
'''
def __init__(self, target_frame):
'''Constructor'''
super(GetPoseInFrameState, self).__init__(outcomes = ['done', 'failed'],
input_keys = ['pose_in'],
output_keys = ['pose_out'])
self._target_frame = target_frame
# Set up service for requesting the transformation
self._service_topic = '/worldmodel_main/get_pose_in_frame_service'
self._srv = ProxyServiceCaller({self._service_topic: GetPoseInFrame})
self._srv_result = None
self._failed = False
self._done = False
def execute(self, userdata):
if self._failed:
return 'failed'
if self._done:
return 'done'
error = self._srv_result.error_code # message is empty if success
if error:
Logger.logwarn('Pose transformation failed because: %s' % error)
self._failed = True
return 'failed'
else:
userdata.pose_out = self._srv_result.transformed_pose
return 'done'
def on_enter(self, userdata):
self._failed = False
self._done = False
# Send the transformation request
try:
Logger.loginfo('Requesting transformation from frame %s to %s' %
(userdata.pose_in.header.frame_id, self._target_frame))
request = GetPoseInFrameRequest(pose = userdata.pose_in,
target_frame = self._target_frame)
self._srv_result = self._srv.call(self._service_topic, request)
except Exception as e:
Logger.logwarn('Failed to send service request:\n%s' % str(e))
self._failed = True
return
class ServiceStringState(EventState):
'''
State for calling std_srvs/Trigger type of services.
-- service_topic string Service_topic_name.
-- state_name string Name of the state used during printing.
-- msg_text string text msg.
<= finished Whenever the calling for successful.
<= failed When the calling failed.
'''
def __init__(self, service_topic, msg_text, state_name=None):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ServiceStringState,
self).__init__(outcomes=['finished', 'failed'])
# Store state parameter for later use.
self._service_topic = rospy.resolve_name(service_topic)
if state_name is None:
self._state_name = ""
else:
self._state_name = '[{}]: '.format(state_name)
# Create proxy service client
self._srv = ProxyServiceCaller(
{self._service_topic: mrs_msgs.srv.String})
self._msg_text = msg_text
self._failed = 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.
if self._failed:
return 'failed'
else:
return 'finished'
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._failed = False
# Check if the ProxyServiceCaller has been registered
if not self._srv.is_available(self._service_topic):
Logger.logerr(
'{}ProxyServiceCaller: Topic \'{}\' not yet registered!'.
format(self._state_name, self._service_topic))
self._failed = True
return
try:
service_request = mrs_msgs.srv.StringRequest()
service_request.value = self._msg_text
service_result = self._srv.call(self._service_topic,
service_request)
if not service_result.success:
self._failed = True
Logger.logwarn(
'{}Calling \'{}\' was not successful: {}'.format(
self._state_name, self._service_topic,
str(service_result.message)))
else:
Logger.loginfo(self._state_name + str(service_result.message))
except Exception as e:
Logger.logerr('{}Failed to call \'{}\' service request: {}'.format(
self._state_name, self._service_topic, str(e)))
self._failed = True
class GetTemplateStandPoseState(EventState):
'''
Requests the pose where to stand in front of a template from the template server.
># template_id int ID of the template to get the stand pose for.
># hand_side string Hand side for which the state will ask the template server for poses {left, right, both}
># preference int Index of the preferred stand pose.
#> grasp PoseStamped Pelvis pose of the robot required to perform grasping.
<= done Pose data is available.
<= failed Failed to get stand pose.
<= not_available The requested preference is not available (index too high).
'''
def __init__(self):
'''Constructor'''
super(GetTemplateStandPoseState, self).__init__(outcomes = ['done', 'failed', 'not_available'],
input_keys = ['template_id', 'hand_side', 'preference'],
output_keys = ['stand_pose'])
self.hand_side = int()
self._service_topic = "/template_info"
self._srv = ProxyServiceCaller({self._service_topic: GetTemplateStateAndTypeInfo})
self._srv_result = None
self._hand_side = None
self._failed = False
self._done = False
self._not_available = False
def execute(self, userdata):
if self._failed or self._srv_result is None:
return 'failed'
if self._done:
return 'done'
if self._not_available:
return 'not_available'
try:
choices = self._srv_result.template_type_information.stand_poses
Logger.loginfo("There are %d stand pose choices and want to select entry %d" % (len(choices), userdata.preference))
if len(choices) <= userdata.preference:
self._not_available = True
return 'not_available'
userdata.stand_pose = choices[userdata.preference].pose
except Exception as e:
Logger.logwarn('Failed to retrieve pose information from service result:\n%s' % str(e))
self._failed = True
return 'failed'
self._done = True
return 'done'
def on_enter(self, userdata):
self._failed = False
self._done = False
self._not_available = False
if userdata.hand_side == 'left':
self._hand_side = GetTemplateStateAndTypeInfoRequest.LEFT_HAND
elif userdata.hand_side == 'right':
self._hand_side = GetTemplateStateAndTypeInfoRequest.RIGHT_HAND
elif userdata.hand_side == 'both':
self._hand_side = GetTemplateStateAndTypeInfoRequest.BOTH_HANDS
else:
Logger.logwarn('Unexpected value of hand side: %s Expected {left, right, both}' % str(userdata.hand_side))
try:
self._srv_result = self._srv.call(self._service_topic, GetTemplateStateAndTypeInfoRequest(userdata.template_id, self._hand_side))
except Exception as e:
Logger.logwarn('Failed to send service call:\n%s' % str(e))
self._failed = True
class ServiceIsBrickPlaceable(EventState):
'''
State for getting whether a brick can be placed in Wall challenge in MBZIRC 2020.
-- service_topic string Service_topic_name.
># brick_id Id of current brick being placed
<= placeable When we can place the brick.
<= wait When we need to wait for placing.
<= failed When the calling failed.
'''
def __init__(self, service_topic):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ServiceIsBrickPlaceable, self).__init__(input_keys=['brick_id'],
output_keys=[],
outcomes = ['placeable', 'wait', 'failed'])
# Store state parameter for later use.
self._service_topic = rospy.resolve_name(service_topic)
# Create proxy service client
self._srv = ProxyServiceCaller({self._service_topic: plan_keeper.srv.IsBrickPlaceable})
self._failed = False
self._placeable = 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.
if self._failed:
return 'failed'
else:
if self._placeable:
return 'placeable'
else:
return 'wait'
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._failed = False
self._placeable = False
# Check if the ProxyServiceCaller has been registered
if not self._srv.is_available(self._service_topic):
Logger.logerr('ProxyServiceCaller: Topic \'{}\' not yet registered!'.format(self._service_topic))
self._failed = True
return
try:
service_request = plan_keeper.srv.IsBrickPlaceableRequest()
service_request.brick_id = userdata.brick_id
Logger.loginfo('ask if brick %d is placeable'%(userdata.brick_id))
service_result = self._srv.call(self._service_topic, service_request)
if not service_result.success:
self._failed = True
Logger.logwarn('Calling \'{}\' was not successful: {}'.format(self._service_topic, str(service_result.message)))
else:
Logger.loginfo(service_result.message)
self._placeable = service_result.placeable
except Exception as e:
Logger.logerr('Failed to call \'{}\' service request: {}'.format(self._service_topic, str(e)))
self._failed = True
class MavrosSetModeState(EventState):
'''
Request to switch FCU operation mode using mavros service: "mavros/set_mode".
-- mode string Specifies to which mode the FCU shoul be switch.
<= finished Whenever the switching for successful.
<= failed When the switching failed.
'''
def __init__(self, request):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(MavrosSetModeState,
self).__init__(outcomes=['finished', 'failed'])
# Store state parameter for later use.
self._request = request
self._service_topic = rospy.resolve_name("mavros/set_mode")
# Create proxy service client
self._srv = ProxyServiceCaller(
{self._service_topic: mavros_msgs.srv.SetMode})
self._failed = 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.
if self._failed:
return 'failed'
else:
return 'finished'
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._failed = False
# Check if the ProxyServiceCaller has been registered
if not self._srv.is_available(self._service_topic):
Logger.logerr(
'[MavrosSetMode]: ProxyServiceCaller: Topic \'{}\' not yet registered!'
.format(self._service_topic))
self._failed = True
return
try:
service_request = mavros_msgs.srv.SetModeRequest()
service_request.custom_mode = self._request
service_result = self._srv.call(self._service_topic,
service_request)
if not service_result.mode_sent:
self._failed = True
Logger.logwarn(
'[MavrosSetMode]: Calling \'{}\' was not successful: {}'.
format(self._service_topic, str(service_result.result)))
else:
Logger.loginfo(
'[MavrosSetMode]: Mavros mode set to: {}'.format(
self._request))
except Exception as e:
Logger.logerr(
'[MavrosSetMode]: Failed to call \'{}\' service request: {}'.
format(self._service_topic, str(e)))
self._failed = True
class GetTemplateAffordanceState(EventState):
'''
Requests affordance information for a template from the template server.
-- identifier string Name to identify the requested affordance.
># template_id int ID of the template to manipulate.
># hand_side string Hand side for which the state will ask the template server for affordances {left, right, both}
#> affordance Affordance A message as defined in vigir_object_template_msgs containing all required information.
<= done Affordance data is available.
<= failed Failed to get affordance information.
<= not_available The requested preference is not available (index too high).
'''
def __init__(self, identifier):
'''Constructor'''
super(GetTemplateAffordanceState, self).__init__(outcomes = ['done', 'failed', 'not_available'],
input_keys = ['template_id', 'hand_side'],
output_keys = ['affordance'])
self._service_topic = "/template_info"
self._srv = ProxyServiceCaller({self._service_topic: GetTemplateStateAndTypeInfo})
self._srv_result = None
self._identifier = identifier
self._hand_side = None
self._failed = False
self._done = False
def execute(self, userdata):
if self._failed or self._srv_result is None:
return 'failed'
if self._done:
return 'done'
try:
choices = self._srv_result.template_type_information.affordances
if not self._identifier in [a.name for a in choices]:
return 'not_available'
chosen_affordance = next(a for a in choices if a.name == self._identifier)
userdata.affordance = chosen_affordance
Logger.loginfo('Using grasp with ID: %s' % str(chosen_affordance.id))
except Exception as e:
Logger.logwarn('Failed to retrieve affordance information from service result:\n%s' % str(e))
self._failed = True
return 'failed'
self._done = True
return 'done'
def on_enter(self, userdata):
self._failed = False
self._done = False
self._srv_result = None
if userdata.hand_side == 'left':
self._hand_side = GetTemplateStateAndTypeInfoRequest.LEFT_HAND
elif userdata.hand_side == 'right':
self._hand_side = GetTemplateStateAndTypeInfoRequest.RIGHT_HAND
elif userdata.hand_side == 'both':
self._hand_side = GetTemplateStateAndTypeInfoRequest.BOTH_HANDS
else:
Logger.logwarn('Unexpected value of hand side: %s Expected {left, right, both}' % str(userdata.hand_side))
try:
self._srv_result = self._srv.call(self._service_topic, GetTemplateStateAndTypeInfoRequest(userdata.template_id, self._hand_side))
except Exception as e:
Logger.logwarn('Failed to send service call:\n%s' % str(e))
self._failed = True
class ServiceSetMappedObjectsIntoPlanKeeper(EventState):
'''
State for saving the plans for robots in Wall challenge in MBZIRC 2020 into plan_keeper node.
-- service_topic string Service_topic_name.
># mapped_objects Mapped walls and bricks.
<= successed Whenever the calling for successful.
<= failed When the calling failed.
'''
def __init__(self, service_topic):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ServiceSetMappedObjectsIntoPlanKeeper,
self).__init__(input_keys=['mapped_objects'],
outcomes=['successed', 'failed'])
# Store state parameter for later use.
self._service_topic = rospy.resolve_name(service_topic)
# Create proxy service client
self._srv = ProxyServiceCaller(
{self._service_topic: plan_keeper.srv.SetMappedPositions})
self._failed = 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.
if self._failed:
return 'failed'
else:
return 'successed'
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._failed = False
# Check if the ProxyServiceCaller has been registered
if not self._srv.is_available(self._service_topic):
Logger.logerr(
'[ServiceSetMappedObjectsIntoPlanKeeper]: Topic \'{}\' not yet registered!'
.format(self._service_topic))
self._failed = True
return
try:
service_request = plan_keeper.srv.SetMappedPositionsRequest()
service_request.mapped_objects = userdata.mapped_objects
Logger.loginfo(
'[ServiceSetMappedObjectsIntoPlanKeeper]: sending mapped_objects to plan_keeper'
)
Logger.loginfo(str(service_request.mapped_objects))
service_result = self._srv.call(self._service_topic,
service_request)
if not service_result.success:
self._failed = True
Logger.logwarn(
'[ServiceSetMappedObjectsIntoPlanKeeper]: Calling \'{}\' was not successful: {}'
.format(self._service_topic, str(service_result.message)))
else:
Logger.loginfo(service_result.message)
except Exception as e:
Logger.logerr(
'[ServiceSetMappedObjectsIntoPlanKeeper]: Failed to call \'{}\' service request: {}'
.format(self._service_topic, str(e)))
self._failed = True
class GetTemplateUsabilityState(EventState):
'''
Requests a grasp for a template from the template server.
-- usability_name string Name of the template usability (e.g. 'tip')
-- usability_id int ID of the template usability.
Set to 100 if requesting by name instead.
># template_id int ID of the template of interest
#> usability_pose PoseStamped Pose of the template's usability in the
reference frame of the wrist (r/l_hand)
<= done The usability has been retrieved.
<= failed Failed to get the template's usability.
'''
def __init__(self, usability_name = '', usability_id = 100):
'''Constructor'''
super(GetTemplateUsabilityState, self).__init__(outcomes = ['done', 'failed'],
input_keys = ['template_id'],
output_keys = ['usability_pose'])
# Set up service for attaching object template
self._service_topic = "/usability_pose"
self._srv = ProxyServiceCaller({
self._service_topic: GetUsabilityInWristFrame})
self._usability_id = usability_id
self._usability_name = usability_name
self._failed = False
def execute(self, userdata):
'''Move along people, nothing to see here!'''
if self._failed:
return 'failed'
else:
return 'done'
def on_enter(self, userdata):
'''Send the usability request and write response to userdata.'''
self._failed = False
if self._usability_id != 100 and self._usability_name != '':
Logger.logwarn('Not clear if you are requesting usability by ID (%d) + \
or name (%s)' % (self._usability_id, self._usability_name))
try:
request = GetUsabilityInWristFrameRequest(
template_id = userdata.template_id,
usability_id = self._usability_id,
usability_name = self._usability_name)
self._srv_result = self._srv.call(self._service_topic, request)
userdata.usability_pose = self._srv_result.wrist_usability
print request, self._srv_result.wrist_usability # debug
except Exception as e:
Logger.logwarn('Failed to send service call:\n%s' % str(e))
self._failed = True
return
class MavrosArmState(EventState):
'''
Request to arm or disarm a robot by calling the mavros service: "mavros/cmd/arming".
-- request bool Request that specifies if a robot should be arm or disarm.
<= finished Whenever the calling for successful.
<= failed When the calling failed.
'''
def __init__(self, request):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(MavrosArmState, self).__init__(outcomes=['finished', 'failed'])
# Store state parameter for later use.
self._request = request
self._service_topic = rospy.resolve_name("mavros/cmd/arming")
# Create proxy service client
self._srv = ProxyServiceCaller(
{self._service_topic: mavros_msgs.srv.CommandBool})
self._failed = 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.
if self._failed:
return 'failed'
else:
return 'finished'
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._failed = False
# Check if the ProxyServiceCaller has been registered
if not self._srv.is_available(self._service_topic):
Logger.logerr(
'ProxyServiceCaller: Topic \'{}\' not yet registered!'.format(
self._service_topic))
self._failed = True
return
try:
service_request = mavros_msgs.srv.CommandBoolRequest()
service_request.value = self._request
service_result = self._srv.call(self._service_topic,
service_request)
if not service_result.success:
self._failed = True
Logger.logwarn('Calling \'{}\' was not successful: {}'.format(
self._service_topic, str(service_result.result)))
else:
if self._request:
Logger.loginfo('[MavrosArmState]: Robot armed.')
else:
Logger.loginfo('[MavrosArmState]: Robot disarmed.')
except Exception as e:
Logger.logerr('Failed to call \'{}\' service request: {}'.format(
self._service_topic, str(e)))
self._failed = True
