def get_cur_maze_pose(pose_listener):
try:
trans = pose_listener.lookupTransform('map', 'base_footprint', rospy.Time(0))
point = map_to_maze(MazePoint(trans[0][0], trans[0][1]))
except:
point = None
return point
def __init__(self):
rospy.Subscriber(STATUS_SUB_TOPIC,
Statuses,
self._status_cb,
queue_size=10)
self.pub_to_parking = rospy.Publisher(POLY_PUB_TOPIC,
Polygons,
queue_size=10)
self.pub_to_cmd = rospy.Publisher(CMD_PUB_TOPIC, UInt8, queue_size=10)
self.pub_to_rviz = list()
for i in range(3):
new_pub = rospy.Publisher(RVIZ_PUB_TOPIC + str(i + 1),
PolygonStamped,
queue_size=10)
self.pub_to_rviz.append(new_pub)
self.parking_polygones, self.rviz_polygones = ParkingSolver._create_polygones(
)
self.fullness = list([-1, -1, -1])
self.PARKING_GOALS = tuple(
(MazePoint(5, 14), MazePoint(5, 16), MazePoint(5, 18)))
self.PARKING_WAITING_GOAL = MazePoint(3, 16)
def process_speed():
return maze_to_map(MazePoint(1, 7)), 1.57
def process(self, maze_crnt_pose):
""" Calcaulate goal_point (in map format) and update path if necessary
- if success: return actual goal point and pub path
- if error: return default goal_point
- if white line: return None that means stop
"""
DEFAULT_GOAL_POINT = MazePoint(MAZE_TARGET_X, MAZE_TARGET_Y)
WHITE_LINE_GOAL_POINT = None
MAP_DIRECTION = 1.57
try:
path = self.maze.get_path()
except:
rospy.logerr("Maze processing error: path can't be empty!")
self.is_recovery_need = True
return DEFAULT_GOAL_POINT, MAP_DIRECTION
local = self.maze.get_local_target()
# Process errors in old path
if maze_crnt_pose is None:
rospy.logerr("Maze processing error: maze crnt pose must exists!")
self.is_recovery_need = True
return DEFAULT_GOAL_POINT, MAP_DIRECTION
elif local is None:
rospy.logerr("Maze processing error: local target must exists!")
self.is_recovery_need = True
return DEFAULT_GOAL_POINT, MAP_DIRECTION
# Process white line and traffic light
self._update_status_of_white_line()
self._update_status_of_crnt_sign()
if self.is_wl_appeared:
if self.tl_color == TLColor.GREEN:
rospy.loginfo("There is WL and TL is green: way is clear.")
self.is_wl_appeared = False
elif self.tl_color == TLColor.UNKNOWN:
rospy.logwarn("There is WL but TL is unknown: is it error?")
self.is_wl_appeared = False
else:
rospy.loginfo_throttle(2, "There is WL and TL is red: wait...")
return WHITE_LINE_GOAL_POINT, MAP_DIRECTION
# Update next local target and pub new path if possible
if local.coord == maze_crnt_pose:
if self.maze.next_local_target() is False:
rospy.logerr(
"Current path length is 0, can't get next tartet!")
self.is_recovery_need = True
return DEFAULT_GOAL_POINT, MAP_DIRECTION
if len(path) == 0:
rospy.logerr("Path is empty!")
self.is_recovery_need = True
return DEFAULT_GOAL_POINT, MAP_DIRECTION
# Process signs
if self.sign_type != SignType.NO_SIGN:
rospy.loginfo("A sign was detected: %s", str(self.sign_type))
self.maze.set_limitation(SIGN_TYPE_TO_MAZE[self.sign_type.value])
try:
path = self.maze.get_path()
except:
rospy.logerr("Maze processing error: path is empty!")
self.is_recovery_need = True
return DEFAULT_GOAL_POINT, MAP_DIRECTION
# Return actual goal point
self._pub_path(path)
goal_point = maze_to_map(path[-1].coord)
return goal_point, MAP_DIRECTION
def reinit(self, maze_initial_pose):
self.maze = Maze(MazeSolver.STRUCTURE, MazeSolver.EDGE_WALLS)
self.maze.set_target(MazePoint(MAZE_TARGET_X, MAZE_TARGET_Y))
self.maze.set_state(
PointDir(maze_initial_pose.x, maze_initial_pose.y, 'L'))
class MazeSolver(object):
STRUCTURE = np.array(
[[8, 8, 8, 0, 0, 0, 0, 0, 0, 0], [8, 8, 8, 0, 8, 8, 0, 8, 8, 0],
[8, 8, 8, 0, 0, 0, 0, 0, 8, 0], [8, 8, 8, 0, 8, 0, 8, 0, 0, 0],
[8, 8, 8, 0, 8, 0, 8, 8, 8, 0], [8, 8, 0, 0, 0, 0, 0, 0, 0, 0],
[8, 8, 8, 0, 8, 8, 0, 8, 8, 0], [8, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[8, 8, 8, 0, 8, 8, 0, 8, 8, 0], [8, 8, 0, 0, 0, 0, 0, 0, 0, 0]],
np.uint8)
EDGE_WALLS = [[MazePoint(6, 9), MazePoint(7, 9)]]
def __init__(self, maze_initial_pose):
self.wl_msg = UInt8()
self.is_wl_appeared = False
self.tl_color = TLColor.UNKNOWN
self.sign_msg = Detections()
self.sign_type = SignType.NO_SIGN
self.is_recovery_need = False
self.line_detector = LineDetectorRos(math.pi*7/16, math.pi*9/16, 10, \
"stereo_camera_converted/left/image_raw", \
"img_from_core")
self.PATH_PUB = rospy.Publisher(PATH_PUB_TOPIC, Path, queue_size=5)
#rospy.Subscriber(SIGN_SUB_TOPIC, UInt8, self._sign_cb, queue_size=10)
rospy.Subscriber(SIGN_SUB_TOPIC,
Detections,
self._sign_cb,
queue_size=10)
rospy.Subscriber(WL_SUB_TOPIC, UInt8, self._wl_cb, queue_size=10)
self.reinit(maze_initial_pose)
#pygame.init()
#self.maze.render_maze()
def reinit(self, maze_initial_pose):
self.maze = Maze(MazeSolver.STRUCTURE, MazeSolver.EDGE_WALLS)
self.maze.set_target(MazePoint(MAZE_TARGET_X, MAZE_TARGET_Y))
self.maze.set_state(
PointDir(maze_initial_pose.x, maze_initial_pose.y, 'L'))
def start_async_processing(self):
self.line_detector.enable()
def stop_async_processing(self):
self.line_detector.disable()
def process(self, maze_crnt_pose):
""" Calcaulate goal_point (in map format) and update path if necessary
- if success: return actual goal point and pub path
- if error: return default goal_point
- if white line: return None that means stop
"""
DEFAULT_GOAL_POINT = MazePoint(MAZE_TARGET_X, MAZE_TARGET_Y)
WHITE_LINE_GOAL_POINT = None
MAP_DIRECTION = 1.57
try:
path = self.maze.get_path()
except:
rospy.logerr("Maze processing error: path can't be empty!")
self.is_recovery_need = True
return DEFAULT_GOAL_POINT, MAP_DIRECTION
local = self.maze.get_local_target()
# Process errors in old path
if maze_crnt_pose is None:
rospy.logerr("Maze processing error: maze crnt pose must exists!")
self.is_recovery_need = True
return DEFAULT_GOAL_POINT, MAP_DIRECTION
elif local is None:
rospy.logerr("Maze processing error: local target must exists!")
self.is_recovery_need = True
return DEFAULT_GOAL_POINT, MAP_DIRECTION
# Process white line and traffic light
self._update_status_of_white_line()
self._update_status_of_crnt_sign()
if self.is_wl_appeared:
if self.tl_color == TLColor.GREEN:
rospy.loginfo("There is WL and TL is green: way is clear.")
self.is_wl_appeared = False
elif self.tl_color == TLColor.UNKNOWN:
rospy.logwarn("There is WL but TL is unknown: is it error?")
self.is_wl_appeared = False
else:
rospy.loginfo_throttle(2, "There is WL and TL is red: wait...")
return WHITE_LINE_GOAL_POINT, MAP_DIRECTION
# Update next local target and pub new path if possible
if local.coord == maze_crnt_pose:
if self.maze.next_local_target() is False:
rospy.logerr(
"Current path length is 0, can't get next tartet!")
self.is_recovery_need = True
return DEFAULT_GOAL_POINT, MAP_DIRECTION
if len(path) == 0:
rospy.logerr("Path is empty!")
self.is_recovery_need = True
return DEFAULT_GOAL_POINT, MAP_DIRECTION
# Process signs
if self.sign_type != SignType.NO_SIGN:
rospy.loginfo("A sign was detected: %s", str(self.sign_type))
self.maze.set_limitation(SIGN_TYPE_TO_MAZE[self.sign_type.value])
try:
path = self.maze.get_path()
except:
rospy.logerr("Maze processing error: path is empty!")
self.is_recovery_need = True
return DEFAULT_GOAL_POINT, MAP_DIRECTION
# Return actual goal point
self._pub_path(path)
goal_point = maze_to_map(path[-1].coord)
return goal_point, MAP_DIRECTION
def _update_status_of_crnt_sign(self):
previous_tl_color = self.tl_color
previous_sign_type = self.sign_type
self.tl_color = TLColor.UNKNOWN
self.sign_type = SignType.NO_SIGN
for detection in self.sign_msg.detections:
sign_type = SIGN_TYPE_FROM_STR.get(detection.object_class)
tl_type = TL_TYPE_FROM_STR.get(detection.object_class)
if sign_type is not None:
self.sign_type = sign_type
if tl_type is not None:
self.tl_color = tl_type
if previous_tl_color != self.tl_color:
rospy.loginfo("A TL status has been changed to %s",
str(self.tl_color))
if previous_sign_type != self.sign_type:
rospy.loginfo("A sign status has been changed to %s",
str(self.sign_type))
def _update_status_of_white_line(self):
previous_wl_status = self.is_wl_appeared
result = self.line_detector.get_lines()
if result is not None:
self.is_wl_appeared = True if len(result) >= 2 else False
if previous_wl_status != self.is_wl_appeared:
rospy.loginfo("A white line status has been changed to %d",
self.wl_msg.data)
def _pub_path(self, nodes):
path = Path()
path.header.seq = 1
path.header.frame_id = FRAME_ID
for i in range(0, len(nodes)):
pose = PoseStamped()
pose.header.seq = 1
pose.header.frame_id = FRAME_ID
point = maze_to_map(nodes[i].coord)
pose.pose.position.x = point.x
pose.pose.position.y = point.y
path.poses.append(pose)
self.PATH_PUB.publish(path)
def _sign_cb(self, msg):
self.sign_msg = msg
def _wl_cb(self, msg):
self.wl_msg = msg
def _get_maze_current_pose(self):
trans = self.pose_listener.lookupTransform(FRAME_ID, 'base_footprint',
rospy.Time(0))
point = map_to_maze(MazePoint(trans[0][0], trans[0][1]))
return point
def map_to_maze(p):
maze_x = round((p.x - 0.5 * CELL_SZ) / CELL_SZ)
maze_y = round((p.y - 0.5 * CELL_SZ) / CELL_SZ)
return MazePoint(maze_x, maze_y)
def maze_to_map(p):
map_x = p.x * CELL_SZ + 0.5 * CELL_SZ
map_y = p.y * CELL_SZ + 0.5 * CELL_SZ
return MazePoint(map_x, map_y)
def _change_tl_cb(self, event):
RoadState.set_tl_type(TLColor.GREEN)
self.gz_pub.publish(RoadState.get_tl_type().value)
self.color = RoadState.get_tl_type()
self._pub_cb()
rospy.init_node("test_solver", log_level=rospy.INFO)
sign_pub = rospy.Publisher(SIGN_SUB_TOPIC, Detections, queue_size=5)
if __name__ == "__main__":
sleep(1)
pose_listener = tf.TransformListener()
sleep(1)
road_objects = list((TL(MazePoint(9, 8), 'traffic_light_0_topic'),
Sign(MazePoint(9, 7), SignType.ONLY_RIGHT),
Sign(MazePoint(7, 7), SignType.ONLY_RIGHT),
Sign(MazePoint(3, 8), SignType.FORWARD_OR_RIGHT),
Sign(MazePoint(3, 5), SignType.BRICK),
Sign(MazePoint(5, 4), SignType.ONLY_FORWARD),
Sign(MazePoint(8, 4), SignType.ONLY_RIGHT),
Sign(MazePoint(9, 3), SignType.FORWARD_OR_LEFT),
TL(MazePoint(9, 1), 'traffic_light_1_topic'),
TL(MazePoint(5, 0), 'traffic_light_2_topic'),
TL(MazePoint(4, 0), 'traffic_light_3_topic'),
TL(MazePoint(5, 2), 'traffic_light_4_topic'),
TL(MazePoint(4, 2), 'traffic_light_5_topic')))
while not rospy.is_shutdown():
# Process road objects