def cbTag(self, tag_msgs):
if self.fsm_mode == "INTERSECTION_CONTROL" or self.fsm_mode == "INTERSECTION_COORDINATION" or self.fsm_mode == "INTERSECTION_PLANNING":
#loop through list of april tags
# filter out the nearest apriltag
dis_min = 999
idx_min = -1
for idx, taginfo in enumerate(tag_msgs.infos):
if (taginfo.tag_type == taginfo.SIGN):
tag_det = (tag_msgs.detections)[idx]
pos = tag_det.pose.pose.position
distance = math.sqrt(pos.x**2 + pos.y**2 + pos.z**2)
if distance < dis_min:
dis_min = distance
idx_min = idx
if idx_min != -1:
taginfo = (tag_msgs.infos)[idx_min]
availableTurns = []
#go through possible intersection types
signType = taginfo.traffic_sign_type
if (signType == taginfo.NO_RIGHT_TURN
or signType == taginfo.LEFT_T_INTERSECT):
rospy.loginfo("left T")
availableTurns = [
0, 1
] # these mystical numbers correspond to the array ordering in open_loop_intersection_control_node (very bad)
elif (signType == taginfo.NO_LEFT_TURN
or signType == taginfo.RIGHT_T_INTERSECT):
rospy.loginfo("right T")
availableTurns = [1, 2]
elif (signType == taginfo.FOUR_WAY):
availableTurns = [0, 1, 2]
elif (signType == taginfo.T_INTERSECTION):
rospy.loginfo("center T")
availableTurns = [0, 2]
#now randomly choose a possible direction
if (len(availableTurns) > 0):
if len(self.route) == 0:
randomIndex = numpy.random.randint(
len(availableTurns)) # по часовой
chosenTurn = availableTurns[randomIndex]
# if randomIndex == 1:
# self.pub_turn.publish("1")
# if randomIndex == 0:
# self.pub_turn.publish("2")
else:
chosenTurn = availableTurns[::-1][self.route[0] - 1]
del self.route[0]
self.turn_type = chosenTurn
self.pub_turn_type.publish(self.turn_type)
id_and_type_msg = TurnIDandType()
id_and_type_msg.tag_id = taginfo.id
id_and_type_msg.turn_type = self.turn_type
self.pub_id_and_type.publish(id_and_type_msg)
rospy.loginfo("possible turns %s." % (availableTurns))
def cbTag(self, tag_msgs):
if (self.fsm_mode == "INTERSECTION_CONTROL"
or self.fsm_mode == "INTERSECTION_COORDINATION"
or self.fsm_mode == "INTERSECTION_PLANNING"):
# loop through list of april tags
# filter out the nearest apriltag
dis_min = 999
idx_min = -1
for idx, taginfo in enumerate(tag_msgs.infos):
if taginfo.tag_type == taginfo.SIGN:
tag_det = (tag_msgs.detections)[idx]
pos = tag_det.transform.translation
distance = math.sqrt(pos.x**2 + pos.y**2 + pos.z**2)
if distance < dis_min:
dis_min = distance
idx_min = idx
if idx_min != -1:
taginfo = (tag_msgs.infos)[idx_min]
availableTurns = []
# go through possible intersection types
signType = taginfo.traffic_sign_type
if signType == taginfo.NO_RIGHT_TURN or signType == taginfo.LEFT_T_INTERSECT:
availableTurns = [
0,
1,
] # these mystical numbers correspond to the array ordering in open_loop_intersection_control_node (very bad)
elif signType == taginfo.NO_LEFT_TURN or signType == taginfo.RIGHT_T_INTERSECT:
availableTurns = [1, 2]
elif signType == taginfo.FOUR_WAY:
availableTurns = [0, 1, 2]
elif signType == taginfo.T_INTERSECTION:
availableTurns = [0, 2]
rospy.loginfo(
f"[{self.node_name}] reports Available turns are: [{availableTurns}]"
)
# now randomly choose a possible direction
if len(availableTurns) > 0:
# 3501: turn off right turns
# randomIndex = numpy.random.randint(len(availableTurns))
# chosenTurn = availableTurns[randomIndex]
while True:
randomIndex = numpy.random.randint(len(availableTurns))
chosenTurn = availableTurns[randomIndex]
rospy.loginfo("Turn type now: %i" % (chosenTurn))
if chosenTurn != 2:
break
# end of fix
self.turn_type = chosenTurn
self.pub_turn_type.publish(self.turn_type)
id_and_type_msg = TurnIDandType()
id_and_type_msg.tag_id = taginfo.id
id_and_type_msg.turn_type = self.turn_type
self.pub_id_and_type.publish(id_and_type_msg)
def cbIntersectionGo(self, msg):
if not self.active:
return
if not msg.data: return
while self.turn_type == -1:
if not self.active:
return
rospy.loginfo("Requested to start intersection, but we do not see an april tag yet.")
rospy.sleep(2)
tag_id = self.tag_id
turn_type = self.turn_type
sleeptimes = [self.time_left_turn, self.time_straight_turn, self.time_right_turn]
LFparams = [self.LFparams_left, self.LFparams_straight, self.LFparams_right]
omega_ffs = [self.ff_left, self.ff_straight, self.ff_right]
omega_maxs = [self.omega_max_left, self.omega_max_straight, self.omega_max_right]
omega_mins = [self.omega_min_left, self.omega_min_straight, self.omega_min_right]
self.changeLFParams(LFparams[turn_type], sleeptimes[turn_type]+1.0)
rospy.set_param("~lane_controller/omega_ff", omega_ffs[turn_type])
rospy.set_param("~lane_controller/omega_max", omega_maxs[turn_type])
rospy.set_param("~lane_controller/omega_min", omega_mins[turn_type])
# Waiting for LF to adapt to new params
rospy.sleep(1)
rospy.loginfo("Starting intersection control - driving to " + str(turn_type))
self.forward_pose = True
rospy.sleep(sleeptimes[turn_type])
self.forward_pose = False
rospy.set_param("~lane_controller/omega_ff", 0)
rospy.set_param("~lane_controller/omega_max", 999)
rospy.set_param("~lane_controller/omega_min", -999)
# Publish intersection done
msg_done = BoolStamped()
msg_done.data = True
self.pub_int_done.publish(msg_done)
# Publish intersection done detailed
msg_done_detailed = TurnIDandType()
msg_done_detailed.tag_id = tag_id
msg_done_detailed.turn_type = turn_type
self.pub_int_done_detailed.publish(msg_done_detailed)
def pathProcessor(self, path, cmd):
# Take current node and command (note: inputs are also 'self.' defined)
cmd_comb = [path[0], cmd[0]]
# Set up outgoing message type
new_cmd = TurnIDandType()
new_cmd.tag_id = cmd_comb[0]
if cmd_comb[1] == 0:
new_cmd.turn_type = 0
rospy.loginfo("Turn left")
elif cmd_comb[1] == 1:
new_cmd.turn_type = 1
rospy.loginfo("Go straight")
elif cmd_comb[1] == 2:
new_cmd.turn_type = 2
rospy.loginfo("Turn right")
return new_cmd
def cbTag(self, tag_msgs):
valid_modes = [
"INTERSECTION_CONTROL", "INTERSECTION_COORDINATION",
"INTERSECTION_PLANNING"
]
if self.fsm_mode in valid_modes:
#loop through list of april tags
# filter out the nearest apriltag
dis_min = 999
idx_min = -1
for idx, taginfo in enumerate(tag_msgs.infos):
if (taginfo.tag_type == taginfo.SIGN):
tag_det = (tag_msgs.detections)[idx]
pos = tag_det.pose.pose.position
distance = math.sqrt(pos.x**2 + pos.y**2 + pos.z**2)
if distance < dis_min:
dis_min = distance
idx_min = idx
if idx_min != -1:
taginfo = (tag_msgs.infos)[idx_min]
availableTurns = []
signType = taginfo.traffic_sign_type
msg = BoolStamped()
msg.header.stamp = rospy.Time.now()
msg.data = False
if self.searching_for_parking:
if signType == TagInfo.PARKING:
msg.data = True
if taginfo.id == 66:
availableTurns = [
1
] # Forward to enter parking area
elif taginfo.id == 63:
availableTurns = [0] # Left to enter parking area
elif taginfo.id == 10:
availableTurns = [2] # Right to enter parking area
else:
#go through possible intersection types
if (signType == taginfo.NO_RIGHT_TURN
or signType == taginfo.LEFT_T_INTERSECT):
availableTurns = [
0, 1
] # these mystical numbers correspond to the array ordering in open_loop_intersection_control_node (very bad)
elif (signType == taginfo.NO_LEFT_TURN
or signType == taginfo.RIGHT_T_INTERSECT):
availableTurns = [1, 2]
elif (signType == taginfo.FOUR_WAY):
availableTurns = [0, 1, 2]
elif (signType == taginfo.T_INTERSECTION):
availableTurns = [0, 2]
#now randomly choose a possible direction
if (len(availableTurns) > 0):
randomIndex = numpy.random.randint(len(availableTurns))
chosenTurn = availableTurns[randomIndex]
self.turn_type = chosenTurn
self.pub_turn_type.publish(self.turn_type)
id_and_type_msg = TurnIDandType()
id_and_type_msg.tag_id = taginfo.id
id_and_type_msg.turn_type = self.turn_type
# `True` if parking intersection, `False` otherwise
self.parking_intersection_pub.publish(msg)
self.pub_id_and_type.publish(id_and_type_msg)
def cbTag(self, tag_msgs):
if self.fsm_mode == "INTERSECTION_CONTROL" or self.fsm_mode == "INTERSECTION_COORDINATION" or self.fsm_mode == "INTERSECTION_PLANNING":
#loop through list of april tags
# filter out the nearest apriltag
dis_min = 999
idx_min = -1
for idx, taginfo in enumerate(tag_msgs.infos):
if (taginfo.tag_type == taginfo.SIGN):
tag_det = (tag_msgs.detections)[idx]
pos = tag_det.pose.pose.position
distance = math.sqrt(pos.x**2 + pos.y**2 + pos.z**2)
if distance < dis_min and tag_msgs.detections[
idx].id >= 9 and tag_msgs.detections[idx].id <= 11:
dis_min = distance
idx_min = idx
if idx_min != -1:
taginfo = (tag_msgs.infos)[idx_min]
availableTurns = []
#go through possible intersection types
signType = taginfo.traffic_sign_type
if (signType == taginfo.NO_RIGHT_TURN
or signType == taginfo.LEFT_T_INTERSECT):
availableTurns = [
0, 1
] # these mystical numbers correspond to the array ordering in open_loop_intersection_control_node (very bad)
elif (signType == taginfo.NO_LEFT_TURN
or signType == taginfo.RIGHT_T_INTERSECT):
availableTurns = [1, 2]
elif (signType == taginfo.FOUR_WAY):
availableTurns = [0, 1, 2]
elif (signType == taginfo.T_INTERSECTION):
availableTurns = [0, 2]
#now randomly choose a possible direction
if (len(availableTurns) > 0):
randomIndex = numpy.random.randint(len(availableTurns))
ids = []
for i in tag_msgs.detections:
ids.append(i.id)
denis_turn_type = int(tag_msgs.detections[idx_min].id)
for i in range(5):
print("DTP :::: " + str(denis_turn_type))
print("IDS::" + str(ids))
global gb
if gb is None:
gb = GraphBuilder()
if (min(ids) < 9):
min_id = int(min(ids))
else:
while (True):
print("VERTEX ID NOT DETECTED! === ")
time.sleep(2)
chosenTurn = gb.get_next_turn(
vertex_qr_code=min_id, turns_qr_code=denis_turn_type)
for i in range(5):
print("Calculated turn :::: " + str(chosenTurn))
print("Vertex id :::: " + str(min_id))
if chosenTurn == -1:
#gb.visualize()
while (True):
print("Graph built")
time.sleep(10)
self.turn_type = chosenTurn
self.pub_turn_type.publish(self.turn_type)
id_and_type_msg = TurnIDandType()
id_and_type_msg.tag_id = taginfo.id
id_and_type_msg.turn_type = self.turn_type
self.pub_id_and_type.publish(id_and_type_msg)