def _cb(self, msg):
json_dir = msg.data
if not osp.isdir(json_dir):
rospy.logfatal_throttle(
10, 'Input json_dir is not directory: %s' % json_dir)
return
bridge = cv_bridge.CvBridge()
if 'item_location' in self._types:
filename = osp.join(json_dir, 'item_location_file.json')
if osp.exists(filename):
order_file = osp.join(json_dir, 'order_file.json')
if not osp.exists(order_file):
order_file = None
img = jsk_arc2017_common.visualize_item_location(
filename, order_file)
imgmsg = bridge.cv2_to_imgmsg(img, encoding='rgb8')
imgmsg.header.stamp = rospy.Time.now()
self.pub_item_location.publish(imgmsg)
else:
rospy.logwarn_throttle(10, '%s does not exists yet' % filename)
if 'order' in self._types:
filename = osp.join(json_dir, 'order_file.json')
if osp.exists(filename):
img = jsk_arc2017_common.visualize_order(filename)
imgmsg = bridge.cv2_to_imgmsg(img, encoding='rgb8')
imgmsg.header.stamp = rospy.Time.now()
self.pub_order.publish(imgmsg)
else:
rospy.logwarn_throttle(10, '%s does not exists yet' % filename)
def _cb(self, msg):
json_dir = msg.data
if not osp.isdir(json_dir):
rospy.logfatal_throttle(
10, 'Input json_dir is not directory: %s' % json_dir)
return
bridge = cv_bridge.CvBridge()
if 'item_location' in self._types:
filename = osp.join(json_dir, 'item_location_file.json')
if osp.exists(filename):
order_file = osp.join(json_dir, 'order_file.json')
if not osp.exists(order_file):
order_file = None
img = jsk_arc2017_common.visualize_item_location(
filename, order_file)
imgmsg = bridge.cv2_to_imgmsg(img, encoding='rgb8')
imgmsg.header.stamp = rospy.Time.now()
self.pub_item_location.publish(imgmsg)
else:
rospy.logwarn_throttle(10, '%s does not exists yet' % filename)
if 'order' in self._types:
filename = osp.join(json_dir, 'order_file.json')
if osp.exists(filename):
img = jsk_arc2017_common.visualize_order(filename)
imgmsg = bridge.cv2_to_imgmsg(img, encoding='rgb8')
imgmsg.header.stamp = rospy.Time.now()
self.pub_order.publish(imgmsg)
else:
rospy.logwarn_throttle(10, '%s does not exists yet' % filename)
def _timer_cb(self, event):
if self.json_dir is None:
rospy.logwarn_throttle(10, 'Input json_dir is not set.')
return
if not osp.isdir(self.json_dir):
rospy.logfatal_throttle(
10, 'Input json_dir is not directory: %s' % self.json_dir)
return
filename = osp.join(self.json_dir, 'item_location_file.json')
if osp.exists(filename):
with open(filename) as location_f:
data = json.load(location_f)
bin_contents = {}
for bin_ in data['bins']:
bin_contents[bin_['bin_id']] = bin_['contents']
tote_contents = data['tote']['contents']
if self.target_location[:3] == 'bin':
contents = bin_contents[self.target_location[4]]
elif self.target_location == 'tote':
contents = tote_contents
else:
return
candidates_fixed = [
l for l in self.label_names if l.startswith('__')
]
candidates = candidates_fixed + contents
label_list = [self.label_names.index(x) for x in candidates]
label_list = sorted(label_list)
labels = []
for label in label_list:
label_msg = Label()
label_msg.id = label
label_msg.name = self.label_names[label]
labels.append(label_msg)
msg = LabelArray()
msg.labels = labels
msg.header.stamp = rospy.Time.now()
self.pub.publish(msg)
def _timer_cb(self, event):
if self.json_dir is None:
rospy.logwarn_throttle(10, 'Input json_dir is not set.')
return
if not osp.isdir(self.json_dir):
rospy.logfatal_throttle(
10, 'Input json_dir is not directory: %s' % self.json_dir)
return
filename = osp.join(self.json_dir, 'item_location_file.json')
if osp.exists(filename):
with open(filename) as location_f:
data = json.load(location_f)
bin_contents = {}
for bin_ in data['bins']:
bin_contents[bin_['bin_id']] = bin_['contents']
tote_contents = data['tote']['contents']
if self.target_location[:3] == 'bin':
contents = bin_contents[self.target_location[4]]
elif self.target_location == 'tote':
contents = tote_contents
else:
return
candidates_fixed = [l for l in self.label_names
if l.startswith('__')]
candidates = candidates_fixed + contents
label_list = [self.label_names.index(x) for x in candidates]
label_list = sorted(label_list)
labels = []
for label in label_list:
label_msg = Label()
label_msg.id = label
label_msg.name = self.label_names[label]
labels.append(label_msg)
msg = LabelArray()
msg.labels = labels
msg.header.stamp = rospy.Time.now()
self.pub.publish(msg)
def yuv_callback(self, yuv_frame):
# Use OpenCV to convert the yuv frame to RGB
info = yuv_frame.info(
) # the VideoFrame.info() dictionary contains some useful information such as the video resolution
rospy.logdebug_throttle(10, "yuv_frame.info = " + str(info))
cv2_cvt_color_flag = {
olympe.PDRAW_YUV_FORMAT_I420: cv2.COLOR_YUV2BGR_I420,
olympe.PDRAW_YUV_FORMAT_NV12: cv2.COLOR_YUV2BGR_NV12,
}[info["yuv"]["format"]] # convert pdraw YUV flag to OpenCV YUV flag
cv2frame = cv2.cvtColor(yuv_frame.as_ndarray(), cv2_cvt_color_flag)
# Publish image
msg_image = self.bridge.cv2_to_imgmsg(cv2frame, "bgr8")
self.pub_image.publish(msg_image)
# yuv_frame.vmeta() returns a dictionary that contains additional metadata from the drone (GPS coordinates, battery percentage, ...)
metadata = yuv_frame.vmeta()
rospy.logdebug_throttle(10, "yuv_frame.vmeta = " + str(metadata))
if metadata[1] != None:
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = '/body'
frame_timestamp = metadata[1][
'frame_timestamp'] # timestamp [millisec]
msg_time = Time()
msg_time.data = frame_timestamp # secs = int(frame_timestamp//1e6), nsecs = int(frame_timestamp%1e6*1e3)
self.pub_time.publish(msg_time)
drone_quat = metadata[1]['drone_quat'] # attitude
msg_attitude = QuaternionStamped()
msg_attitude.header = header
msg_attitude.quaternion = Quaternion(drone_quat['x'],
-drone_quat['y'],
-drone_quat['z'],
drone_quat['w'])
self.pub_attitude.publish(msg_attitude)
location = metadata[1][
'location'] # GPS location [500.0=not available] (decimal deg)
msg_location = PointStamped()
if location != {}:
msg_location.header = header
msg_location.header.frame_id = '/world'
msg_location.point.x = location['latitude']
msg_location.point.y = location['longitude']
msg_location.point.z = location['altitude']
self.pub_location.publish(msg_location)
ground_distance = metadata[1]['ground_distance'] # barometer (m)
self.pub_height.publish(ground_distance)
speed = metadata[1]['speed'] # opticalflow speed (m/s)
msg_speed = Vector3Stamped()
msg_speed.header = header
msg_speed.header.frame_id = '/world'
msg_speed.vector.x = speed['north']
msg_speed.vector.y = -speed['east']
msg_speed.vector.z = -speed['down']
self.pub_speed.publish(msg_speed)
air_speed = metadata[1][
'air_speed'] # air speed [-1=no data, > 0] (m/s)
self.pub_air_speed.publish(air_speed)
link_goodput = metadata[1][
'link_goodput'] # throughput of the connection (b/s)
self.pub_link_goodput.publish(link_goodput)
link_quality = metadata[1]['link_quality'] # [0=bad, 5=good]
self.pub_link_quality.publish(link_quality)
wifi_rssi = metadata[1][
'wifi_rssi'] # signal strength [-100=bad, 0=good] (dBm)
self.pub_wifi_rssi.publish(wifi_rssi)
battery_percentage = metadata[1][
'battery_percentage'] # [0=empty, 100=full]
self.pub_battery.publish(battery_percentage)
state = metadata[1][
'state'] # ['LANDED', 'MOTOR_RAMPING', 'TAKINGOFF', 'HOWERING', 'FLYING', 'LANDING', 'EMERGENCY']
self.pub_state.publish(state)
mode = metadata[1][
'mode'] # ['MANUAL', 'RETURN_HOME', 'FLIGHT_PLAN', 'TRACKING', 'FOLLOW_ME', 'MOVE_TO']
self.pub_mode.publish(mode)
msg_pose = PoseStamped()
msg_pose.header = header
msg_pose.pose.position = msg_location.point
msg_pose.pose.position.z = ground_distance
msg_pose.pose.orientation = msg_attitude.quaternion
self.pub_pose.publish(msg_pose)
Rot = R.from_quat([
drone_quat['x'], -drone_quat['y'], -drone_quat['z'],
drone_quat['w']
])
drone_rpy = Rot.as_euler('xyz')
msg_odometry = Odometry()
msg_odometry.header = header
msg_odometry.child_frame_id = '/body'
msg_odometry.pose.pose = msg_pose.pose
msg_odometry.twist.twist.linear.x = math.cos(
drone_rpy[2]) * msg_speed.vector.x + math.sin(
drone_rpy[2]) * msg_speed.vector.y
msg_odometry.twist.twist.linear.y = -math.sin(
drone_rpy[2]) * msg_speed.vector.x + math.cos(
drone_rpy[2]) * msg_speed.vector.y
msg_odometry.twist.twist.linear.z = msg_speed.vector.z
self.pub_odometry.publish(msg_odometry)
# log battery percentage
if battery_percentage >= 30:
if battery_percentage % 10 == 0:
rospy.loginfo_throttle(
100, "Battery level: " + str(battery_percentage) + "%")
else:
if battery_percentage >= 20:
rospy.logwarn_throttle(
10, "Low battery: " + str(battery_percentage) + "%")
else:
if battery_percentage >= 10:
rospy.logerr_throttle(
1, "Critical battery: " + str(battery_percentage) +
"%")
else:
rospy.logfatal_throttle(
0.1,
"Empty battery: " + str(battery_percentage) + "%")
# log signal strength
if wifi_rssi <= -60:
if wifi_rssi >= -70:
rospy.loginfo_throttle(
100, "Signal strength: " + str(wifi_rssi) + "dBm")
else:
if wifi_rssi >= -80:
rospy.logwarn_throttle(
10, "Weak signal: " + str(wifi_rssi) + "dBm")
else:
if wifi_rssi >= -90:
rospy.logerr_throttle(
1,
"Unreliable signal:" + str(wifi_rssi) + "dBm")
else:
rospy.logfatal_throttle(
0.1,
"Unusable signal: " + str(wifi_rssi) + "dBm")
else:
rospy.logwarn("Packet lost!")
def test_log(self):
rosout_logger = logging.getLogger('rosout')
import rospy
self.assertTrue(len(rosout_logger.handlers) == 2)
self.assertTrue(rosout_logger.handlers[0],
rosgraph.roslogging.RosStreamHandler)
self.assertTrue(rosout_logger.handlers[1],
rospy.impl.rosout.RosOutHandler)
default_ros_handler = rosout_logger.handlers[0]
# Remap stdout for testing
lout = StringIO()
lerr = StringIO()
test_ros_handler = rosgraph.roslogging.RosStreamHandler(colorize=False,
stdout=lout,
stderr=lerr)
try:
# hack to replace the stream handler with a debug version
rosout_logger.removeHandler(default_ros_handler)
rosout_logger.addHandler(test_ros_handler)
import rospy
rospy.loginfo("test 1")
lout_last = lout.getvalue().splitlines()[-1]
self.assert_("test 1" in lout_last)
rospy.logwarn("test 2")
lerr_last = lerr.getvalue().splitlines()[-1]
self.assert_("[WARN]" in lerr_last)
self.assert_("test 2" in lerr_last)
rospy.logerr("test 3")
lerr_last = lerr.getvalue().splitlines()[-1]
self.assert_("[ERROR]" in lerr_last)
self.assert_("test 3" in lerr_last)
rospy.logfatal("test 4")
lerr_last = lerr.getvalue().splitlines()[-1]
self.assert_("[FATAL]" in lerr_last)
self.assert_("test 4" in lerr_last)
# logXXX_once
lout_len = -1
for i in range(3):
rospy.loginfo_once("test 1")
lout_last = lout.getvalue().splitlines()[-1]
if i == 0:
self.assert_("test 1" in lout_last)
lout_len = len(lout.getvalue())
else: # making sure the length of lout doesnt change
self.assert_(lout_len == len(lout.getvalue()))
lerr_len = -1
for i in range(3):
rospy.logwarn_once("test 2")
lerr_last = lerr.getvalue().splitlines()[-1]
if i == 0:
self.assert_("test 2" in lerr_last)
lerr_len = len(lerr.getvalue())
else: # making sure the length of lerr doesnt change
self.assert_(lerr_len == len(lerr.getvalue()))
lerr_len = -1
for i in range(3):
rospy.logerr_once("test 3")
lerr_last = lerr.getvalue().splitlines()[-1]
if i == 0:
self.assert_("test 3" in lerr_last)
lerr_len = len(lerr.getvalue())
else: # making sure the length of lerr doesnt change
self.assert_(lerr_len == len(lerr.getvalue()))
lerr_len = -1
for i in range(3):
rospy.logfatal_once("test 4")
lerr_last = lerr.getvalue().splitlines()[-1]
if i == 0:
self.assert_("test 4" in lerr_last)
lerr_len = len(lerr.getvalue())
else: # making sure the length of lerr doesnt change
self.assert_(lerr_len == len(lerr.getvalue()))
# logXXX_throttle
lout_len = -1
for i in range(3):
rospy.loginfo_throttle(3, "test 1")
lout_last = lout.getvalue().splitlines()[-1]
if i == 0:
self.assert_("test 1" in lout_last)
lout_len = len(lout.getvalue())
rospy.sleep(rospy.Duration(1))
elif i == 1: # making sure the length of lout doesnt change
self.assert_(lout_len == len(lout.getvalue()))
rospy.sleep(rospy.Duration(2))
else:
self.assert_("test 1" in lout_last)
lerr_len = -1
for i in range(3):
rospy.logwarn_throttle(3, "test 2")
lerr_last = lerr.getvalue().splitlines()[-1]
if i == 0:
self.assert_("test 2" in lerr_last)
lerr_len = len(lerr.getvalue())
rospy.sleep(rospy.Duration(1))
elif i == 1: # making sure the length of lerr doesnt change
self.assert_(lerr_len == len(lerr.getvalue()))
rospy.sleep(rospy.Duration(2))
else:
self.assert_("test 2" in lerr_last)
lerr_len = -1
for i in range(3):
rospy.logerr_throttle(3, "test 3")
lerr_last = lerr.getvalue().splitlines()[-1]
if i == 0:
self.assert_("test 3" in lerr_last)
lerr_len = len(lerr.getvalue())
rospy.sleep(rospy.Duration(1))
elif i == 1: # making sure the length of lerr doesnt change
self.assert_(lerr_len == len(lerr.getvalue()))
rospy.sleep(rospy.Duration(2))
else:
self.assert_("test 3" in lerr_last)
lerr_len = -1
for i in range(3):
rospy.logfatal_throttle(3, "test 4")
lerr_last = lerr.getvalue().splitlines()[-1]
if i == 0:
self.assert_("test 4" in lerr_last)
lerr_len = len(lerr.getvalue())
rospy.sleep(rospy.Duration(1))
elif i == 1: # making sure the length of lerr doesnt change
self.assert_(lerr_len == len(lerr.getvalue()))
rospy.sleep(rospy.Duration(2))
else:
self.assert_("test 4" in lerr_last)
# logXXX_throttle_identical
lout_len = -1
for i in range(5):
if i < 2:
test_text = "test 1"
else:
test_text = "test 1a"
rospy.loginfo_throttle_identical(2, test_text)
lout_last = lout.getvalue().splitlines()[-1]
if i == 0: # Confirm test text was logged
self.assert_(test_text in lout_last)
lout_len = len(lout.getvalue())
elif i == 1: # Confirm the length of lout hasn't changed
self.assert_(lout_len == len(lout.getvalue()))
elif i == 2: # Confirm the new message was logged correctly
self.assert_(test_text in lout_last)
lout_len = len(lout.getvalue())
rospy.sleep(rospy.Duration(2))
elif i == 3: # Confirm the length of lout has changed
self.assert_(lout_len != len(lout.getvalue()))
else: # Confirm new test text is in last log
self.assert_(test_text in lout_last)
lerr_len = -1
for i in range(5):
if i < 2:
test_text = "test 2"
else:
test_text = "test 2a"
rospy.logwarn_throttle_identical(2, test_text)
lerr_last = lerr.getvalue().splitlines()[-1]
if i == 0: # Confirm test text was logged
self.assert_(test_text in lerr_last)
lerr_len = len(lerr.getvalue())
elif i == 1: # Confirm the length of lerr hasn't changed
self.assert_(lerr_len == len(lerr.getvalue()))
elif i == 2: # Confirm the new message was logged correctly
self.assert_(test_text in lerr_last)
lerr_len = len(lerr.getvalue())
rospy.sleep(rospy.Duration(2))
elif i == 3: # Confirm the length of lerr has incremented
self.assert_(lerr_len != len(lerr.getvalue()))
else: # Confirm new test text is in last log
self.assert_(test_text in lerr_last)
lerr_len = -1
for i in range(5):
if i < 2:
test_text = "test 3"
else:
test_text = "test 3a"
rospy.logerr_throttle_identical(2, test_text)
lerr_last = lerr.getvalue().splitlines()[-1]
if i == 0: # Confirm test text was logged
self.assert_(test_text in lerr_last)
lerr_len = len(lerr.getvalue())
elif i == 1: # Confirm the length of lerr hasn't changed
self.assert_(lerr_len == len(lerr.getvalue()))
elif i == 2: # Confirm the new message was logged correctly
self.assert_(test_text in lerr_last)
lerr_len = len(lerr.getvalue())
rospy.sleep(rospy.Duration(2))
elif i == 3: # Confirm the length of lerr has incremented
self.assert_(lerr_len != len(lerr.getvalue()))
else: # Confirm new test text is in last log
self.assert_(test_text in lerr_last)
lerr_len = -1
for i in range(5):
if i < 2:
test_text = "test 4"
else:
test_text = "test 4a"
rospy.logfatal_throttle_identical(2, test_text)
lerr_last = lerr.getvalue().splitlines()[-1]
if i == 0: # Confirm test text was logged
self.assert_(test_text in lerr_last)
lerr_len = len(lerr.getvalue())
elif i == 1: # Confirm the length of lerr hasn't changed
self.assert_(lerr_len == len(lerr.getvalue()))
elif i == 2: # Confirm the new message was logged correctly
self.assert_(test_text in lerr_last)
lerr_len = len(lerr.getvalue())
rospy.sleep(rospy.Duration(2))
elif i == 3: # Confirm the length of lerr has incremented
self.assert_(lerr_len != len(lerr.getvalue()))
else: # Confirm new test text is in last log
self.assert_(test_text in lerr_last)
rospy.loginfo("test child logger 1", logger_name="log1")
lout_last = lout.getvalue().splitlines()[-1]
self.assert_("test child logger 1" in lout_last)
rospy.logwarn("test child logger 2", logger_name="log2")
lerr_last = lerr.getvalue().splitlines()[-1]
self.assert_("[WARN]" in lerr_last)
self.assert_("test child logger 2" in lerr_last)
rospy.logerr("test child logger 3", logger_name="log3")
lerr_last = lerr.getvalue().splitlines()[-1]
self.assert_("[ERROR]" in lerr_last)
self.assert_("test child logger 3" in lerr_last)
rospy.logfatal("test child logger 4", logger_name="log4")
lerr_last = lerr.getvalue().splitlines()[-1]
self.assert_("[FATAL]" in lerr_last)
self.assert_("test child logger 4" in lerr_last)
finally:
# restoring default ros handler
rosout_logger.removeHandler(test_ros_handler)
lout.close()
lerr.close()
rosout_logger.addHandler(default_ros_handler)
def readOdom(self, odom_msg):
avaliable = self.listener.canTransform("/map", "/odom", rospy.Time())
if not avaliable:
rospy.logfatal_throttle(10, "TF frame is avaliable %s" % avaliable)
return
self.listener.waitForTransform("/map", "/odom", rospy.Time(),
rospy.Duration(9999))
self.pose_odom.pose = odom_msg.pose.pose
self.pose_odom.header.stamp = rospy.Time()
self.pose_odom.header.frame_id = '/odom'
# transform the odom expression under map frame
# t1 = time.time()
self.pose_map = self.listener.transformPose("/map", self.pose_odom)
# t2 = time.time()
x = self.pose_map.pose.position.x
y = self.pose_map.pose.position.y
angle = quat2eul([
self.pose_map.pose.orientation.w, self.pose_map.pose.orientation.x,
self.pose_map.pose.orientation.y, self.pose_map.pose.orientation.z
])
theta = angle[0]
self.odom_x = x
self.odom_y = y
self.odom_z = theta
# odx, ody = odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y
# send data back to serial port
# TODO read all self variable here in order to avoid sudden change
vx, wz = self.vel_vx, self.vel_wz
ID = self.ID
px, py, pz = self.posx, self.posy, self.posz
status = self.moveStatus
cmd_srv = self.cmd_srv
arg_srv = self.arg_srv
# TODO Sending velocity part
if vx < 0:
self.content[0] = 255 + int(vx)
self.content[1] = int((1 - (vx % 1)) * 100) % 100
else:
self.content[0] = int(vx)
self.content[1] = int((vx % 1) * 100)
if wz < 0:
self.content[2] = 255 + int(wz)
self.content[3] = int((1 - (wz % 1)) * 100) % 100
else:
self.content[2] = int(wz)
self.content[3] = int((wz % 1) * 100)
self.content[4] = 0
self.content[5] = 2 if vx > 0 else 0
# TODO Sending odom part
if x < 0:
self.content[6] = 255 + int(x)
self.content[7] = int((1 - (x % 1)) * 100) % 100
else:
self.content[6] = int(x)
self.content[7] = int((x % 1) * 100)
if y < 0:
self.content[8] = 255 + int(y)
self.content[9] = int((1 - (y % 1)) * 100) % 100
else:
self.content[8] = int(y)
self.content[9] = int((y % 1) * 100)
if theta < 0:
ang = (2 * np.pi + theta) * 10000
self.content[10] = int(ang / 256)
self.content[11] = int(ang % 256)
else:
ang = theta * 10000
self.content[10] = int(ang / 256)
self.content[11] = int(ang % 256)
# TODO sending goal/initial pose part
if px < 0:
self.content[12] = 255 + int(px)
self.content[13] = int((1 - (px % 1)) * 100) % 100
else:
self.content[12] = int(px)
self.content[13] = int((px % 1) * 100)
if py < 0:
self.content[14] = 255 + int(py)
self.content[15] = int((1 - (py % 1)) * 100) % 100
else:
self.content[14] = int(py)
self.content[15] = int((py % 1) * 100)
if pz < 0:
ang = (2 * np.pi + pz) * 10000
self.content[16] = int(ang / 256)
self.content[17] = int(ang % 256)
else:
ang = pz * 10000
self.content[16] = int(ang / 256)
self.content[17] = int(ang % 256)
# TODO commands part
self.content[18] = cmd_srv
self.content[19] = arg_srv / 256
self.content[20] = arg_srv % 256
# TODO status part
self.content[21] = status
# cmd_serial = [self.sendSize] + [ID] + self.content
# odx, ody = odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y
# print("%3.4f,%3.4f, %s, x=%2.3f, y=%2.3f, th=%2.3f, xo=%2.3f, yo=%2.3f" %((t2-t1), (t3-t2),[ID] + self.content, x, y, theta, odx, ody))
# cs = checksum([ID] + self.content)
# cs_high = cs / 256
# cs_low = cs % 256
# cmdstr_start = array("B", self.header + cmd_serial + [cs_high, cs_low]).tostring()
# self.device.write(cmdstr_start)
return
def test_log(self):
# we can't do anything fancy here like capture stdout as rospy
# grabs the streams before we do. Instead, just make sure the
# routines don't crash.
real_stdout = sys.stdout
real_stderr = sys.stderr
try:
try:
from cStringIO import StringIO
except ImportError:
from io import StringIO
sys.stdout = StringIO()
sys.stderr = StringIO()
import rospy
rospy.loginfo("test 1")
self.assert_("test 1" in sys.stdout.getvalue())
rospy.logwarn("test 2")
self.assert_("[WARN]" in sys.stderr.getvalue())
self.assert_("test 2" in sys.stderr.getvalue())
sys.stderr = StringIO()
rospy.logerr("test 3")
self.assert_("[ERROR]" in sys.stderr.getvalue())
self.assert_("test 3" in sys.stderr.getvalue())
sys.stderr = StringIO()
rospy.logfatal("test 4")
self.assert_("[FATAL]" in sys.stderr.getvalue())
self.assert_("test 4" in sys.stderr.getvalue())
# logXXX_throttle
for i in range(3):
sys.stdout = StringIO()
rospy.loginfo_throttle(3, "test 1")
if i == 0:
self.assert_("test 1" in sys.stdout.getvalue())
rospy.sleep(rospy.Duration(1))
elif i == 1:
self.assert_("" == sys.stdout.getvalue())
rospy.sleep(rospy.Duration(2))
else:
self.assert_("test 1" in sys.stdout.getvalue())
for i in range(3):
sys.stderr = StringIO()
rospy.logwarn_throttle(3, "test 2")
if i == 0:
self.assert_("test 2" in sys.stderr.getvalue())
rospy.sleep(rospy.Duration(1))
elif i == 1:
self.assert_("" == sys.stderr.getvalue())
rospy.sleep(rospy.Duration(2))
else:
self.assert_("test 2" in sys.stderr.getvalue())
for i in range(3):
sys.stderr = StringIO()
rospy.logerr_throttle(3, "test 3")
if i == 0:
self.assert_("test 3" in sys.stderr.getvalue())
rospy.sleep(rospy.Duration(1))
elif i == 1:
self.assert_("" == sys.stderr.getvalue())
rospy.sleep(rospy.Duration(2))
else:
self.assert_("test 3" in sys.stderr.getvalue())
for i in range(3):
sys.stderr = StringIO()
rospy.logfatal_throttle(3, "test 4")
if i == 0:
self.assert_("test 4" in sys.stderr.getvalue())
rospy.sleep(rospy.Duration(1))
elif i == 1:
self.assert_("" == sys.stderr.getvalue())
rospy.sleep(rospy.Duration(2))
else:
self.assert_("test 4" in sys.stderr.getvalue())
finally:
sys.stdout = real_stdout
sys.stderr = real_stderr
