def __init__(self, screen):
rospy.init_node('status_node', argv=sys.argv)
self.rate = rospy.get_param('~rate', default=1.0)
# Curses setup
self.screen = curses.initscr()
self.rows, self.cols = self.screen.getmaxyx()
height_status = 15
self.status = curses.newwin(height_status, self.cols, 1, 2)
# self.console = curses.newwin(self.rows - height_status, self.cols, 12, 2)
self.lines = 0
self.text = ''
self.screen.keypad(True)
curses.curs_set(False) # Hide cursor
colors = [
curses.COLOR_BLACK, curses.COLOR_BLUE, curses.COLOR_CYAN,
curses.COLOR_GREEN, curses.COLOR_MAGENTA, curses.COLOR_RED,
curses.COLOR_WHITE, curses.COLOR_YELLOW
]
# Curses color setup
curses.use_default_colors()
for color in colors:
curses.init_pair(color, color, -1)
# Default variables
self.status_battery_perc = None
self.state = State()
self.state_sub = rospy.Subscriber('mavros/state',
State,
callback=self.state_callback,
queue_size=1)
self.battery = BatteryState()
self.battery_sub = rospy.Subscriber('mavros/battery',
BatteryState,
callback=self.battery_callback,
queue_size=1)
self.extended = ExtendedState()
self.extended_sub = rospy.Subscriber('mavros/extended_state',
ExtendedState,
callback=self.extended_callback,
queue_size=1)
# self.statustext = StatusText()
# self.statustext_sub = rospy.Subscriber('mavros/statustext/recv', StatusText,
# callback=self.statustext_callback,
# queue_size=1)
self.gps = NavSatFix()
self.gps_sub = rospy.Subscriber('mavros/global_position/raw/fix',
NavSatFix,
callback=self.gps_callback,
queue_size=1)
self.local_pose = PoseStamped()
self.local_pose_sub = rospy.Subscriber(
'mavros/local_position/pose',
PoseStamped,
callback=self.local_pose_callback,
queue_size=1)
self.global_pose = PoseStamped()
self.global_pose_sub = rospy.Subscriber(
'global_position/pose',
PoseStamped,
callback=self.global_pose_callback,
queue_size=1)
self.diagnostics = DiagnosticArray()
self.diagnostic_gps = DiagnosticStatus()
self.diagnostics_sub = rospy.Subscriber(
'/diagnostics',
DiagnosticArray,
callback=self.diagnostics_callback,
queue_size=1)
self.setpoint = PositionTarget()
self.setpoint_sub = rospy.Subscriber('mavros/setpoint_raw/local',
PositionTarget,
callback=self.setpoint_callback,
queue_size=1)
self.cameras = ['front', 'right', 'back', 'left']
self.image_subscribers = []
self.images = {c: deque(maxlen=10) for c in self.cameras}
for camera in self.cameras:
topic = f'camera_{camera}/image_raw'
subscriber = rospy.Subscriber(topic,
Image,
callback=self.image_callback,
callback_args=camera,
queue_size=1,
buff_size=2**24)
self.image_subscribers.append(subscriber)
def __init__(self):
device = get_param('~device', 'auto')
baudrate = get_param('~baudrate', 0)
timeout = get_param('~timeout', 0.02)
if device == 'auto':
devs = mtdevice.find_devices()
if devs:
device, baudrate = devs[0]
rospy.loginfo("Detected MT device on port %s @ %d bps"
% (device, baudrate))
else:
rospy.logerr("Fatal: could not find proper MT device.")
rospy.signal_shutdown("Could not find proper MT device.")
return
if not baudrate:
baudrate = mtdevice.find_baudrate(device)
if not baudrate:
rospy.logerr("Fatal: could not find proper baudrate.")
rospy.signal_shutdown("Could not find proper baudrate.")
return
rospy.loginfo("MT node interface: %s at %d bd." % (device, baudrate))
self.mt = mtdevice.MTDevice(device, baudrate, timeout)
# optional no rotation procedure for internal calibration of biases
# (only mark iv devices)
no_rotation_duration = get_param('~no_rotation_duration', 0)
if no_rotation_duration:
rospy.loginfo("Starting the no-rotation procedure to estimate the "
"gyroscope biases for %d s. Please don't move the IMU"
" during this time." % no_rotation_duration)
self.mt.SetNoRotation(no_rotation_duration)
self.frame_id = get_param('~frame_id', '/base_imu')
self.frame_local = get_param('~frame_local', 'ENU')
self.diag_msg = DiagnosticArray()
self.stest_stat = DiagnosticStatus(name='mtnode: Self Test', level=1,
message='No status information')
self.xkf_stat = DiagnosticStatus(name='mtnode: XKF Valid', level=1,
message='No status information')
self.gps_stat = DiagnosticStatus(name='mtnode: GPS Fix', level=1,
message='No status information')
self.diag_msg.status = [self.stest_stat, self.xkf_stat, self.gps_stat]
# publishers created at first use to reduce topic clutter
self.diag_pub = None
self.imu_pub = None
self.pos_pub = None
self.gps_pub = None
self.vel_pub = None
self.mag_pub = None
self.temp_pub = None
self.press_pub = None
self.analog_in1_pub = None # decide type+header
self.analog_in2_pub = None # decide type+header
self.ecef_pub = None
self.time_ref_pub = None
# TODO pressure, ITOW from raw GPS?
self.old_bGPS = 256 # publish GPS only if new
# publish a string version of all data; to be parsed by clients
self.str_pub = rospy.Publisher('imu/imu_data_str', String, queue_size=10)
# predefinition of used variables
self.imu_msg = Imu()
self.imu_msg_old = Imu()
self.pos_msg = NavSatFix()
self.pos_msg_old = NavSatFix()
self.gps_msg = NavSatFix()
self.gps_msg_old = NavSatFix()
self.vel_msg = TwistStamped()
self.vel_msg_old = TwistStamped()
self.mag_msg = MagneticField()
self.mag_msg_old = MagneticField()
self.temp_msg = Temperature()
self.temp_msg_old = Temperature()
self.press_msg = FluidPressure()
self.press_msg_old = FluidPressure()
self.anin1_msg = UInt16()
self.anin1_msg_old = UInt16()
self.anin2_msg = UInt16()
self.anin2_msg_old = UInt16()
self.ecef_msg = PointStamped()
self.ecef_msg_old = PointStamped()
# triggers for new msg to publish
self.pub_imu = False
self.pub_pos = False
self.pub_gps = False
self.pub_vel = False
self.pub_mag = False
self.pub_temp = False
self.pub_press = False
self.pub_anin1 = False
self.pub_anin2 = False
self.pub_ecef = False
self.pub_diag = False
def publish(self, sensor_state, gyro):
curr_time = sensor_state.header.stamp
# limit to 5hz
if (curr_time - self.last_diagnostics_time).to_sec() < 0.2:
return
self.last_diagnostics_time = curr_time
diag = DiagnosticArray()
diag.header.stamp = curr_time
stat = DiagnosticStatus()
#node status
stat = DiagnosticStatus(name="TurtleBot Node",
level=DiagnosticStatus.OK,
message="RUNNING")
diag.status.append(stat)
#mode info
stat = DiagnosticStatus(name="Operating Mode",
level=DiagnosticStatus.OK)
try:
stat.message = self.oi_mode[sensor_state.oi_mode]
except KeyError as ex:
stat.level = DiagnosticStatus.ERROR
stat.message = "Invalid OI Mode Reported %s" % ex
rospy.logwarn(stat.message)
diag.status.append(stat)
#battery info
stat = DiagnosticStatus(name="Battery",
level=DiagnosticStatus.OK,
message="OK")
values = stat.values
if sensor_state.charging_state == 5:
stat.level = DiagnosticStatus.ERROR
stat.message = "Charging Fault Condition"
values.append(
KeyValue("Charging State",
self.charging_state[sensor_state.charging_state]))
values.extend([
KeyValue("Voltage (V)", str(sensor_state.voltage / 1000.0)),
KeyValue("Current (A)", str(sensor_state.current / 1000.0)),
KeyValue("Temperature (C)", str(sensor_state.temperature)),
KeyValue("Charge (Ah)", str(sensor_state.charge / 1000.0)),
KeyValue("Capacity (Ah)", str(sensor_state.capacity / 1000.0))
])
diag.status.append(stat)
#charging source
stat = DiagnosticStatus(name="Charging Sources",
level=DiagnosticStatus.OK)
try:
stat.message = self.charging_source[
sensor_state.charging_sources_available]
except KeyError as ex:
stat.level = DiagnosticStatus.ERROR
stat.message = "Invalid Charging Source %s, actual value: %i" % (
ex, sensor_state.charging_sources_available)
rospy.logwarn(stat.message)
diag.status.append(stat)
#cliff sensors
stat = DiagnosticStatus(name="Cliff Sensor",
level=DiagnosticStatus.OK,
message="OK")
if sensor_state.cliff_left or sensor_state.cliff_front_left or sensor_state.cliff_right or sensor_state.cliff_front_right:
stat.level = DiagnosticStatus.WARN
if (sensor_state.current / 1000.0) > 0:
stat.message = "Near Cliff: While the irobot create is charging, the create thinks it's near a cliff."
stat.level = DiagnosticStatus.OK # We're OK here
else:
stat.message = "Near Cliff"
stat.values = [
KeyValue("Left", str(sensor_state.cliff_left)),
KeyValue("Left Signal", str(sensor_state.cliff_left_signal)),
KeyValue("Front Left", str(sensor_state.cliff_front_left)),
KeyValue("Front Left Signal",
str(sensor_state.cliff_front_left_signal)),
KeyValue("Front Right", str(sensor_state.cliff_right)),
KeyValue("Front Right Signal",
str(sensor_state.cliff_right_signal)),
KeyValue("Right", str(sensor_state.cliff_front_right)),
KeyValue("Right Signal",
str(sensor_state.cliff_front_right_signal))
]
diag.status.append(stat)
#Wall sensors
stat = DiagnosticStatus(name="Wall Sensor",
level=DiagnosticStatus.OK,
message="OK")
#wall always seems to be false???
if sensor_state.wall:
stat.level = DiagnosticStatus.ERROR
stat.message = "Near Wall"
stat.values = [
KeyValue("Wall", str(sensor_state.wall)),
KeyValue("Wall Signal", str(sensor_state.wall_signal)),
KeyValue("Virtual Wall", str(sensor_state.virtual_wall))
]
diag.status.append(stat)
#Gyro
stat = DiagnosticStatus(name="Gyro Sensor",
level=DiagnosticStatus.OK,
message="OK")
if gyro is None:
stat.level = DiagnosticStatus.WARN
stat.message = "Gyro Not Enabled: To enable the gyro set the has_gyro param in the turtlebot_node."
elif gyro.cal_offset < 100:
stat.level = DiagnosticStatus.ERROR
stat.message = "Gyro Power Problem: For more information visit http://answers.ros.org/question/2091/turtlebot-bad-gyro-calibration-also."
elif gyro.cal_offset > 575.0 or gyro.cal_offset < 425.0:
stat.level = DiagnosticStatus.ERROR
stat.message = "Bad Gyro Calibration Offset: The gyro average is outside the standard deviation."
if gyro is not None:
stat.values = [
KeyValue("Gyro Enabled", str(gyro is not None)),
KeyValue("Raw Gyro Rate", str(sensor_state.user_analog_input)),
KeyValue("Calibration Offset", str(gyro.cal_offset)),
KeyValue("Calibration Buffer", str(gyro.cal_buffer))
]
diag.status.append(stat)
#Digital IO
stat = DiagnosticStatus(name="Digital Outputs",
level=DiagnosticStatus.OK,
message="OK")
out_byte = sensor_state.user_digital_outputs
stat.values = [
KeyValue("Raw Byte", str(out_byte)),
KeyValue("Digital Out 2", self.digital_outputs[out_byte % 2]),
KeyValue("Digital Out 1",
self.digital_outputs[(out_byte >> 1) % 2]),
KeyValue("Digital Out 0",
self.digital_outputs[(out_byte >> 2) % 2])
]
diag.status.append(stat)
#publish
self.diag_pub.publish(diag)
import rospy
from time import sleep
from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus
if __name__ == '__main__':
rospy.init_node('diag_pub')
pub = rospy.Publisher('/diagnostics', DiagnosticArray)
start_time = rospy.get_time()
while not rospy.is_shutdown():
array = DiagnosticArray()
array.status = [
# GenericAnalyzer my_path
DiagnosticStatus(0, 'expected1', 'OK', '', []),
DiagnosticStatus(1, 'expected2', 'OK', '', []),
DiagnosticStatus(2, 'expected3', 'OK', '', []),
DiagnosticStatus(0, 'startswith1', 'OK', '', []),
DiagnosticStatus(0, 'startswith2', 'OK', '', []),
DiagnosticStatus(1, 'startswith3', 'OK', '', []),
# OtherAnalyzer for Other
DiagnosticStatus(2, 'other2', 'OK', '', []),
DiagnosticStatus(0, 'other3', 'OK', '', [])
]
array.header.stamp = rospy.get_rostime()
if rospy.get_time() - start_time < 5:
# Shouldn't disappear
def __init__(self, coz):
"""
:type coz: cozmo.Robot
:param coz: The cozmo SDK robot handle (object).
"""
# vars
self._cozmo = coz
self._lin_vel = .0
self._ang_vel = .0
self._cmd_lin_vel = .0
self._cmd_ang_vel = .0
self._last_pose = self._cozmo.pose
self._wheel_vel = (0, 0)
self._optical_frame_orientation = quaternion_from_euler(
-np.pi / 2., .0, -np.pi / 2.)
self._camera_info_manager = CameraInfoManager(
'cozmo_camera', namespace='/cozmo_camera')
# tf
self._tfb = TransformBroadcaster()
# params
self._odom_frame = rospy.get_param('~odom_frame', 'odom')
self._footprint_frame = rospy.get_param('~footprint_frame',
'base_footprint')
self._base_frame = rospy.get_param('~base_frame', 'base_link')
self._head_frame = rospy.get_param('~head_frame', 'head_link')
self._camera_frame = rospy.get_param('~camera_frame', 'camera_link')
self._camera_optical_frame = rospy.get_param('~camera_optical_frame',
'cozmo_camera')
camera_info_url = rospy.get_param('~camera_info_url', '')
# pubs
self._joint_state_pub = rospy.Publisher('joint_states',
JointState,
queue_size=1)
self._odom_pub = rospy.Publisher('odom', Odometry, queue_size=1)
self._imu_pub = rospy.Publisher('imu', Imu, queue_size=1)
self._battery_pub = rospy.Publisher('battery',
BatteryState,
queue_size=1)
# Note: camera is published under global topic (preceding "/")
self._image_pub = rospy.Publisher('/cozmo_camera/image',
Image,
queue_size=10)
self._camera_info_pub = rospy.Publisher('/cozmo_camera/camera_info',
CameraInfo,
queue_size=10)
# subs
self._backpack_led_sub = rospy.Subscriber('backpack_led',
ColorRGBA,
self._set_backpack_led,
queue_size=1)
self._twist_sub = rospy.Subscriber('cmd_vel',
Twist,
self._twist_callback,
queue_size=1)
self._say_sub = rospy.Subscriber('say',
String,
self._say_callback,
queue_size=1)
self._head_sub = rospy.Subscriber('head_angle',
Float64,
self._move_head,
queue_size=1)
self._lift_sub = rospy.Subscriber('lift_height',
Float64,
self._move_lift,
queue_size=1)
# diagnostics
self._diag_array = DiagnosticArray()
self._diag_array.header.frame_id = self._base_frame
diag_status = DiagnosticStatus()
diag_status.hardware_id = 'Cozmo Robot'
diag_status.name = 'Cozmo Status'
diag_status.values.append(KeyValue(key='Battery Voltage', value=''))
diag_status.values.append(KeyValue(key='Head Angle', value=''))
diag_status.values.append(KeyValue(key='Lift Height', value=''))
self._diag_array.status.append(diag_status)
self._diag_pub = rospy.Publisher('/diagnostics',
DiagnosticArray,
queue_size=1)
# camera info manager
self._camera_info_manager.setURL(camera_info_url)
self._camera_info_manager.loadCameraInfo()
def add_sentence(self, nmea_string, frame_id, timestamp=None):
if not check_nmea_checksum(nmea_string):
rospy.logwarn("Received a sentence with an invalid checksum. " +
"Sentence was: %s" % repr(nmea_string))
return False
parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence(
nmea_string)
if not parsed_sentence:
rospy.logdebug("Failed to parse NMEA sentence. Sentence was: %s" %
nmea_string)
return False
if timestamp:
current_time = timestamp
else:
current_time = rospy.get_rostime()
current_fix = NavSatFix()
current_fix.header.stamp = current_time
current_fix.header.frame_id = frame_id
current_pose_utm = GpsLocal()
current_pose_utm.header.stamp = current_time
current_pose_utm.header.frame_id = frame_id
current_time_ref = TimeReference()
current_time_ref.header.stamp = current_time
current_time_ref.header.frame_id = frame_id
if self.time_ref_source:
current_time_ref.source = self.time_ref_source
else:
current_time_ref.source = frame_id
if not self.use_RMC and 'GGA' in parsed_sentence:
self.seq = self.seq + 1
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_APPROXIMATED
data = parsed_sentence['GGA']
gps_qual = data['fix_type']
try:
# Unpack the fix params for this quality value
current_fix.status.status, self.default_epe, self.fix_type = self.fix_mappings[
gps_qual]
except KeyError:
current_fix.status.status = NavSatStatus.STATUS_NO_FIX
self.fix_type = 'Unknown'
if gps_qual == 0:
current_fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
self.using_receiver_epe = False
self.invalid_cnt += 1
current_fix.status.service = NavSatStatus.SERVICE_GPS
current_fix.header.stamp = current_time
latitude = data['latitude']
if data['latitude_direction'] == 'S':
latitude = -latitude
current_fix.latitude = latitude
longitude = data['longitude']
if data['longitude_direction'] == 'W':
longitude = -longitude
current_fix.longitude = longitude
# use default epe std_dev unless we've received a GST sentence with epes
if not self.using_receiver_epe or math.isnan(self.lon_std_dev):
self.lon_std_dev = self.default_epe
if not self.using_receiver_epe or math.isnan(self.lat_std_dev):
self.lat_std_dev = self.default_epe
if not self.using_receiver_epe or math.isnan(self.alt_std_dev):
self.alt_std_dev = self.default_epe * 2
hdop = data['hdop']
current_fix.position_covariance[0] = (hdop * self.lon_std_dev)**2
current_fix.position_covariance[4] = (hdop * self.lat_std_dev)**2
current_fix.position_covariance[8] = (hdop *
self.alt_std_dev)**2 # FIXME
# Altitude is above ellipsoid, so adjust for mean-sea-level
altitude = data['altitude'] + data['mean_sea_level']
current_fix.altitude = altitude
self.fix_pub.publish(current_fix)
if not math.isnan(latitude) and not math.isnan(longitude) and (
gps_qual == 5 or gps_qual == 4):
UTMNorthing, UTMEasting = LLtoUTM(latitude, longitude)[0:2]
current_pose_utm.position.x = UTMEasting
current_pose_utm.position.y = UTMNorthing
current_pose_utm.position.z = altitude
# Pose x/y/z covariance is whatever we decided h & v covariance is.
# Here is it the same as for ECEF coordinates
current_pose_utm.covariance[0] = (hdop * self.lon_std_dev)**2
current_pose_utm.covariance[4] = (hdop * self.lat_std_dev)**2
current_pose_utm.covariance[8] = (hdop * self.alt_std_dev)**2
current_pose_utm.rtk_fix = True if gps_qual == 4 else False
self.local_pub.publish(current_pose_utm)
if not math.isnan(data['utc_time']):
current_time_ref.time_ref = rospy.Time.from_sec(
data['utc_time'])
self.time_ref_pub.publish(current_time_ref)
if (self.diag_pub_time < rospy.get_time()):
self.diag_pub_time += 1
diag_arr = DiagnosticArray()
diag_arr.header.stamp = rospy.get_rostime()
diag_arr.header.frame_id = frame_id
diag_msg = DiagnosticStatus()
diag_msg.name = 'GPS_status'
diag_msg.hardware_id = 'GPS'
diag_msg.level = DiagnosticStatus.OK
diag_msg.message = 'Received GGA Fix'
diag_msg.values.append(
KeyValue('Sequence number', str(self.seq)))
diag_msg.values.append(
KeyValue('Invalid fix count', str(self.invalid_cnt)))
diag_msg.values.append(KeyValue('Latitude', str(latitude)))
diag_msg.values.append(KeyValue('Longitude', str(longitude)))
diag_msg.values.append(KeyValue('Altitude', str(altitude)))
diag_msg.values.append(KeyValue('GPS quality', str(gps_qual)))
diag_msg.values.append(KeyValue('Fix type', self.fix_type))
diag_msg.values.append(
KeyValue('Number of satellites',
str(data['num_satellites'])))
diag_msg.values.append(
KeyValue('Receiver providing accuracy',
str(self.using_receiver_epe)))
diag_msg.values.append(KeyValue('Hdop', str(hdop)))
diag_msg.values.append(
KeyValue('Latitude std dev', str(hdop * self.lat_std_dev)))
diag_msg.values.append(
KeyValue('Longitude std dev',
str(hdop * self.lon_std_dev)))
diag_msg.values.append(
KeyValue('Altitude std dev', str(hdop * self.alt_std_dev)))
diag_arr.status.append(diag_msg)
self.diag_pub.publish(diag_arr)
elif 'RMC' in parsed_sentence:
data = parsed_sentence['RMC']
# Only publish a fix from RMC if the use_RMC flag is set.
if self.use_RMC:
if data['fix_valid']:
current_fix.status.status = NavSatStatus.STATUS_FIX
else:
current_fix.status.status = NavSatStatus.STATUS_NO_FIX
current_fix.status.service = NavSatStatus.SERVICE_GPS
latitude = data['latitude']
if data['latitude_direction'] == 'S':
latitude = -latitude
current_fix.latitude = latitude
longitude = data['longitude']
if data['longitude_direction'] == 'W':
longitude = -longitude
current_fix.longitude = longitude
current_fix.altitude = float('NaN')
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_UNKNOWN
self.fix_pub.publish(current_fix)
if not math.isnan(data['utc_time']):
current_time_ref.time_ref = rospy.Time.from_sec(
data['utc_time'])
self.time_ref_pub.publish(current_time_ref)
# Publish velocity from RMC regardless, since GGA doesn't provide it.
if data['fix_valid']:
current_vel = TwistStamped()
current_vel.header.stamp = current_time
current_vel.header.frame_id = frame_id
current_vel.twist.linear.x = data['speed'] * \
math.sin(data['true_course'])
current_vel.twist.linear.y = data['speed'] * \
math.cos(data['true_course'])
self.vel_pub.publish(current_vel)
elif 'GST' in parsed_sentence:
data = parsed_sentence['GST']
# Use receiver-provided error estimate if available
self.using_receiver_epe = True
self.lon_std_dev = data['lon_std_dev']
self.lat_std_dev = data['lat_std_dev']
self.alt_std_dev = data['alt_std_dev']
else:
return False
def drawCubicBezier(self, ele, da):
print('绘制贝塞尔')
# 开始点
ps = np.array([ele.start.real, ele.start.imag])
# 控制点
p1 = np.array([ele.control1.real, ele.control1.imag])
p2 = np.array([ele.control2.real, ele.control2.imag])
# 结束点
pe = np.array([ele.end.real, ele.end.imag])
result = self.CubicBezier(ps, p1, p2, pe, 0)
print(result)
start = int(result[0]), int(result[1])
z = 30.25
# 创建点
point = {
'x': str(start[0]),
'y': str(start[1]),
'z': str(z),
'type': 'MOVE',
'type2': 'drawCubicBezierStart'
}
points = DiagnosticStatus()
points.values.append(KeyValue('x', str(start[0])))
points.values.append(KeyValue('y', str(start[1])))
points.values.append(KeyValue('z', str(z)))
points.values.append(KeyValue('type', 'MOVE'))
points.values.append(KeyValue('type2', 'drawCubicBezierStart'))
da.status.append(points)
# 添加到容器中
self.points.append(point)
# 40个点
for i in range(1, 41):
result = self.CubicBezier(ps, p1, p2, pe, i / 40)
end = int(result[0]), int(result[1])
# 创建点
point = {
'x': str(end[0]),
'y': str(end[1]),
'z': str(z),
'type': 'MOVE',
'type2': 'drawCubicBezierEnd'
}
points = DiagnosticStatus()
points.values.append(KeyValue('x', str(end[0])))
points.values.append(KeyValue('y', str(end[1])))
points.values.append(KeyValue('z', str(z)))
points.values.append(KeyValue('type', 'MOVE'))
points.values.append(KeyValue('type2', 'drawCubicBezierEnd'))
da.status.append(points)
# 添加到容器中
self.points.append(point)
def __init__(self, msg=DiagnosticStatus(), timestamp=0):
self.msg = msg
self.timestamp = timestamp
def __init__(self, hostname, diag_hostname):
self._diag_pub = rospy.Publisher('/diagnostics',
DiagnosticArray,
queue_size=3)
self._mutex = threading.Lock()
self._check_ipmi = rospy.get_param('~check_ipmi_tool', True)
self._enforce_speed = rospy.get_param('~enforce_clock_speed', True)
self._check_core_temps = rospy.get_param('~check_core_temps', False)
if self._check_core_temps:
rospy.logwarn(
'Checking CPU core temperatures is deprecated. This will be removed in D-turtle'
)
self._check_nfs = rospy.get_param('~check_nfs', False)
if self._check_nfs:
rospy.logwarn(
'NFS checking is deprecated for CPU monitor. This will be removed in D-turtle'
)
# sane defaults according to http://blog.scoutapp.com/articles/2009/07/31/understanding-load-averages
self._load1_threshold = rospy.get_param('~load1_threshold',
0.7 * cpu_count())
self._load5_threshold = rospy.get_param('~load5_threshold',
1.0 * cpu_count())
self._num_cores = rospy.get_param('~num_cores', 8.0)
self._temps_timer = None
self._usage_timer = None
self._nfs_timer = None
# Get temp_input files
self._temp_vals = get_core_temp_names()
# CPU stats
self._temp_stat = DiagnosticStatus()
self._temp_stat.name = '%s CPU Temperature' % diag_hostname
self._temp_stat.level = 1
self._temp_stat.hardware_id = hostname
self._temp_stat.message = 'No Data'
self._temp_stat.values = [
KeyValue(key='Update Status', value='No Data'),
KeyValue(key='Time Since Last Update', value='N/A')
]
self._usage_stat = DiagnosticStatus()
self._usage_stat.name = '%s CPU Usage' % diag_hostname
self._usage_stat.level = 1
self._usage_stat.hardware_id = hostname
self._usage_stat.message = 'No Data'
self._usage_stat.values = [
KeyValue(key='Update Status', value='No Data'),
KeyValue(key='Time Since Last Update', value='N/A')
]
self._nfs_stat = DiagnosticStatus()
self._nfs_stat.name = '%s NFS IO' % diag_hostname
self._nfs_stat.level = 1
self._nfs_stat.hardware_id = hostname
self._nfs_stat.message = 'No Data'
self._nfs_stat.values = [
KeyValue(key='Update Status', value='No Data'),
KeyValue(key='Time Since Last Update', value='N/A')
]
self._last_temp_time = 0
self._last_usage_time = 0
self._last_nfs_time = 0
self._last_publish_time = 0
# Start checking everything
self.check_temps()
if self._check_nfs:
self.check_nfs_stat()
self.check_usage()
def initialize_origin():
global _origin
rospy.init_node('initialize_origin', anonymous=True)
ros_distro = os.environ.get('ROS_DISTRO')
if not ros_distro:
rospy.logerror('ROS_DISTRO environment variable was not set.')
exit(1)
if ros_distro == 'indigo':
# ROS Indigo uses the GPSFix message
global _origin_pub
global _local_xy_frame
_origin_pub = rospy.Publisher('/local_xy_origin', GPSFix, latch=True, queue_size=2)
diagnostic_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=2)
local_xy_origin = rospy.get_param('~local_xy_origin', 'auto')
_local_xy_frame = rospy.get_param('~local_xy_frame', 'map')
_local_xy_frame_identity = rospy.get_param('~local_xy_frame_identity', _local_xy_frame + "__identity")
if local_xy_origin == "auto":
global _sub
_sub = rospy.Subscriber("gps", GPSFix, gps_callback)
else:
parse_origin(local_xy_origin)
if len(_local_xy_frame):
tf_broadcaster = tf.TransformBroadcaster()
else:
tf_broadcaster = None
hw_id = rospy.get_param('~hw_id', 'none')
while not rospy.is_shutdown():
if tf_broadcaster:
# Publish transform involving map (to an anonymous unused
# frame) so that TransformManager can support /tf<->/wgs84
# conversions without requiring additional nodes.
tf_broadcaster.sendTransform(
(0, 0, 0),
(0, 0, 0, 1),
rospy.Time.now(),
_local_xy_frame_identity, _local_xy_frame)
if _gps_fix == None:
diagnostic = DiagnosticArray()
diagnostic.header.stamp = rospy.Time.now()
status = DiagnosticStatus()
status.name = "LocalXY Origin"
status.hardware_id = hw_id
status.level = DiagnosticStatus.ERROR
status.message = "No Origin"
diagnostic.status.append(status)
diagnostic_pub.publish(diagnostic)
else:
_origin_pub.publish(_gps_fix) # Publish this at 1Hz for bag convenience
diagnostic = DiagnosticArray()
diagnostic.header.stamp = rospy.Time.now()
status = DiagnosticStatus()
status.name = "LocalXY Origin"
status.hardware_id = hw_id
if local_xy_origin == 'auto':
status.level = DiagnosticStatus.OK
status.message = "Has Origin (auto)"
else:
status.level = DiagnosticStatus.WARN
status.message = "Origin is static (non-auto)"
value0 = KeyValue()
value0.key = "Origin"
value0.value = _gps_fix.status.header.frame_id
status.values.append(value0)
value1 = KeyValue()
value1.key = "Latitude"
value1.value = "%f" % _gps_fix.latitude
status.values.append(value1)
value2 = KeyValue()
value2.key = "Longitude"
value2.value = "%f" % _gps_fix.longitude
status.values.append(value2)
value3 = KeyValue()
value3.key = "Altitude"
value3.value = "%f" % _gps_fix.altitude
status.values.append(value3)
diagnostic.status.append(status)
diagnostic_pub.publish(diagnostic)
rospy.sleep(1.0)
else:
# ROS distros later than Indigo use NavSatFix
origin_pub = rospy.Publisher('/local_xy_origin', PoseStamped, latch=True, queue_size=2)
diagnostic_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=2)
origin_name = rospy.get_param('~local_xy_origin', 'auto')
rospy.loginfo('Local XY origin is "' + origin_name + '"')
origin_frame_id = rospy.get_param(rospy.search_param('local_xy_frame'), 'map')
origin_frame_identity = rospy.get_param('~local_xy_frame_identity', origin_frame_id + "__identity")
rospy.loginfo('Local XY frame ID is "' + origin_frame_id + '"')
if len(origin_frame_id):
tf_broadcaster = tf.TransformBroadcaster()
else:
tf_broadcaster = None
if origin_name != "auto":
origin_list = rospy.get_param('~local_xy_origins', [])
_origin = make_origin_from_list(origin_frame_id, origin_name, origin_list)
if _origin is not None:
origin_pub.publish(_origin)
else:
origin_name = "auto"
if origin_name == "auto":
sub = rospy.Subscriber("gps", NavSatFix)
sub.impl.add_callback(navsatfix_callback, (origin_frame_id, origin_pub, sub))
rospy.loginfo('Subscribed to NavSat on ' + sub.resolved_name)
while not rospy.is_shutdown():
if tf_broadcaster:
# Publish transform involving map (to an anonymous unused
# frame) so that TransformManager can support /tf<->/wgs84
# conversions without requiring additional nodes.
tf_broadcaster.sendTransform(
(0, 0, 0),
(0, 0, 0, 1),
rospy.Time.now(),
origin_frame_identity, origin_frame_id)
diagnostic_pub.publish(make_diagnostic(_origin, origin_name != "auto"))
rospy.sleep(1.0)
def __init__(self, coz):
"""
:type coz: cozmo.Robot
:param coz: The cozmo SDK robot handle (object).
"""
# vars
self._cozmo = coz
self._lin_vel = .0
self._ang_vel = .0
self._cmd_lin_vel = .0
self._cmd_ang_vel = .0
self._last_pose = self._cozmo.pose
self._wheel_vel = (0, 0)
self._optical_frame_orientation = quaternion_from_euler(
-np.pi / 2., .0, -np.pi / 2.)
self._camera_info_manager = CameraInfoManager(
'cozmo_camera', namespace='/cozmo_camera')
# tf
self._tfb = TransformBroadcaster()
# params
self._odom_frame = rospy.get_param('~odom_frame', 'odom')
self._footprint_frame = rospy.get_param('~footprint_frame',
'base_footprint')
self._base_frame = rospy.get_param('~base_frame', 'base_link')
self._head_frame = rospy.get_param('~head_frame', 'head_link')
self._camera_frame = rospy.get_param('~camera_frame', 'camera_link')
self._camera_optical_frame = rospy.get_param('~camera_optical_frame',
'cozmo_camera')
camera_info_url = rospy.get_param('~camera_info_url', '')
# pubs
self._joint_state_pub = rospy.Publisher('joint_states',
JointState,
queue_size=1)
self._odom_pub = rospy.Publisher('odom', Odometry, queue_size=1)
self._imu_pub = rospy.Publisher('imu', Imu, queue_size=1)
self._battery_pub = rospy.Publisher('battery',
BatteryState,
queue_size=1)
# Note: camera is published under global topic (preceding "/")
self._image_pub = rospy.Publisher('/cozmo_camera/image',
Image,
queue_size=10)
self._camera_info_pub = rospy.Publisher('/cozmo_camera/camera_info',
CameraInfo,
queue_size=10)
# subs
self._backpack_led_sub = rospy.Subscriber('backpack_led',
ColorRGBA,
self._set_backpack_led,
queue_size=1)
self._twist_sub = rospy.Subscriber('cmd_vel',
Twist,
self._twist_callback,
queue_size=1)
self._say_sub = rospy.Subscriber('say',
String,
self._say_callback,
queue_size=1)
self._head_sub = rospy.Subscriber('head_angle',
Float64,
self._move_head,
queue_size=1)
self._lift_sub = rospy.Subscriber('lift_height',
Float64,
self._move_lift,
queue_size=1)
# diagnostics
self._diag_array = DiagnosticArray()
self._diag_array.header.frame_id = self._base_frame
diag_status = DiagnosticStatus()
diag_status.hardware_id = 'Cozmo Robot'
diag_status.name = 'Cozmo Status'
diag_status.values.append(KeyValue(key='Battery Voltage', value=''))
diag_status.values.append(KeyValue(key='Head Angle', value=''))
diag_status.values.append(KeyValue(key='Lift Height', value=''))
self._diag_array.status.append(diag_status)
self._diag_pub = rospy.Publisher('/diagnostics',
DiagnosticArray,
queue_size=1)
# camera info manager
self._camera_info_manager.setURL(camera_info_url)
self._camera_info_manager.loadCameraInfo()
# Raise the head
self.move_head(10) # level the head
self.move_lift(0) # raise the lift
# Start the tasks
self.task = 0
self.goal_pose = self._cozmo.pose
self.action = None
self._cozmo.add_event_handler(cozmo.objects.EvtObjectAppeared,
self.handle_object_appeared)
self._cozmo.add_event_handler(cozmo.objects.EvtObjectDisappeared,
self.handle_object_disappeared)
self.front_wall = self._cozmo.world.define_custom_wall(
CustomObjectTypes.CustomType00, CustomObjectMarkers.Hexagons2, 300,
44, 63, 63, True)
self.ramp_bottom = self._cozmo.world.define_custom_wall(
CustomObjectTypes.CustomType01, CustomObjectMarkers.Triangles5,
100, 100, 63, 63, True)
self.ramp_top = self._cozmo.world.define_custom_wall(
CustomObjectTypes.CustomType02, CustomObjectMarkers.Diamonds2, 5,
5, 44, 44, True)
self.drop_spot = self._cozmo.world.define_custom_wall(
CustomObjectTypes.CustomType03, CustomObjectMarkers.Circles4, 70,
70, 63, 63, True)
self.back_wall = self._cozmo.world.define_custom_wall(
CustomObjectTypes.CustomType04, CustomObjectMarkers.Diamonds4, 300,
50, 63, 63, True)
self.drop_target = self._cozmo.world.define_custom_wall(
CustomObjectTypes.CustomType05, CustomObjectMarkers.Hexagons5, 5,
5, 32, 32, True)
self.drop_clue = self._cozmo.world.define_custom_wall(
CustomObjectTypes.CustomType06, CustomObjectMarkers.Triangles3, 5,
5, 32, 32, True)
self.front_wall_pose = None
self.ramp_bottom_pose = None
self.drop_spot_pose = None
self.back_wall_pose = None
self.drop_target_pose = None
self.drop_clue_pose = None
self.cube_found = False
self.target_cube = None
self.is_up_top = False # Is Cozmo on the platform
def hwboard(self):
# initialize local variables
send_channel = 0
read_channel = 0
send_specifier = 0
read_specifier = 0
read_status = 0
read_data = 0
read_id = 0
read_crc = 0
# init ros-node
pub = rospy.Publisher('diagnostics', DiagnosticArray)
while not rospy.is_shutdown():
# init publisher message
pub_message = DiagnosticArray()
# init array for storing data
status_array = []
# init local variable for error detection
error_while_reading = 0
for send_specifier in range(0, 7, 3):
if send_specifier == 0:
count_range = range(6)
elif send_specifier == 3:
count_range = [0, 1, 2, 3, 6, 7]
else:
count_range = [1, 2, 3, 6, 7]
for send_channel in count_range:
# init message and local variables
send_buff_array = [
send_channel, send_specifier, 0x00, 0x00, 0x00
]
message = ""
preamble_bytes = 4
preamble_error = 1
crc_error = 1
retry = 0
# calculate crc
crc = 0x00
for i in range(4):
data = send_buff_array[i]
for k in range(8):
feedback_bit = (crc ^ data) & 0x80
feedback_bit = (feedback_bit >> 7) & 0xFF
if feedback_bit == 1:
crc = (crc << 1) & 0xFF
crc = crc ^ 0x31
else:
crc = (crc << 1) & 0xFF
data = (data << 1) & 0xFF
send_buff_array[4] = crc
# send message
while (preamble_error == 1
or crc_error == 1) and retry < 8:
message = ""
for i in range(preamble_bytes):
message += chr(0x55)
for i in send_buff_array:
message += chr(i)
self.s.write(message)
# receive message
# check for first preamble byte of reveiced message
read_buff_array = []
buff = self.s.read(1)
preamble_count = 0
for i in buff:
read_buff_array.append(ord(i))
if read_buff_array[0] == 0x55:
# check for following preamble bytes
while read_buff_array[
0] == 0x55 and preamble_count < 10:
read_buff_array = []
buff = self.s.read(1)
for i in buff:
read_buff_array.append(ord(i))
preamble_count = preamble_count + 1
buff = self.s.read(13)
# check preamble length
if preamble_count > 6:
preamble_error = 1
preamble_bytes = preamble_bytes + 1
retry = retry + 1
if preamble_bytes == 7:
preamble_bytes = 2
elif preamble_count < 2:
preamble_error = 1
preamble_bytes = preamble_bytes + 1
retry = retry + 1
if preamble_bytes == 7:
preamble_bytes = 2
else:
# preamble ok. evaluate message
preamble_error = 0
# get remaining message
for i in buff:
read_buff_array.append(ord(i))
#check crc
crc = 0x00
for i in range(14):
data = read_buff_array[i]
for k in range(8):
feedback_bit = (crc ^ data) & 0x80
feedback_bit = (
feedback_bit >> 7) & 0xFF
if feedback_bit == 1:
crc = (crc << 1) & 0xFF
crc = crc ^ 0x31
else:
crc = (crc << 1) & 0xFF
data = (data << 1) & 0xFF
if crc != 0:
crc_error = 1
preamble_bytes = preamble_bytes + 1
retry = retry + 1
if preamble_bytes == 7:
preamble_bytes = 2
else:
crc_error = 0
# no preamble detected
else:
buff = s.read(14)
preamble_error = 1
preamble_bytes = preamble_bytes + 1
retry = retry + 1
if preamble_bytes == 7:
preamble_bytes = 2
# get channel byte
read_channel = int(read_buff_array[0])
# get specifier byte
read_specifier = int(read_buff_array[1])
# get status byte
read_status = int(read_buff_array[2])
# get data bytes
read_data = 256 * int(read_buff_array[3])
read_data = read_data + int(read_buff_array[4])
# get id bytes
read_id = read_buff_array[5] << 8
read_id = (read_id | read_buff_array[6]) << 8
read_id = (read_id | read_buff_array[7]) << 8
read_id = read_id | read_buff_array[8]
# evaluate recieved message
if read_channel == send_channel:
if read_specifier == send_specifier:
if read_status == 0 or read_status == 8:
if send_specifier == 0:
read_data = read_data / 10.0
else:
read_data = read_data / 1000.0
erro_while_reading = 0
else:
read_data = 0
error_while_reading = 1
else:
read_data = 0
error_while_reading = 1
else:
read_data = 0
error_while_reading = 1
#prepare status object for publishing
# init sensor object
status_object = DiagnosticStatus()
# init local variable for data
key_value = KeyValue()
# set values for temperature parameters
if send_specifier == 0:
if read_data == 85:
level = 1
status_object.message = "sensor damaged"
elif read_data > 50:
level = 2
status_object.message = "temperature critical"
elif read_data > 40:
level = 1
status_object.message = "temperature high"
elif read_data > 10:
level = 0
status_object.message = "temperature ok"
elif read_data > -1:
level = 1
status_object.message = "temperature low"
else:
level = 2
status_object.message = "temperature critical"
# mapping for temperature sensors
if read_id == self.head_sensor_param:
status_object.name = "Head Temperature"
status_object.hardware_id = "hwboard_channel " + str(
send_channel)
elif read_id == self.eye_sensor_param:
status_object.name = "Eye Camera Temperature"
status_object.hardware_id = "hwboard_channel = " + str(
send_channel)
elif read_id == self.torso_module_sensor_param:
status_object.name = "Torso Module Temperature"
status_object.hardware_id = "hwboard_channel =" + str(
send_channel)
elif read_id == self.torso_sensor_param:
status_object.name = "Torso Temperature"
status_object.hardware_id = "hwboard_channel =" + str(
send_channel)
elif read_id == self.pc_sensor_param:
status_object.name = "PC Temperature"
status_object.hardware_id = "hwboard_channel =" + str(
send_channel)
elif read_id == self.engine_sensor_param:
status_object.name = "Engine Temperature"
status_object.hardware_id = "hwboard_channel = " + str(
send_channel)
else:
level = 1
status_object.message = "cannot map if from yaml file to temperature sensor"
# set values for voltage parameters
elif send_specifier == 3:
if send_channel == 0:
if read_data > 58:
level = 2
status_object.message = "voltage critical"
elif read_data > 56:
level = 1
status_object.message = "voltage high"
elif read_data > 44:
level = 0
status_object.message = "voltage ok"
elif read_data > 42:
level = 1
status_object.message = "voltage low"
else:
level = 2
status_object.message = "voltage critical"
else:
if read_data > 27:
level = 2
status_object.message = "voltage critical"
elif read_data > 25:
level = 1
status_object.message = "voltage_high"
elif read_data > 23:
level = 0
status_object.message = "voltage ok"
elif read_data > 19:
level = 1
status_object.message = "voltage low"
else:
level = 2
status_object.message = "voltage critical"
if send_channel == 0:
status_object.name = "Akku Voltage"
status_object.hardware_id = "hwboard_channel = 0"
elif send_channel == 1:
status_object.name = "Torso Engine Voltage"
status_object.hardware_id = "hwboard_channel = 1"
elif send_channel == 2:
status_object.name = "Torso Logic Voltage"
status_object.hardware_id = "hwboard_channel = 2"
elif send_channel == 3:
status_object.name = "Tray Logic Voltage"
status_object.hardware_id = "hwboard_channel = 3"
elif send_channel == 6:
status_object.name = "Arm Engine Voltage"
status_object.hardware_id = "hwboard_channel = 6"
elif send_channel == 7:
status_object.name = "Tray Engine Voltage"
status_object.hardware_id = "hwboard_channel = 7"
# set values for current parameters
else:
if read_data > 15:
level = 2
status_object.message = "current critical"
elif read_data > 10:
level = 1
status_object.message = "current high"
elif read_data < 0:
level = 2
status_object.message = "current critical"
else:
level = 0
status_object.message = "current ok"
if send_channel == 1:
status_object.name = "Torso Engine Current"
status_object.hardware_id = "hwboard_channel = 1"
elif send_channel == 2:
status_object.name = "Torso Logic Current"
status_object.hardware_id = "hwboard_channel = 2"
elif send_channel == 3:
status_object.name = "Tray Logic Current"
status_object.hardware_id = "hwboard_channel = 3"
elif send_channel == 6:
status_object.name = "Arm Engine Current"
status_object.hardware_id = "hwboard_channel = 6"
elif send_channel == 7:
status_object.name = "Tray Engine Current"
status_object.hardware_id = "hwboard_channel = 7"
# evaluate error detection
if error_while_reading == 1:
level = 1
status_object.message = "detected error while receiving answer from hardware"
# append status object to publishing message
status_object.level = level
key_value.value = str(read_data)
status_object.values.append(key_value)
pub_message.status.append(status_object)
# publish message
pub.publish(pub_message)
rospy.sleep(1.0)
insock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
insock.bind(('',11111))
rospy.init_node('mbr_logging')
pubs = {}
diagnostic_pub = rospy.Publisher('/diagnostics',DiagnosticArray,queue_size=10)
while not rospy.is_shutdown():
data = insock.recv(1024)
values = data.decode('utf8').split(',')
sn = values[0]
if len(values) >= 24:
diag_array = DiagnosticArray()
diag_array.header.stamp = rospy.Time.now()
ds = DiagnosticStatus()
ds.name = 'mbr'
ds.hardware_id = values[0]
for i in range(len(fields)):
ds.values.append(KeyValue(fields[i],values[i]))
for i in (21,22):
if not sn+'/'+fields[i] in pubs:
pubs[sn+'/'+fields[i]] = rospy.Publisher('mbr/'+sn+'/'+fields[i],Float32, queue_size=10)
pubs[sn+'/'+fields[i]].publish(float(values[i]))
site_count = int(values[23])
for i in range(site_count):
base_index = 24+i*21
if len(values) >= base_index+21:
topic = 'mbr/'+sn+'/'+values[base_index+0]+'/mean_margin'
def ntp_monitor(ntp_hostname,
offset=500,
self_offset=500,
diag_hostname=None,
error_offset=5000000):
pub = rospy.Publisher("/diagnostics", DiagnosticArray, queue_size=50)
rospy.init_node(NAME, anonymous=True)
hostname = socket.gethostname()
if diag_hostname is None:
diag_hostname = hostname
stat = DiagnosticStatus()
stat.level = 0
stat.name = "NTP offset from " + diag_hostname + " to " + ntp_hostname
stat.message = "OK"
stat.hardware_id = hostname
stat.values = []
self_stat = DiagnosticStatus()
self_stat.level = DiagnosticStatus.OK
self_stat.name = "NTP self-offset for " + diag_hostname
self_stat.message = "OK"
self_stat.hardware_id = hostname
self_stat.values = []
while not rospy.is_shutdown():
for st, host, off in [(stat, ntp_hostname, offset),
(self_stat, hostname, self_offset)]:
try:
p = Popen(["ntpdate", "-q", host],
stdout=PIPE,
stdin=PIPE,
stderr=PIPE)
res = p.wait()
(o, e) = p.communicate()
except OSError, (errno, msg):
if errno == 4:
break #ctrl-c interrupt
else:
raise
if (res == 0):
measured_offset = float(re.search("offset (.*),",
o).group(1)) * 1000000
st.level = DiagnosticStatus.OK
st.message = "OK"
st.values = [
KeyValue("Offset (us)", str(measured_offset)),
KeyValue("Offset tolerance (us)", str(off)),
KeyValue("Offset tolerance (us) for Error",
str(error_offset))
]
if (abs(measured_offset) > off):
st.level = DiagnosticStatus.WARN
st.message = "NTP Offset Too High"
if (abs(measured_offset) > error_offset):
st.level = DiagnosticStatus.ERROR
st.message = "NTP Offset Too High"
else:
st.level = DiagnosticStatus.ERROR
st.message = "Error Running ntpdate. Returned %d" % res
st.values = [
KeyValue("Offset (us)", "N/A"),
KeyValue("Offset tolerance (us)", str(off)),
KeyValue("Offset tolerance (us) for Error",
str(error_offset)),
KeyValue("Output", o),
KeyValue("Errors", e)
]
msg = DiagnosticArray()
msg.header.stamp = rospy.get_rostime()
msg.status = [stat, self_stat]
pub.publish(msg)
time.sleep(1)
def __init__(self):
device = get_param('~device', 'auto')
baudrate = get_param('~baudrate', 0)
timeout = get_param('~timeout', 0.002)
if device == 'auto':
devs = mtdevice.find_devices()
if devs:
device, baudrate = devs[0]
rospy.loginfo("Detected MT device on port %s @ %d bps" %
(device, baudrate))
else:
rospy.logerr("Fatal: could not find proper MT device.")
rospy.signal_shutdown("Could not find proper MT device.")
return
if not baudrate:
baudrate = mtdevice.find_baudrate(device)
if not baudrate:
rospy.logerr("Fatal: could not find proper baudrate.")
rospy.signal_shutdown("Could not find proper baudrate.")
return
rospy.loginfo("MT node interface: %s at %d bd." % (device, baudrate))
self.mt = mtdevice.MTDevice(device, baudrate, timeout)
self.frame_id = get_param('~frame_id', '/base_imu')
self.frame_local = get_param('~frame_local', 'ENU')
self.diag_msg = DiagnosticArray()
self.stest_stat = DiagnosticStatus(name='mtnode: Self Test',
level=1,
message='No status information')
self.xkf_stat = DiagnosticStatus(name='mtnode: XKF Valid',
level=1,
message='No status information')
self.gps_stat = DiagnosticStatus(name='mtnode: GPS Fix',
level=1,
message='No status information')
self.diag_msg.status = [self.stest_stat, self.xkf_stat, self.gps_stat]
# publishers created at first use to reduce topic clutter
self.diag_pub = None
self.imu_pub = None
self.view_pub = None
self.ang_pub = None
self.ang_pub2 = None
self.ang_pub3 = None
self.gps_pub = None
self.vel_pub = None
self.mag_pub = None
self.temp_pub = None
self.press_pub = None
self.analog_in1_pub = None # decide type+header
self.analog_in2_pub = None # decide type+header
self.ecef_pub = None
self.time_ref_pub = None
# TODO pressure, ITOW from raw GPS?
self.old_bGPS = 256 # publish GPS only if new
# publish a string version of all data; to be parsed by clients
self.str_pub = rospy.Publisher('imu_data_str', String, queue_size=10)
outputString = commands.getoutput("sensors | grep \"fan3:\"")
split = outputString.split()
if (len(split) >= 2):
self.fanSpeed += "/" + split[1]
else:
self.fanSpeed += "/ "
if __name__ == '__main__':
signal.signal(signal.SIGINT, signal_handler)
rospy.init_node('systemStatus')
pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1)
array = DiagnosticArray()
status = DiagnosticStatus(name='SystemStatus',\
level=0,\
message='System Status')
array.status = [status]
if not rospy.has_param('/systemStatus/cpuTempHigh') or\
not rospy.has_param('/systemStatus/cpuTempCrit') or\
not rospy.has_param('/systemStatus/batteryLow') or\
not rospy.has_param('/systemStatus/batteryCrit') or\
not rospy.has_param('/systemStatus/wirelessLow') or\
not rospy.has_param('/systemStatus/wirelessCrit') or\
not rospy.has_param('/systemStatus/dataDrive') or\
not rospy.has_param('/systemStatus/dataDriveSpaceHighPercent') or\
not rospy.has_param('/systemStatus/dataDriveSpaceCritPercent'):
rospy.logerr("Could not get all SystemStatus parameters")
import roslib
roslib.load_manifest(PKG)
import rospy
from time import sleep
from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus
if __name__ == '__main__':
rospy.init_node('diag_pub')
pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=10)
start_time = rospy.get_time()
while not rospy.is_shutdown():
array = DiagnosticArray()
array.header.stamp = rospy.get_rostime()
array.status = [
# GenericAnalyzer my_path
DiagnosticStatus(0, 'multi', 'OK', '', []),
DiagnosticStatus(1, 'Something', 'OK', '', []),
DiagnosticStatus(2, 'Something Else', 'OK', '', []),
# OtherAnalyzer for Other
DiagnosticStatus(2, 'other2', 'OK', '', []),
DiagnosticStatus(0, 'other3', 'OK', '', [])
]
pub.publish(array)
sleep(1)
