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, battery_status_topic, num_battery_cells):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(IsBatteryBellowValueState, self).__init__(input_keys=['cell_voltage_target'],
output_keys=['current_cell_voltage'],
outcomes=['is_bellow', 'is_ok']
)
self._topic = rospy.resolve_name(battery_status_topic)
self._num_battery_cells = num_battery_cells
# Store state parameter for later use.
(msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic)
#Logger.loginfo('[IsBatteryBellowValueState]: topic %s msg_topic %s'%(self._topic,msg_topic))
if msg_topic == self._topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub_battery = ProxySubscriberCached({self._topic: msg_type})
self._connected = True
Logger.loginfo('[IsBatteryBellowValueState]: Successfully subscribed to topic %s' % self._topic)
#brick_subs_ = rospy.Subscriber(self._topic, sensor_msgs.msg.BatteryState, self.battery_status_callback)
self.moving_average_hist_v_param = 0.1
self.volage_per_cell = 4.2
self.voltage = num_battery_cells * self.volage_per_cell
# The constructor is called when building the state machine, not when actually starting the behavior.
# Thus, we cannot save the starting time now and will do so later.
self._start_time = None
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.loginfo('[IsRealsenseConnectedState] entering state')
self.last_time = rospy.get_time()
if not self._connected:
(msg_path, msg_topic,
fn) = rostopic.get_topic_type(self.realsense_camera_info_topic)
if msg_topic == self.realsense_camera_info_topic:
msg_type = self._get_msg_from_path(msg_path)
self.realsense_subs_ = ProxySubscriberCached(
{self.realsense_camera_info_topic: msg_type})
self._connected = True
Logger.loginfo(
'[IsRealsenseConnectedState]: Successfully subscribed to previously unavailable topic %s'
% self.realsense_camera_info_topic)
else:
Logger.logwarn(
'[IsRealsenseConnectedState]: Topic %s still not available, giving up.'
% self.realsense_camera_info_topic)
if self._connected and self.realsense_subs_.has_msg(
self.realsense_camera_info_topic):
Logger.loginfo(
'[IsRealsenseConnectedState]: Waiting for msg from topic %s' %
self.realsense_camera_info_topic)
self.realsense_subs_.remove_last_msg(
self.realsense_camera_info_topic)
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 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 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('[GoToReference]: Successfully subscribed to previously unavailable topic %s' % self._control_manager_diagnostics_topic)
else:
Logger.logwarn('[GoToReference]: 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('[GoToReference]: 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('[GoToReference]: 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 = userdata.goal_tracker_point.position.x
service_request.reference.position.y = userdata.goal_tracker_point.position.y
service_request.reference.position.z = userdata.goal_tracker_point.position.z
service_request.reference.heading = userdata.goal_tracker_point.heading
self.target = service_request.reference
Logger.loginfo('[GoToReference]: called to ref x%.2f y%.2f z%.2f head%.2f'%(service_request.reference.position.x,service_request.reference.position.y,service_request.reference.position.z,service_request.reference.heading))
service_result = self._srv.call(self._service_topic, service_request)
if not service_result.success:
self._failed = True
Logger.logwarn('[GoToReference]: Calling \'{}\' was not successful: {} for reference position (x,y,z) = ({},{},{})'.format(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('[GoToReference]: {}'.format(str(service_result.message)))
except Exception as e:
Logger.logerr('[GoToReference]: Failed to call \'{}\' service request: {}'.format(self._service_topic, str(e)))
self._failed = True
def __init__(self, plan_keeper_mapped_arena_topic):
'''
Constructor
'''
super(WaitForArenaMappedObjectsState,
self).__init__(outcomes=['successed', 'failed'],
output_keys=['mapped_objects'])
self._topic = rospy.resolve_name(plan_keeper_mapped_arena_topic)
#self._sub = rospy.Subscriber(self._topic, brick_mapping.msg.MappedArenaObjectsStamped, self.mapped_area_callback)
#self._connected = True
#
(msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic)
#Logger.loginfo('[IsBatteryBellowValueState]: topic %s msg_topic %s'%(self._topic,msg_topic))
if msg_topic == self._topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub = ProxySubscriberCached({self._topic: msg_type})
self._connected = True
Logger.loginfo(
'[WaitForArenaMappedObjectsState]: subscribing topic %s ' %
(self._topic))
self.received = False
self.mapped_objects = None
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 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 __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, realsense_camera_info_topic):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(IsRealsenseConnectedState,
self).__init__(input_keys=['max_time_unconnected_realsense_s'],
outcomes=['is_connected', 'unconnected'])
# Store state parameter for later use.
self.realsense_camera_info_topic = rospy.resolve_name(
realsense_camera_info_topic)
(msg_path, msg_topic,
fn) = rostopic.get_topic_type(self.realsense_camera_info_topic)
if msg_topic == self.realsense_camera_info_topic:
msg_type = self._get_msg_from_path(msg_path)
self.realsense_subs_ = ProxySubscriberCached(
{self.realsense_camera_info_topic: msg_type})
self._connected = True
Logger.loginfo(
'[IsRealsenseConnectedState]: Successfully subscribed to topic %s'
% self.realsense_camera_info_topic)
#realsense_subs_ = rospy.Subscriber(self.realsense_camera_info_topic, msg_type, self.realsense_image_info_callback)
self.max_non_connected_time_s = 3.0
self.last_time = rospy.get_time()
def on_enter(self, userdata):
Logger.loginfo('[WaitForArenaMappedObjectsState]: entering state %s' %
self._topic)
if not self._connected:
(msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic)
#Logger.loginfo('[IsBatteryBellowValueState]: topic %s msg_topic %s'%(self._topic,msg_topic))
if msg_topic == self._topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub = ProxySubscriberCached({self._topic: msg_type})
self._connected = True
Logger.loginfo(
'[WaitForArenaMappedObjectsState]: subscribing topic %s ' %
(self._topic))
else:
Logger.logwarn(
'[WaitForArenaMappedObjectsState]: Topic %s still not available, giving up.'
% self._topic)
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 IsBatteryBellowValueState(EventState):
'''
Checking if battery is bellow something
-- battery_topic string topic of battery data
<= continue Given time has passed.
<= failed Example for a failure outcome.
'''
def __init__(self, battery_status_topic, num_battery_cells):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(IsBatteryBellowValueState, self).__init__(input_keys=['cell_voltage_target'],
output_keys=['current_cell_voltage'],
outcomes=['is_bellow', 'is_ok']
)
self._topic = rospy.resolve_name(battery_status_topic)
self._num_battery_cells = num_battery_cells
# Store state parameter for later use.
(msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic)
#Logger.loginfo('[IsBatteryBellowValueState]: topic %s msg_topic %s'%(self._topic,msg_topic))
if msg_topic == self._topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub_battery = ProxySubscriberCached({self._topic: msg_type})
self._connected = True
Logger.loginfo('[IsBatteryBellowValueState]: Successfully subscribed to topic %s' % self._topic)
#brick_subs_ = rospy.Subscriber(self._topic, sensor_msgs.msg.BatteryState, self.battery_status_callback)
self.moving_average_hist_v_param = 0.1
self.volage_per_cell = 4.2
self.voltage = num_battery_cells * self.volage_per_cell
# The constructor is called when building the state machine, not when actually starting the behavior.
# Thus, we cannot save the starting time now and will do so later.
self._start_time = None
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.
"""
sensor_msgs/BatteryState
std_msgs/Header header
uint32 seq
time stamp
string frame_id
float32 voltage
float32 current
float32 charge
float32 capacity
float32 design_capacity
float32 percentage
uint8 power_supply_status
uint8 power_supply_health
uint8 power_supply_technology
bool present
float32[] cell_voltage
string location
string serial_number
"""
if self._sub_battery.has_msg(self._topic):
message = self._sub_battery.get_last_msg(self._topic)
Logger.loginfo('[IsBatteryBellowValueState]: received voltage per cell %.2f' % (message.voltage/ float(self._num_battery_cells)))
self._sub_battery.remove_last_msg(self._topic)
#print('[IsBatteryBellowValueState] message received')
if self.voltage is None:
self.voltage = message.voltage
cell_voltage_target = userdata.cell_voltage_target
self.voltage = self.voltage * self.moving_average_hist_v_param + (1.0 - self.moving_average_hist_v_param) * message.voltage
self.volage_per_cell = self.voltage / float(self._num_battery_cells)
userdata.current_cell_voltage = self.volage_per_cell
rospy.loginfo_throttle(20,('[IsBatteryBellowValueState] voltage per cell is %f'%(self.volage_per_cell)))
if self.volage_per_cell < cell_voltage_target:
Logger.loginfo('[IsBatteryBellowValueState] voltage per cell %f is bellow set threshold %f'%(self.volage_per_cell,cell_voltage_target))
return 'is_bellow'
else:
Logger.loginfo('[IsBatteryBellowValueState] voltage per cell %f is above set threshold %f'%(self.volage_per_cell,cell_voltage_target))
return 'is_ok'
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.loginfo('[IsBatteryBellowValueState] entering state')
if not self._connected:
(msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic)
if msg_topic == self._topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub_battery = ProxySubscriberCached({self._topic: msg_type})
self._connected = True
Logger.loginfo('[IsBatteryBellowValueState]: Successfully subscribed to previously unavailable topic %s' % self._topic)
else:
Logger.logwarn('[IsBatteryBellowValueState]: Topic %s still not available, giving up.' % self._topic)
if self._connected and self._sub_battery.has_msg(self._topic):
Logger.loginfo('[IsBatteryBellowValueState]: Waiting for msg from topic %s' % self._topic)
self._sub_battery.remove_last_msg(self._topic)
def _get_msg_from_path(self, msg_path):
msg_import = msg_path.split('/')
msg_module = '%s.msg' % (msg_import[0])
package = __import__(msg_module, fromlist=[msg_module])
clsmembers = inspect.getmembers(package, lambda member: inspect.isclass(member) and member.__module__.endswith(msg_import[1]))
return clsmembers[0][1]
class 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 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 IsRealsenseConnectedState(EventState):
'''
Checking if realsense is connected
-- battery_topic string topic of realsense image info
<= continue Given time has passed.
<= failed Example for a failure outcome.
'''
def __init__(self, realsense_camera_info_topic):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(IsRealsenseConnectedState,
self).__init__(input_keys=['max_time_unconnected_realsense_s'],
outcomes=['is_connected', 'unconnected'])
# Store state parameter for later use.
self.realsense_camera_info_topic = rospy.resolve_name(
realsense_camera_info_topic)
(msg_path, msg_topic,
fn) = rostopic.get_topic_type(self.realsense_camera_info_topic)
if msg_topic == self.realsense_camera_info_topic:
msg_type = self._get_msg_from_path(msg_path)
self.realsense_subs_ = ProxySubscriberCached(
{self.realsense_camera_info_topic: msg_type})
self._connected = True
Logger.loginfo(
'[IsRealsenseConnectedState]: Successfully subscribed to topic %s'
% self.realsense_camera_info_topic)
#realsense_subs_ = rospy.Subscriber(self.realsense_camera_info_topic, msg_type, self.realsense_image_info_callback)
self.max_non_connected_time_s = 3.0
self.last_time = rospy.get_time()
# The constructor is called when building the state machine, not when actually starting the behavior.
# Thus, we cannot save the starting time now and will do so later.
def realsense_image_info_callback(self, data):
self.last_time = rospy.get_time()
rospy.loginfo_throttle(
20, '[IsRealsenseConnectedState] realsense still connected')
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.
self.max_non_connected_time_s = userdata.max_time_unconnected_realsense_s
if self.realsense_subs_.has_msg(self.realsense_camera_info_topic):
self.last_time = rospy.get_time()
Logger.loginfo(
'[IsRealsenseConnectedState] realsense is connected')
return 'is_connected'
diff_time = rospy.get_time() - self.last_time
if diff_time > self.max_non_connected_time_s:
Logger.logerr('[IsRealsenseConnectedState] realsense ')
return 'unconnected'
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.loginfo('[IsRealsenseConnectedState] entering state')
self.last_time = rospy.get_time()
if not self._connected:
(msg_path, msg_topic,
fn) = rostopic.get_topic_type(self.realsense_camera_info_topic)
if msg_topic == self.realsense_camera_info_topic:
msg_type = self._get_msg_from_path(msg_path)
self.realsense_subs_ = ProxySubscriberCached(
{self.realsense_camera_info_topic: msg_type})
self._connected = True
Logger.loginfo(
'[IsRealsenseConnectedState]: Successfully subscribed to previously unavailable topic %s'
% self.realsense_camera_info_topic)
else:
Logger.logwarn(
'[IsRealsenseConnectedState]: Topic %s still not available, giving up.'
% self.realsense_camera_info_topic)
if self._connected and self.realsense_subs_.has_msg(
self.realsense_camera_info_topic):
Logger.loginfo(
'[IsRealsenseConnectedState]: Waiting for msg from topic %s' %
self.realsense_camera_info_topic)
self.realsense_subs_.remove_last_msg(
self.realsense_camera_info_topic)
def _get_msg_from_path(self, msg_path):
msg_import = msg_path.split('/')
msg_module = '%s.msg' % (msg_import[0])
package = __import__(msg_module, fromlist=[msg_module])
clsmembers = inspect.getmembers(
package, lambda member: inspect.isclass(member) and member.
__module__.endswith(msg_import[1]))
return clsmembers[0][1]
