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 __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(ServiceGoToState,
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.Vec4})
(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(
'[ServiceGoToState]: Successfully subscribed to topic %s' %
self._control_manager_diagnostics_topic)
self._failed = False
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 __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 __init__(self, service_topic):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ServiceSetPlanIntoPlanKeeper, self).__init__(input_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: plan_keeper.srv.RobotPlan})
self._failed = False
def __init__(self, service_topic):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ServiceResetMapState,
self).__init__(outcomes=['finished', '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: std_srvs.srv.Trigger})
self._failed = False
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 __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 __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 __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 __init__(self, service_topic):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ServiceSetBrickPlacedIntoPlanKeeper, self).__init__(input_keys=['brick_id','placing_result'],
outcomes = ['successed', 'failed'])
# Store state parameter for later use.
self._service_topic = service_topic
# Create proxy service client
self._srv = ProxyServiceCaller({self._service_topic: plan_keeper.srv.BrickResult})
self._failed = False
Logger.loginfo("[ServiceSetBrickPlacedIntoPlanKeeper]: init with service_topic %s"%(str(self._service_topic)))
def __init__(self):
'''Constructor'''
super(GetTemplateGraspState, self).__init__(outcomes = ['done', 'failed', 'not_available'],
input_keys = ['template_id', 'hand_side', 'preference'],
output_keys = ['grasp'])
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
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 __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 __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 __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 __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 __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 __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 set_up_affordance_service(self):
hands_in_use = self.determine_hands_in_use()
# Set up services for transforming cartesian affordances to wrist frame
self._service_topics = dict()
for hand in hands_in_use:
topic = '/manipulation_control/%s_hand/wrist_affordance_srv' % hand[
0]
self._service_topics[hand] = topic
self._srv = ProxyServiceCaller({
t: GetAffordanceInWristFrame
for t in self._service_topics.values()
})
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 __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 __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 __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 __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 __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 __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
