def update(self):
try:
(world_trans, world_rot) = self.listener.lookupTransform(
self.utm_link, self.base_link, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException):
rospy.logerr_throttle_identical(
5, "Could not get transform between {} and {}".format(
self.utm_link, self.base_link))
return pt.Status.FAILURE
except:
rospy.logerr_throttle_identical(
5, "Could not do tf lookup for some other reason")
return pt.Status.FAILURE
self.bb.set(bb_enums.WORLD_TRANS, world_trans)
self.bb.set(bb_enums.WORLD_ROT, world_rot)
# also create this pointstamped object so that we can transform this
# easily to w/e other frame is needed later
ps = PointStamped()
ps.header.frame_id = self.utm_link
ps.header.stamp = rospy.Time(0)
ps.point.x = world_trans[0]
ps.point.y = world_trans[1]
ps.point.z = world_trans[2]
self.bb.set(bb_enums.LOCATION_POINT_STAMPED, ps)
return pt.Status.SUCCESS
def update(self):
"""
Check only to see whether the underlying action server has
succeeded, is running, or has cancelled/aborted for some reason and
map these to the usual behaviour return states.
"""
if not self.action_server_ok:
self.feedback_message = "Action Server for gotowp action can not be used!"
rospy.logerr_throttle_identical(5, self.feedback_message)
return pt.Status.FAILURE
# if your action client is not valid
if not self.action_client:
self.feedback_message = "ActionClient is invalid! Client:" + str(
self.action_client)
rospy.logerr(self.feedback_message)
return pt.Status.FAILURE
# if the action_goal is invalid
if not self.action_goal:
self.feedback_message = "No action_goal!"
rospy.logwarn(self.feedback_message)
return pt.Status.FAILURE
# if goal hasn't been sent yet
if not self.sent_goal:
self.action_goal_handle = self.action_client.send_goal(
self.action_goal, feedback_cb=self.feedback_cb)
self.sent_goal = True
rospy.loginfo("Sent goal to action server:" +
str(self.action_goal))
self.feedback_message = "Goal sent"
return pt.Status.RUNNING
# if the goal was aborted or preempted
if self.action_client.get_state() in [
actionlib_msgs.GoalStatus.ABORTED,
actionlib_msgs.GoalStatus.PREEMPTED
]:
self.feedback_message = "Aborted goal"
rospy.loginfo(self.feedback_message)
return pt.Status.FAILURE
result = self.action_client.get_result()
# if the goal was accomplished
if result:
self.feedback_message = "Completed goal"
rospy.loginfo(self.feedback_message)
return pt.Status.SUCCESS
return pt.Status.RUNNING
def path_to_list(self, path_msg):
frame = path_msg.header.frame_id
if frame != '' and frame != self.local_frame:
rospy.logerr_throttle_identical(5, "Refined waypoints are not in "+self.local_frame+" they are in "+frame+" !")
return []
wps = []
for pose_stamped in path_msg.poses:
wp = (
pose_stamped.pose.position.x,
pose_stamped.pose.position.y,
pose_stamped.pose.position.z
)
wps.append(wp)
return wps
def latlon_to_utm(lat, lon, z, latlontoutm_service_name):
rospy.loginfo("Waiting for latlontoutm service "+str(latlontoutm_service_name))
rospy.wait_for_service(latlontoutm_service_name)
rospy.loginfo("Got latlontoutm service")
try:
latlontoutm_service = rospy.ServiceProxy(latlontoutm_service_name,
LatLonToUTM)
gp = GeoPoint()
gp.latitude = np.degrees(lat)
gp.longitude = np.degrees(lon)
gp.altitude = z
res = latlontoutm_service(gp)
return (res.utm_point.x, res.utm_point.y)
except rospy.service.ServiceException:
rospy.logerr_throttle_identical(5, "LatLon to UTM service failed! namespace:{}".format(latlontoutm_service_name))
return (None, None)
def update(self):
if not self.action_server_ok:
self.feedback_message = "Action Server for emergency action can not be used!"
rospy.logerr_throttle_identical(5, self.feedback_message)
return pt.Status.FAILURE
# if your action client is not valid
if not self.action_client:
self.feedback_message = "ActionClient for emergency action is invalid!"
rospy.logwarn_throttle_identical(5, self.feedback_message)
return pt.Status.FAILURE
# if the action_goal is invalid
if not self.action_goal:
self.feedback_message = "No action_goal!"
rospy.logwarn(self.feedback_message)
return pt.Status.FAILURE
# if goal hasn't been sent yet
if not self.sent_goal:
self.action_goal_handle = self.action_client.send_goal(
self.action_goal, feedback_cb=self.feedback_cb)
self.sent_goal = True
rospy.loginfo("Sent goal to action server:" +
str(self.action_goal))
self.feedback_message = "Emergency goal sent"
return pt.Status.RUNNING
# if the goal was aborted or preempted
if self.action_client.get_state() in [
actionlib_msgs.GoalStatus.ABORTED,
actionlib_msgs.GoalStatus.PREEMPTED
]:
self.feedback_message = "Aborted emergency"
rospy.loginfo(self.feedback_message)
return pt.Status.FAILURE
result = self.action_client.get_result()
# if the goal was accomplished
if result:
self.feedback_message = "Completed emergency"
rospy.loginfo(self.feedback_message)
return pt.Status.SUCCESS
# if we're still trying to accomplish the goal
return pt.Status.RUNNING
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)