class StatusManager:
def __init__(self, config):
# X. Initialize announcer.
self.announcer = TextToSpeech()
self.announcer.add_speech_text(
"Initializing system. Please wait for a moment.")
# X. Tf
self.broadcaster = tf.TransformBroadcaster()
self.listener = tf.TransformListener()
# Prepare shared object.
self.data = SharedData()
# Publishers & Subscribers
self.pubs = Publishers()
self.subs = Subscribers(self.announcer)
# Store tf
if (not config.pose_init):
while (self.subs.odom.get_object() is None):
time.sleep(0.5)
odom = self.subs.odom.get_object()
odom_xyzrpy = create_xyzrpy_from_pose(odom.pose.pose)
pose_stamped = PoseStamped()
pose_stamped.header.stamp = rospy.Time.now()
pose_stamped.header.frame_id = 'map'
pose_stamped.pose = create_pose_from_xyzrpy(config.xyzrpy)
self.data.last_valid_ndt_pose = pose_stamped
self.data.last_valid_tf_odom_to_map = create_transform_from_pose(
config.xyzrpy, odom_xyzrpy)
# Initilize Pose
self.initialize_pose(config)
# Start checking thread
self.checking_thread = threading.Thread(target=self.run_checking)
self.checking_thread.start()
# Start control
self.run_control()
# Terminate thread.
self.announcer.terminate()
self.checking_thread.join()
def initialize_pose(self, config):
init_xyzrpy = config.xyzrpy
if (config.via_gnss):
# X. Wait update from gnss.
while (self.subs.gnss_pose.get_object() is None
and not rospy.is_shutdown()):
rospy.logwarn("Waiting for GNSS data to be received.")
time.sleep(0.5)
init_xyzrpy = create_xyzrpy_from_pose(
self.subs.gnss_pose.get_object().pose)
init_xyzrpy.roll = 0
init_xyzrpy.pitch = 0
init_xyzrpy.yaw = 0
# X. Start pose initializer if necessary.
if (config.pose_init):
self.announcer.add_speech_text(
"Start pose initializer. This will take about twenty seconds.")
# X. Switch to low resolution.
self.pubs.cfg_voxel.publish(CONFIG_VOXEL_FILT_SEARCH())
req = FULL_INIT_REQUEST(init_xyzrpy)
init_xyzrpy = call_pose_initialize_service(req)
# X. Set high resolution mode.
self.pubs.cfg_voxel.publish(CONFIG_VOXEL_FILT_RUN())
# X. Send for ndt localizer.
self.pubs.cfg_ntd.publish(DEFAULT_CONFIG_NDT(init_xyzrpy))
# X. Send for amcl localizer.
rospy.logwarn("amcl : {}".format(
create_pose_stamped_with_cov(init_xyzrpy)))
self.pubs.amcl_init.publish(create_pose_stamped_with_cov(init_xyzrpy))
# X. Notify
self.announcer.add_speech_text(
"Pose initialization is done. Start control.")
def run_pose_initialize_if_necessary(self):
if (self.data.last_pose_init_time is None \
or rospy.Time.now() - self.data.last_pose_init_time > rospy.Duration(LOCALIZATION_REINIT_MINIMUM_PERIOD)):
if (LOCALIZATION_CNT_REINIT <=
self.data.localization_not_reliable_cnt):
self.data.pose_initializing = True
INIT_POSE_STANDSTILL_COUNT = 10
wait_cnt = 0
while (self.data.standstill_count <
INIT_POSE_STANDSTILL_COUNT):
wait_cnt += 1
if (wait_cnt % 3):
self.announcer.add_speech_text("Wait robot to stop.")
time.sleep(1.0)
if (self.data.last_valid_ndt_pose is not None \
and rospy.Time.now() - self.data.last_valid_ndt_pose.header.stamp < rospy.Duration(LOCALIZATION_ODOM_TRUST_PERIOD)):
self.announcer.add_speech_text(
"Reinitialize localization based on odometry. Robot will stop."
)
config = StatusManagerConfig(
True, False,
create_xyzrpy_from_pose(
self.data.last_cur_pose.get_object().pose))
else:
self.announcer.add_speech_text(
"Reinitialize localization based on GNSS. Robot will stop."
)
config = StatusManagerConfig(
True, True, XYZRPY(0.0, 0.0, 0.0, 0.0, 0.0, 0.0))
# X. This function takes time.
self.initialize_pose(config)
self.data.last_pose_init_time = rospy.Time.now()
self.data.pose_initializing = False
self.data.localization_not_reliable_cnt \
= max(0, self.data.localization_not_reliable_cnt - LOCALIZATION_CNT_REINIT / 2)
def check_robot_stop_reason(self):
obs_idx = self.subs.obst_idx.get_object()
if (obs_idx is None or obs_idx.data == -1):
self.data.stopped_due_to_obstable_cnt = 0
return
twist_raw = self.subs.twist_raw.get_object()
if (twist_raw is None or \
not twist_is_almost_zero(twist_raw.twist, STANDSTILL_VX_THR, STANDSTILL_WX_THR)):
self.data.stopped_due_to_obstable_cnt = 0
return
fin_wps = self.subs.final_wps.get_object()
#ndt_pose = self.subs.ndt_pose.get_object()
last_cur_pose = self.data.last_cur_pose.get_object()
if (fin_wps is None or last_cur_pose is None):
self.data.stopped_due_to_obstable_cnt = 0
return
print("Distance to obstacle : {}".format(
compute_distance_to_obstacle_on_waypoint(obs_idx.data, fin_wps,
last_cur_pose.pose)))
if (compute_distance_to_obstacle_on_waypoint(
obs_idx.data, fin_wps, last_cur_pose.pose) < OBSTACLE_DIST):
if (self.data.stopped_due_to_obstable_cnt % 3 == 0):
self.announcer.add_speech_text(
"Robot stops due to obstacle in front.")
self.data.stopped_due_to_obstable_cnt += 1
if (WAIT_BEFORE_AVOID_OBST_SEC < \
self.data.stopped_due_to_obstable_cnt / STATUS_CHECK_HZ):
if (is_avoidance_ok_waypoint(obs_idx.data, fin_wps)):
self.announcer.add_speech_text(
"Rerouting for obstacle avoidance.")
header = Header()
header.stamp = rospy.Time.now()
self.pubs.astar_avoid.publish(header)
self.data.stopped_due_to_obstable_cnt = 0
else:
self.announcer.add_speech_text(
"Rerouting is not allowed here. Keep waiting.")
self.data.stopped_due_to_obstable_cnt = 0
def run_checking(self):
rate = rospy.Rate(STATUS_CHECK_HZ)
while not rospy.is_shutdown():
# X. Robot stuck reason.
self.check_robot_stop_reason()
# X. Localization status.
self.run_pose_initialize_if_necessary()
rate.sleep()
def run_control(self):
rate = rospy.Rate(CONTROL_LOOP_HZ)
while not rospy.is_shutdown():
cur_time = rospy.Time.now()
# X. Control command.
twist_st = self.subs.twist_raw.get_object()
if (twist_st is not None and not self.data.pose_initializing):
self.pubs.cmd_vel.publish(
self.subs.twist_raw.get_object().twist)
else:
self.pubs.cmd_vel.publish(
create_twist(0.0, 0.0, 0.0, 0.0, 0.0, 0.0))
# X. Velocity routing.
odom = self.subs.odom.get_object()
if (odom is not None):
twist_stamped = TwistStamped()
twist_stamped.header.stamp = cur_time
twist_stamped.twist = odom.twist.twist
self.pubs.cur_vel.publish(twist_stamped)
if (twist_is_almost_zero(odom.twist.twist, STANDSTILL_VX_THR,
STANDSTILL_WX_THR)):
self.data.standstill_count += 1
else:
self.data.standstill_count = 0
# X. Check pose validity
if (not self.data.pose_initializing):
self.check_pose_validity()
# X. Current pose
self.publish_curpose(cur_time)
rate.sleep()
def check_pose_validity(self):
# X. When map pose is unstable.
ndt_pose = self.subs.ndt_pose.get_object()
amcl_pose = self.subs.amcl_pose.get_object()
if (ndt_pose is not None and amcl_pose is not None and \
not pose_is_almost_same(ndt_pose, amcl_pose.pose, \
LOCALIZATION_TRANS_THRESH, LOCALIZATION_YAW_THRESH)):
self.data.localization_not_reliable_cnt = \
min(self.data.localization_not_reliable_cnt + 1, LOCALIZATION_CNT_REINIT)
# X. When map pose is stable.
else:
# X. Extract last valid pose.
if (ndt_pose is not None and amcl_pose is not None and \
pose_is_almost_same(ndt_pose, amcl_pose.pose, \
LOCALIZATION_TRANS_RELIABLE_THRESH, LOCALIZATION_YAW_RELIABLE_THRESH)):
try:
transform = self.listener.lookupTransform(
'map', 'odom', ndt_pose.header.stamp)
self.data.last_valid_ndt_pose_queue.append(
[ndt_pose, transform])
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
pass
# If reaches stable count, treat the first one as valid pose.
if (len(self.data.last_valid_ndt_pose_queue) ==
LOCALIZATION_RELIABLE_CNT):
self.data.last_valid_ndt_pose = self.data.last_valid_ndt_pose_queue[
0][0]
self.data.last_valid_tf_odom_to_map = self.data.last_valid_ndt_pose_queue[
0][1]
else:
self.data.last_valid_ndt_pose_queue.clear()
self.data.localization_not_reliable_cnt \
= max(0, self.data.localization_not_reliable_cnt - 1)
def publish_curpose(self, cur_time):
odom = self.subs.odom.get_object()
if (odom is not None):
pose_stamped = PoseStamped()
pose_stamped.header.stamp = cur_time
pose_stamped.header.frame_id = "map"
pose_st = self.subs.ndt_pose.get_object()
if (pose_st is not None and \
self.data.localization_not_reliable_cnt == 0 and \
not self.data.pose_initializing):
# X. Use ndt pose as cur_pose
pose_stamped.pose = pose_st.pose
transform = create_tf_transform_from_pose(
pose_st.pose, odom.pose.pose)
# X. Send tf
self.broadcaster.sendTransform(transform[0], transform[1],
cur_time, 'odom', 'map')
self.pubs.cur_pose.publish(pose_stamped)
self.data.last_cur_pose.set_object(pose_stamped)
self.data.ndt_pose_unreliable_cnt = 0
else:
# X. Use odometry as cur_pose
if (self.data.last_valid_tf_odom_to_map is not None):
trans = self.data.last_valid_tf_odom_to_map[0]
rot = self.data.last_valid_tf_odom_to_map[1]
# X. Send tf
self.broadcaster.sendTransform(trans, rot, cur_time,
'odom', 'map')
trans_pose = transform_pose(odom.pose.pose, trans, rot)
pose_stamped.pose = trans_pose
self.pubs.cur_pose.publish(pose_stamped)
self.data.last_cur_pose.set_object(pose_stamped)
self.data.ndt_pose_unreliable_cnt += 1
if (self.data.ndt_pose_unreliable_cnt %
(5 * CONTROL_LOOP_HZ) == 0):
self.announcer.add_speech_text(
"Ndt pose is unstable. ")
