diff --git a/.github/workflows/build_test.yml b/.github/workflows/build_test.yml new file mode 100644 index 0000000..067f1c3 --- /dev/null +++ b/.github/workflows/build_test.yml @@ -0,0 +1,32 @@ +name: Build test + +on: + push: + branches: + - main + pull_request: + branches: + - main + +jobs: + build: + runs-on: ubuntu-22.04 + container: ros:humble-ros-base-jammy + steps: + - uses: actions/checkout@v4 + - name: create a ros2 workspace + run: mkdir -p ros2_ws/src + - name: Off secrets + run: git config --global --add safe.directory '*' + - name: Pull selectes submoules + run: git submodule update --init --recursive external/autoware_msgs/ external/tier4_autoware_msgs/ crp_APL/planners/plan_lat_lane_follow_ldm/src/functionCode/ + - name: copy the package to the workspace + run: cp -r crp_APL crp_CIL/ crp_VIL/ doc/ external/ scripts/ ros2_ws/src + - name: pacmod3 install + run: apt install apt-transport-https && sudo sh -c 'echo "deb [trusted=yes] https://s3.amazonaws.com/autonomoustuff-repo/ $(lsb_release -sc) main" > /etc/apt/sources.list.d/autonomoustuff-public.list' && apt update && apt install -y ros-humble-pacmod3 + - name: install dependencies + run: cd ros2_ws && source /opt/ros/humble/setup.bash && rosdep update && rosdep install --from-paths src --ignore-src -r -y + shell: bash + - name: build the workspace + run: cd ros2_ws && source /opt/ros/humble/setup.bash && colcon build + shell: bash \ No newline at end of file diff --git a/.gitignore b/.gitignore index b1fa845..d81454e 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,5 @@ logs/ build/ -install/ \ No newline at end of file +install/ + +.vscode/ \ No newline at end of file diff --git a/crp_APL/ctrl_vehicle_control/CMakeLists.txt b/crp_APL/ctrl_vehicle_control/CMakeLists.txt index a36b8b1..c283407 100644 --- a/crp_APL/ctrl_vehicle_control/CMakeLists.txt +++ b/crp_APL/ctrl_vehicle_control/CMakeLists.txt @@ -8,6 +8,7 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclpy REQUIRED) find_package(std_msgs REQUIRED) find_package(autoware_control_msgs REQUIRED) find_package(pacmod3_msgs REQUIRED) diff --git a/crp_APL/ctrl_vehicle_control/launch/ctrl_vehicle_control_lqr.launch.py b/crp_APL/ctrl_vehicle_control/launch/ctrl_vehicle_control_lqr.launch.py new file mode 100644 index 0000000..e69de29 diff --git a/crp_APL/ctrl_vehicle_control/scripts/cubic_spline_planner.py b/crp_APL/ctrl_vehicle_control/scripts/cubic_spline_planner.py new file mode 100644 index 0000000..d53a67e --- /dev/null +++ b/crp_APL/ctrl_vehicle_control/scripts/cubic_spline_planner.py @@ -0,0 +1,398 @@ +""" +Cubic spline planner + +Author: Atsushi Sakai(@Atsushi_twi) + +""" +import math +import numpy as np +import bisect + +class CubicSpline1D: + """ + 1D Cubic Spline class + + Parameters + ---------- + x : list + x coordinates for data points. This x coordinates must be + sorted + in ascending order. + y : list + y coordinates for data points + + Examples + -------- + You can interpolate 1D data points. + + >>> import numpy as np + >>> import matplotlib.pyplot as plt + >>> x = np.arange(5) + >>> y = [1.7, -6, 5, 6.5, 0.0] + >>> sp = CubicSpline1D(x, y) + >>> xi = np.linspace(0.0, 5.0) + >>> yi = [sp.calc_position(x) for x in xi] + >>> plt.plot(x, y, "xb", label="Data points") + >>> plt.plot(xi, yi , "r", label="Cubic spline interpolation") + >>> plt.grid(True) + >>> plt.legend() + >>> plt.show() + + .. image:: cubic_spline_1d.png + + """ + + def __init__(self, x, y): + + h = np.diff(x) + if np.any(h < 0): + raise ValueError("x coordinates must be sorted in ascending order") + + self.a, self.b, self.c, self.d = [], [], [], [] + self.x = x + self.y = y + self.nx = len(x) # dimension of x + + # calc coefficient a + self.a = [iy for iy in y] + + # calc coefficient c + A = self.__calc_A(h) + B = self.__calc_B(h, self.a) + self.c = np.linalg.solve(A, B) + + # calc spline coefficient b and d + for i in range(self.nx - 1): + d = (self.c[i + 1] - self.c[i]) / (3.0 * h[i]) + b = 1.0 / h[i] * (self.a[i + 1] - self.a[i]) \ + - h[i] / 3.0 * (2.0 * self.c[i] + self.c[i + 1]) + self.d.append(d) + self.b.append(b) + + + + def calc_position(self, x): + """ + Calc `y` position for given `x`. + + if `x` is outside the data point's `x` range, return None. + + Returns + ------- + y : float + y position for given x. + """ + if x < self.x[0]: + return None + elif x > self.x[-1]: + return None + + i = self.__search_index(x) + dx = x - self.x[i] + position = self.a[i] + self.b[i] * dx + \ + self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0 + + return position + + + def calc_first_derivative(self, x): + """ + Calc first derivative at given x. + + if x is outside the input x, return None + + Returns + ------- + dy : float + first derivative for given x. + """ + + if x < self.x[0]: + return None + elif x > self.x[-1]: + return None + + i = self.__search_index(x) + dx = x - self.x[i] + dy = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0 + return dy + + + def calc_second_derivative(self, x): + """ + Calc second derivative at given x. + + if x is outside the input x, return None + + Returns + ------- + ddy : float + second derivative for given x. + """ + + if x < self.x[0]: + return None + elif x > self.x[-1]: + return None + + i = self.__search_index(x) + dx = x - self.x[i] + ddy = 2.0 * self.c[i] + 6.0 * self.d[i] * dx + return ddy + + + def __search_index(self, x): + """ + search data segment index + """ + return bisect.bisect(self.x, x) - 1 + + + def __calc_A(self, h): + """ + calc matrix A for spline coefficient c + """ + A = np.zeros((self.nx, self.nx)) + A[0, 0] = 1.0 + for i in range(self.nx - 1): + if i != (self.nx - 2): + A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1]) + A[i + 1, i] = h[i] + A[i, i + 1] = h[i] + + A[0, 1] = 0.0 + A[self.nx - 1, self.nx - 2] = 0.0 + A[self.nx - 1, self.nx - 1] = 1.0 + return A + + + def __calc_B(self, h, a): + """ + calc matrix B for spline coefficient c + """ + B = np.zeros(self.nx) + for i in range(self.nx - 2): + B[i + 1] = 3.0 * (a[i + 2] - a[i + 1]) / h[i + 1]\ + - 3.0 * (a[i + 1] - a[i]) / h[i] + return B + + +class CubicSpline2D: + """ + Cubic CubicSpline2D class + + Parameters + ---------- + x : list + x coordinates for data points. + y : list + y coordinates for data points. + + Examples + -------- + You can interpolate a 2D data points. + + >>> import matplotlib.pyplot as plt + >>> x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0] + >>> y = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0] + >>> ds = 0.1 # [m] distance of each interpolated points + >>> sp = CubicSpline2D(x, y) + >>> s = np.arange(0, sp.s[-1], ds) + >>> rx, ry, ryaw, rk = [], [], [], [] + >>> for i_s in s: + ... ix, iy = sp.calc_position(i_s) + ... rx.append(ix) + ... ry.append(iy) + ... ryaw.append(sp.calc_yaw(i_s)) + ... rk.append(sp.calc_curvature(i_s)) + >>> plt.subplots(1) + >>> plt.plot(x, y, "xb", label="Data points") + >>> plt.plot(rx, ry, "-r", label="Cubic spline path") + >>> plt.grid(True) + >>> plt.axis("equal") + >>> plt.xlabel("x[m]") + >>> plt.ylabel("y[m]") + >>> plt.legend() + >>> plt.show() + + .. image:: cubic_spline_2d_path.png + + >>> plt.subplots(1) + >>> plt.plot(s, [np.rad2deg(iyaw) for iyaw in ryaw], "-r", label="yaw") + >>> plt.grid(True) + >>> plt.legend() + >>> plt.xlabel("line length[m]") + >>> plt.ylabel("yaw angle[deg]") + + .. image:: cubic_spline_2d_yaw.png + + >>> plt.subplots(1) + >>> plt.plot(s, rk, "-r", label="curvature") + >>> plt.grid(True) + >>> plt.legend() + >>> plt.xlabel("line length[m]") + >>> plt.ylabel("curvature [1/m]") + + .. image:: cubic_spline_2d_curvature.png + """ + + def __init__(self, x, y): + self.s = self.__calc_s(x, y) + self.sx = CubicSpline1D(self.s, x) + self.sy = CubicSpline1D(self.s, y) + + + def __calc_s(self, x, y): + dx = np.diff(x) + dy = np.diff(y) + self.ds = np.hypot(dx, dy) + s = [0] + s.extend(np.cumsum(self.ds)) + return s + + + def calc_position(self, s): + """ + calc position + + Parameters + ---------- + s : float + distance from the start point. if `s` is outside the data point's + range, return None. + + Returns + ------- + x : float + x position for given s. + y : float + y position for given s. + """ + x = self.sx.calc_position(s) + y = self.sy.calc_position(s) + + return x, y + + + def calc_curvature(self, s): + """ + calc curvature + + Parameters + ---------- + s : float + distance from the start point. if `s` is outside the data point's + range, return None. + + Returns + ------- + k : float + curvature for given s. + """ + dx = self.sx.calc_first_derivative(s) + ddx = self.sx.calc_second_derivative(s) + dy = self.sy.calc_first_derivative(s) + ddy = self.sy.calc_second_derivative(s) + k = (ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2)) + return k + + + def calc_yaw(self, s): + """ + calc yaw + + Parameters + ---------- + s : float + distance from the start point. if `s` is outside the data point's + range, return None. + + Returns + ------- + yaw : float + yaw angle (tangent vector) for given s. + """ + dx = self.sx.calc_first_derivative(s) + dy = self.sy.calc_first_derivative(s) + yaw = math.atan2(dy, dx) + return yaw + + +def calc_spline_course(x, y, ds=0.1): + sp = CubicSpline2D(x, y) + s = list(np.arange(0, sp.s[-1], ds)) + + rx, ry, ryaw, rk = [], [], [], [] + for i_s in s: + ix, iy = sp.calc_position(i_s) + rx.append(ix) + ry.append(iy) + ryaw.append(sp.calc_yaw(i_s)) + rk.append(sp.calc_curvature(i_s)) + + return rx, ry, ryaw, rk, s + + +def main_1d(): + print("CubicSpline1D test") + import matplotlib.pyplot as plt + x = np.arange(5) + y = [1.7, -6, 5, 6.5, 0.0] + sp = CubicSpline1D(x, y) + xi = np.linspace(0.0, 5.0) + + plt.plot(x, y, "xb", label="Data points") + plt.plot(xi, [sp.calc_position(x) for x in xi], "r", + label="Cubic spline interpolation") + plt.grid(True) + plt.legend() + plt.show() + + +def main_2d(): # pragma: no cover + print("CubicSpline1D 2D test") + import matplotlib.pyplot as plt + x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0] + y = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0] + ds = 0.1 # [m] distance of each interpolated points + + sp = CubicSpline2D(x, y) + s = np.arange(0, sp.s[-1], ds) + + rx, ry, ryaw, rk = [], [], [], [] + for i_s in s: + ix, iy = sp.calc_position(i_s) + rx.append(ix) + ry.append(iy) + ryaw.append(sp.calc_yaw(i_s)) + rk.append(sp.calc_curvature(i_s)) + + plt.subplots(1) + plt.plot(x, y, "xb", label="Data points") + plt.plot(rx, ry, "-r", label="Cubic spline path") + plt.grid(True) + plt.axis("equal") + plt.xlabel("x[m]") + plt.ylabel("y[m]") + plt.legend() + + plt.subplots(1) + plt.plot(s, [np.rad2deg(iyaw) for iyaw in ryaw], "-r", label="yaw") + plt.grid(True) + plt.legend() + plt.xlabel("line length[m]") + plt.ylabel("yaw angle[deg]") + + plt.subplots(1) + plt.plot(s, rk, "-r", label="curvature") + plt.grid(True) + plt.legend() + plt.xlabel("line length[m]") + plt.ylabel("curvature [1/m]") + + plt.show() + + +if __name__ == '__main__': + # main_1d() + main_2d() \ No newline at end of file diff --git a/crp_APL/ctrl_vehicle_control/scripts/lqr_controller.py b/crp_APL/ctrl_vehicle_control/scripts/lqr_controller.py new file mode 100644 index 0000000..8a580cc --- /dev/null +++ b/crp_APL/ctrl_vehicle_control/scripts/lqr_controller.py @@ -0,0 +1,282 @@ +""" + +Path tracking simulation with LQR speed and steering control + +author Atsushi Sakai (@Atsushi_twi) + +""" +import math +import sys +import matplotlib.pyplot as plt +import numpy as np +import scipy.linalg as la +import cubic_spline_planner +import time +import rclpy +from rclpy.node import Node +from autoware_control_msgs.msg import Control +from autoware_planning_msgs.msg import Trajectory,TrajectoryPoint +from crp_msgs.msg import Ego + +# === Parameters ===== + +# LQR parameter +lqr_Q = np.eye(5) +lqr_R = np.eye(2) +dt = 0.1 # time tick[s] +L = 2.9 # Wheel base of the vehicle [m] +max_steer = np.deg2rad(20.0) # maximum steering angle[rad] + +lqr_Q[0, 0] = 0.01 +lqr_Q[1, 1] = 0.001 +lqr_Q[2, 2] = 0.01 +lqr_Q[3, 3] = 0.001 +lqr_Q[4, 4] = 0.6 + +lqr_R[0, 0] = 30.0 +lqr_R[1, 1] = 30.0 + +show_animation = True + +def angle_mod(x, zero_2_2pi=False, degree=False): + + + if isinstance(x, float): + is_float = True + else: + is_float = False + + x = np.asarray(x).flatten() + if degree: + x = np.deg2rad(x) + + if zero_2_2pi: + mod_angle = x % (2 * np.pi) + else: + mod_angle = (x + np.pi) % (2 * np.pi) - np.pi + + if degree: + mod_angle = np.rad2deg(mod_angle) + + if is_float: + return mod_angle.item() + else: + return mod_angle + +class State: + + def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0,steer=0.0): + self.x = x + self.y = y + self.yaw = yaw + self.v = v + self.steer = steer + +def pi_2_pi(angle): + return angle_mod(angle) + +# create an ROS controller class +class ROSController(Node): + + def __init__(self): + + super().__init__('ROS_LQR_Controller') + + self.cx = [] + self.cy = [] + self.cyaw = [] + self.ck = [] + self.sp = [] + + self.pe = 0.0 + self.pth_e = 0.0 + + self.state = State(x=0.0, y=0.0, yaw=0.0, v=0.0, steer=0.0) + + self.ctrl_publisher = self.create_publisher(Control, '/control/command/contol_cmd', 10) + self.traj_subscriber = self.create_subscription(Trajectory, '/plan/trajetory', self.recive_trajectory, 10) + self.ego_subscriber = self.create_subscription(Ego, '/ego', self.recive_ego, 10) + + self.control_clock = self.create_timer(0.05, self.control_loop) + + + def recive_trajectory(self, msg): + + for i in range(len(msg.points)): + self.cx.append(msg.points[i].pose.position.x) + self.cy.append(msg.points[i].pose.position.y) + self.cyaw.append(msg.points[i].pose.orientation.z) + self.sp.append(msg.points[i].longitudinal_velocity_mps) + self.ck.append(msg.points[i].heading_rate_rps) + + + def recive_ego(self, msg): + self.state.x = msg.pose.pose.position.x + self.state.y = msg.pose.pose.position.y + self.state.yaw = msg.pose.pose.orientation.z + self.state.v = msg.twist.twist.linear.x + self.state.steer = msg.tire_angle_front + + def solve_dare(self, A, B, Q, R): + """ + solve a discrete time_Algebraic Riccati equation (DARE) + """ + x = Q + x_next = Q + max_iter = 150 + eps = 0.01 + + for i in range(max_iter): + x_next = A.T @ x @ A - A.T @ x @ B @ \ + la.inv(R + B.T @ x @ B) @ B.T @ x @ A + Q + if (abs(x_next - x)).max() < eps: + break + x = x_next + + return x_next + + + def dlqr(self, A, B, Q, R): + """Solve the discrete time lqr controller. + x[k+1] = A x[k] + B u[k] + cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k] + # ref Bertsekas, p.151 + """ + + # first, try to solve the ricatti equation + X = self.solve_dare(A, B, Q, R) + + # compute the LQR gain + K = la.inv(B.T @ X @ B + R) @ (B.T @ X @ A) + + eig_result = la.eig(A - B @ K) + + return K, X, eig_result[0] + + + def control_loop(self): + + Control_msg = Control() + Control_msg.stamp = self.get_clock().now().to_msg() + Control_msg.longitudinal.acceleration = 0.0 + Control_msg.lateral.steering_tire_angle = 0.0 + + if len(self.cx) <= 0: + self.ctrl_publisher.publish(Control_msg) + return + + + ind, e = self.calc_nearest_index(self.state, self.cx, self.cy, self.cyaw) + + tv = self.sp[ind] + + k = self.ck[ind] + v = self.state.v + th_e = pi_2_pi(self.state.yaw - self.cyaw[ind]) + + # A = [1.0, dt, 0.0, 0.0, 0.0 + # 0.0, 0.0, v, 0.0, 0.0] + # 0.0, 0.0, 1.0, dt, 0.0] + # 0.0, 0.0, 0.0, 0.0, 0.0] + # 0.0, 0.0, 0.0, 0.0, 1.0] + A = np.zeros((5, 5)) + A[0, 0] = 1.0 + A[0, 1] = dt + A[1, 2] = v + A[2, 2] = 1.0 + A[2, 3] = dt + A[4, 4] = 1.0 + + # B = [0.0, 0.0 + # 0.0, 0.0 + # 0.0, 0.0 + # v/L, 0.0 + # 0.0, dt] + B = np.zeros((5, 2)) + B[3, 0] = v / L + B[4, 1] = dt + + K, _, _ = self.dlqr(A, B, lqr_Q, lqr_R) + + # state vector + # x = [e, dot_e, th_e, dot_th_e, delta_v] + # e: lateral distance to the path + # dot_e: derivative of e + # th_e: angle difference to the path + # dot_th_e: derivative of th_e + # delta_v: difference between current speed and target speed + x = np.zeros((5, 1)) + x[0, 0] = e + x[1, 0] = (e - self.pe) / dt + x[2, 0] = th_e + x[3, 0] = (th_e - self.pth_e) / dt + x[4, 0] = v - tv + + # input vector + # u = [delta, accel] + # delta: steering angle + # accel: acceleration + ustar = -K @ x + + # calc steering input + ff = math.atan2(L * k, 1) # feedforward steering angle + fb = pi_2_pi(ustar[0, 0]) # feedback steering angle + delta = ff + fb + + # calc accel input + accel = ustar[1, 0] + + + Control_msg.longitudinal.acceleration = accel + Control_msg.lateral.steering_tire_angle = delta + + print("Control command:" + str(accel) + " " + str(delta)) + + self.ctrl_publisher.publish(Control_msg) + + + def calc_nearest_index(self, state, cx, cy, cyaw): + dx = [state.x - icx for icx in cx] + dy = [state.y - icy for icy in cy] + + d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)] + + mind = min(d) + + ind = d.index(mind) + + mind = math.sqrt(mind) + + dxl = cx[ind] - state.x + dyl = cy[ind] - state.y + + angle = pi_2_pi(cyaw[ind] - math.atan2(dyl, dxl)) + if angle < 0: + mind *= -1 + + # Project RMS error onto vehicle + vehicle_vector = [-np.cos(state.yaw + np.pi / 2), + -np.sin(state.yaw + np.pi / 2)] + lateral_error_from_vehicle = np.dot([dx[ind], dy[ind]], vehicle_vector) + + return ind, -lateral_error_from_vehicle + + + +def main(args=None): + rclpy.init(args=args) + + node = ROSController() + + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + + # Clean up and shutdown + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/crp_VIL/lanelet_handler/CMakeLists.txt b/crp_VIL/lanelet_handler/CMakeLists.txt index 39337ab..19e1609 100644 --- a/crp_VIL/lanelet_handler/CMakeLists.txt +++ b/crp_VIL/lanelet_handler/CMakeLists.txt @@ -9,18 +9,21 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(autoware_map_msgs REQUIRED) -find_package(map_loader REQUIRED) find_package(lanelet2_core REQUIRED) find_package(lanelet2_projection REQUIRED) find_package(lanelet2_io REQUIRED) find_package(autoware_lanelet2_extension REQUIRED) +find_package(visualization_msgs REQUIRED) + include_directories( include ) add_executable(lanelet_file_loader src/laneletFileLoader.cpp) -ament_target_dependencies(lanelet_file_loader rclcpp autoware_map_msgs lanelet2_core lanelet2_projection lanelet2_io autoware_lanelet2_extension) +ament_target_dependencies(lanelet_file_loader rclcpp autoware_map_msgs lanelet2_core autoware_lanelet2_extension lanelet2_projection lanelet2_io) + + install(TARGETS lanelet_file_loader diff --git a/crp_VIL/lanelet_handler/launch/laneletFileLoader.launch.py b/crp_VIL/lanelet_handler/launch/laneletFileLoader.launch.py index fd808fe..1484822 100644 --- a/crp_VIL/lanelet_handler/launch/laneletFileLoader.launch.py +++ b/crp_VIL/lanelet_handler/launch/laneletFileLoader.launch.py @@ -7,7 +7,7 @@ def generate_launch_description(): lanelet2_path_arg = DeclareLaunchArgument( 'map_file_path', - default_value="", + default_value="/home/matyko/lanelet2_maps/ZalaZone/ZalaZone_Uni_track_full_early.osm", description='Path to the lanelet2 map file' ) diff --git a/crp_VIL/map_loader/CMakeLists.txt b/crp_VIL/map_loader/CMakeLists.txt new file mode 100644 index 0000000..c6b1ead --- /dev/null +++ b/crp_VIL/map_loader/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.14) +project(map_loader) + +find_package(autoware_cmake REQUIRED) +find_package(tier4_map_msgs REQUIRED) +autoware_package() + + +ament_auto_add_library(lanelet2_map_visualization_node SHARED + src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp +) + +rclcpp_components_register_node(lanelet2_map_visualization_node + PLUGIN "Lanelet2MapVisualizationNode" + EXECUTABLE lanelet2_map_visualization +) + + +install(PROGRAMS + script/map_hash_generator + DESTINATION lib/${PROJECT_NAME} +) + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/crp_VIL/map_loader/README.md b/crp_VIL/map_loader/README.md new file mode 100644 index 0000000..bc3eb80 --- /dev/null +++ b/crp_VIL/map_loader/README.md @@ -0,0 +1,174 @@ +# map_loader package + +This package provides the features of loading various maps. + +## pointcloud_map_loader + +### Feature + +`pointcloud_map_loader` provides pointcloud maps to the other Autoware nodes in various configurations. +Currently, it supports the following two types: + +- Publish raw pointcloud map +- Publish downsampled pointcloud map +- Send partial pointcloud map loading via ROS 2 service +- Send differential pointcloud map loading via ROS 2 service + +NOTE: **We strongly recommend to use divided maps when using large pointcloud map to enable the latter two features (partial and differential load). Please go through the prerequisites section for more details, and follow the instruction for dividing the map and preparing the metadata.** + +### Prerequisites + +#### Prerequisites on pointcloud map file(s) + +You may provide either a single .pcd file or multiple .pcd files. If you are using multiple PCD data, it MUST obey the following rules: + +1. **The pointcloud map should be projected on the same coordinate defined in `map_projection_loader`**, in order to be consistent with the lanelet2 map and other packages that converts between local and geodetic coordinates. For more information, please refer to [the readme of `map_projection_loader`](https://github.com/autowarefoundation/autoware.universe/tree/main/map/autoware_map_projection_loader/README.md). +2. **It must be divided by straight lines parallel to the x-axis and y-axis**. The system does not support division by diagonal lines or curved lines. +3. **The division size along each axis should be equal.** +4. **The division size should be about 20m x 20m.** Particularly, care should be taken as using too large division size (for example, more than 100m) may have adverse effects on dynamic map loading features in [ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/autoware_ndt_scan_matcher) and [autoware_compare_map_segmentation](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/autoware_compare_map_segmentation). +5. **All the split maps should not overlap with each other.** +6. **Metadata file should also be provided.** The metadata structure description is provided below. + +#### Metadata structure + +The metadata should look like this: + +```yaml +x_resolution: 20.0 +y_resolution: 20.0 +A.pcd: [1200, 2500] # -> 1200 < x < 1220, 2500 < y < 2520 +B.pcd: [1220, 2500] # -> 1220 < x < 1240, 2500 < y < 2520 +C.pcd: [1200, 2520] # -> 1200 < x < 1220, 2520 < y < 2540 +D.pcd: [1240, 2520] # -> 1240 < x < 1260, 2520 < y < 2540 +``` + +where, + +- `x_resolution` and `y_resolution` +- `A.pcd`, `B.pcd`, etc, are the names of PCD files. +- List such as `[1200, 2500]` are the values indicate that for this PCD file, x coordinates are between 1200 and 1220 (`x_resolution` + `x_coordinate`) and y coordinates are between 2500 and 2520 (`y_resolution` + `y_coordinate`). + +You may use [pointcloud_divider](https://github.com/autowarefoundation/autoware_tools/tree/main/map/autoware_pointcloud_divider) for dividing pointcloud map as well as generating the compatible metadata.yaml. + +#### Directory structure of these files + +If you only have one pointcloud map, Autoware will assume the following directory structure by default. + +```bash +sample-map-rosbag +├── lanelet2_map.osm +├── pointcloud_map.pcd +``` + +If you have multiple rosbags, an example directory structure would be as follows. Note that you need to have a metadata when you have multiple pointcloud map files. + +```bash +sample-map-rosbag +├── lanelet2_map.osm +├── pointcloud_map.pcd +│ ├── A.pcd +│ ├── B.pcd +│ ├── C.pcd +│ └── ... +├── map_projector_info.yaml +└── pointcloud_map_metadata.yaml +``` + +### Specific features + +#### Publish raw pointcloud map (ROS 2 topic) + +The node publishes the raw pointcloud map loaded from the `.pcd` file(s). + +#### Publish downsampled pointcloud map (ROS 2 topic) + +The node publishes the downsampled pointcloud map loaded from the `.pcd` file(s). You can specify the downsample resolution by changing the `leaf_size` parameter. + +#### Publish metadata of pointcloud map (ROS 2 topic) + +The node publishes the pointcloud metadata attached with an ID. Metadata is loaded from the `.yaml` file. Please see [the description of `PointCloudMapMetaData.msg`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs#pointcloudmapmetadatamsg) for details. + +#### Send partial pointcloud map (ROS 2 service) + +Here, we assume that the pointcloud maps are divided into grids. + +Given a query from a client node, the node sends a set of pointcloud maps that overlaps with the queried area. +Please see [the description of `GetPartialPointCloudMap.srv`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs#getpartialpointcloudmapsrv) for details. + +#### Send differential pointcloud map (ROS 2 service) + +Here, we assume that the pointcloud maps are divided into grids. + +Given a query and set of map IDs, the node sends a set of pointcloud maps that overlap with the queried area and are not included in the set of map IDs. +Please see [the description of `GetDifferentialPointCloudMap.srv`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs#getdifferentialpointcloudmapsrv) for details. + +#### Send selected pointcloud map (ROS 2 service) + +Here, we assume that the pointcloud maps are divided into grids. + +Given IDs query from a client node, the node sends a set of pointcloud maps (each of which attached with unique ID) specified by query. +Please see [the description of `GetSelectedPointCloudMap.srv`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs#getselectedpointcloudmapsrv) for details. + +### Parameters + +{{ json_to_markdown("map/map_loader/schema/pointcloud_map_loader.schema.json") }} + +### Interfaces + +- `output/pointcloud_map` (sensor_msgs/msg/PointCloud2) : Raw pointcloud map +- `output/pointcloud_map_metadata` (autoware_map_msgs/msg/PointCloudMapMetaData) : Metadata of pointcloud map +- `output/debug/downsampled_pointcloud_map` (sensor_msgs/msg/PointCloud2) : Downsampled pointcloud map +- `service/get_partial_pcd_map` (autoware_map_msgs/srv/GetPartialPointCloudMap) : Partial pointcloud map +- `service/get_differential_pcd_map` (autoware_map_msgs/srv/GetDifferentialPointCloudMap) : Differential pointcloud map +- `service/get_selected_pcd_map` (autoware_map_msgs/srv/GetSelectedPointCloudMap) : Selected pointcloud map +- pointcloud map file(s) (.pcd) +- metadata of pointcloud map(s) (.yaml) + +--- + +## lanelet2_map_loader + +### Feature + +lanelet2_map_loader loads Lanelet2 file and publishes the map data as autoware_map_msgs/LaneletMapBin message. +The node projects lan/lon coordinates into arbitrary coordinates defined in `/map/map_projector_info` from `map_projection_loader`. +Please see [tier4_autoware_msgs/msg/MapProjectorInfo.msg](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_map_msgs/msg/MapProjectorInfo.msg) for supported projector types. + +### How to run + +`ros2 run map_loader lanelet2_map_loader --ros-args -p lanelet2_map_path:=path/to/map.osm` + +### Subscribed Topics + +- ~input/map_projector_info (tier4_map_msgs/MapProjectorInfo) : Projection type for Autoware + +### Published Topics + +- ~output/lanelet2_map (autoware_map_msgs/LaneletMapBin) : Binary data of loaded Lanelet2 Map + +### Parameters + +{{ json_to_markdown("map/map_loader/schema/lanelet2_map_loader.schema.json") }} + +`use_waypoints` decides how to handle a centerline. +This flag enables to use the `overwriteLaneletsCenterlineWithWaypoints` function instead of `overwriteLaneletsCenterline`. Please see [the document of the autoware_lanelet2_extension package](https://github.com/autowarefoundation/autoware_lanelet2_extension/blob/main/autoware_lanelet2_extension/docs/lanelet2_format_extension.md#centerline) in detail. + +--- + +## lanelet2_map_visualization + +### Feature + +lanelet2_map_visualization visualizes autoware_map_msgs/LaneletMapBin messages into visualization_msgs/MarkerArray. + +### How to Run + +`ros2 run map_loader lanelet2_map_visualization` + +### Subscribed Topics + +- ~input/lanelet2_map (autoware_map_msgs/LaneletMapBin) : binary data of Lanelet2 Map + +### Published Topics + +- ~output/lanelet2_map_marker (visualization_msgs/MarkerArray) : visualization messages for RViz diff --git a/crp_VIL/map_loader/config/lanelet2_map_loader.param.yaml b/crp_VIL/map_loader/config/lanelet2_map_loader.param.yaml new file mode 100755 index 0000000..48d392a --- /dev/null +++ b/crp_VIL/map_loader/config/lanelet2_map_loader.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + allow_unsupported_version: true # flag to load unsupported format_version anyway. If true, just prints warning. + center_line_resolution: 5.0 # [m] + use_waypoints: true # "centerline" in the Lanelet2 map will be used as a "waypoints" tag. + lanelet2_map_path: $(var lanelet2_map_path) # The lanelet2 map path diff --git a/crp_VIL/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp b/crp_VIL/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp new file mode 100644 index 0000000..049d714 --- /dev/null +++ b/crp_VIL/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp @@ -0,0 +1,40 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MAP_LOADER__LANELET2_MAP_VISUALIZATION_NODE_HPP_ +#define MAP_LOADER__LANELET2_MAP_VISUALIZATION_NODE_HPP_ + +#include + +#include +#include + +#include +#include + +class Lanelet2MapVisualizationNode : public rclcpp::Node +{ +public: + explicit Lanelet2MapVisualizationNode(const rclcpp::NodeOptions & options); + +private: + rclcpp::Subscription::SharedPtr sub_map_bin_; + rclcpp::Publisher::SharedPtr pub_marker_; + + bool viz_lanelets_centerline_; + + void on_map_bin(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); +}; + +#endif // MAP_LOADER__LANELET2_MAP_VISUALIZATION_NODE_HPP_ diff --git a/crp_VIL/map_loader/launch/lanelet2_map_loader.launch.xml b/crp_VIL/map_loader/launch/lanelet2_map_loader.launch.xml new file mode 100644 index 0000000..e71906b --- /dev/null +++ b/crp_VIL/map_loader/launch/lanelet2_map_loader.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/crp_VIL/map_loader/package.xml b/crp_VIL/map_loader/package.xml new file mode 100644 index 0000000..b86183c --- /dev/null +++ b/crp_VIL/map_loader/package.xml @@ -0,0 +1,42 @@ + + + + map_loader + 0.1.0 + The map_loader package + Yamato Ando + Ryu Yamamoto + Masahiro Sakamoto + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + + Apache License 2.0 + Ryohsuke Mitsudome + Koji Minoda + + ament_cmake_auto + autoware_cmake + + autoware_lanelet2_extension + autoware_map_msgs + fmt + geometry_msgs + libpcl-all-dev + pcl_conversions + rclcpp + rclcpp_components + tier4_map_msgs + visualization_msgs + yaml-cpp + + ament_cmake_gmock + ament_lint_auto + autoware_lint_common + ros_testing + + + ament_cmake + + diff --git a/crp_VIL/map_loader/schema/lanelet2_map_loader.schema.json b/crp_VIL/map_loader/schema/lanelet2_map_loader.schema.json new file mode 100644 index 0000000..a55050e --- /dev/null +++ b/crp_VIL/map_loader/schema/lanelet2_map_loader.schema.json @@ -0,0 +1,48 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for lanelet2 map loader Node", + "type": "object", + "definitions": { + "lanelet2_map_loader": { + "type": "object", + "properties": { + "allow_unsupported_version": { + "type": "boolean", + "description": "Flag to load unsupported format_version anyway. If true, just prints warning.", + "default": "true" + }, + "center_line_resolution": { + "type": "number", + "description": "Resolution of the Lanelet center line [m]", + "default": "5.0" + }, + "use_waypoints": { + "type": "boolean", + "description": "If true, `centerline` in the Lanelet2 map will be used as a `waypoints` tag.", + "default": true + }, + "lanelet2_map_path": { + "type": "string", + "description": "The lanelet2 map path pointing to the .osm file", + "default": "" + } + }, + "required": ["center_line_resolution", "use_waypoints", "lanelet2_map_path"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/lanelet2_map_loader" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/crp_VIL/map_loader/script/map_hash_generator b/crp_VIL/map_loader/script/map_hash_generator new file mode 100755 index 0000000..5fb5755 --- /dev/null +++ b/crp_VIL/map_loader/script/map_hash_generator @@ -0,0 +1,111 @@ +#!/usr/bin/env python3 + +# Copyright 2021 Tier IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import hashlib +import pathlib + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSProfile +from tier4_external_api_msgs.msg import MapHash +from tier4_external_api_msgs.msg import ResponseStatus +from tier4_external_api_msgs.srv import GetTextFile + + +class MapHashGenerator(Node): + def __init__(self): + super().__init__("map_hash_generator") + self.lanelet_path = self.declare_parameter("lanelet2_map_path", "").value + self.lanelet_text = self.load_lanelet_text(self.lanelet_path) + self.lanelet_hash = self.generate_lanelet_file_hash(self.lanelet_text) + + self.pcd_map_path = self.declare_parameter("pointcloud_map_path", "").value + self.pcd_map_hash = self.generate_pcd_file_hash(self.pcd_map_path) + + qos_profile = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) + msg = MapHash() + msg.lanelet = self.lanelet_hash + msg.pcd = self.pcd_map_hash + self.pub = self.create_publisher(MapHash, "/api/autoware/get/map/info/hash", qos_profile) + self.pub.publish(msg) + + self.srv = self.create_service( + GetTextFile, "/api/autoware/get/map/lanelet/xml", self.on_get_lanelet_xml + ) # noqa: E501 + + def on_get_lanelet_xml(self, request, response): + response.status.code = ResponseStatus.SUCCESS + response.file.text = self.lanelet_text + return response + + @staticmethod + def load_lanelet_text(path): + path = pathlib.Path(path) + return path.read_text() if path.is_file() else "" + + @staticmethod + def generate_lanelet_file_hash(data): + return hashlib.sha256(data.encode()).hexdigest() if data else "" + + def update_hash(self, m, path): + try: + with open(path, "rb") as pcd_file: + m.update(pcd_file.read()) + except FileNotFoundError as e: + self.get_logger().error(e) + return False + return True + + def generate_pcd_file_hash(self, path): + path = pathlib.Path(path) + if path.is_file(): + if not path.suffix == ".pcd": + self.get_logger().error(f"[{path}] is not pcd file") + return "" + m = hashlib.sha256() + if not self.update_hash(m, path): + return "" + return m.hexdigest() + + if path.is_dir(): + m = hashlib.sha256() + for pcd_file_path in sorted(path.iterdir()): + if not pcd_file_path.suffix == ".pcd": + continue + if not self.update_hash(m, pcd_file_path): + return "" + if m.hexdigest() == hashlib.sha256().hexdigest(): + self.get_logger().error(f"there are no pcd files in [{path}]") + return "" + return m.hexdigest() + + self.get_logger().error(f"[{path}] is neither file nor directory") + return "" + + +def main(args=None): + rclpy.init(args=args) + node = MapHashGenerator() + rclpy.spin(node) + rclpy.shutdown() + + +if __name__ == "__main__": + try: + main() + except KeyboardInterrupt: + pass diff --git a/crp_VIL/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/crp_VIL/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp new file mode 100644 index 0000000..60e34f8 --- /dev/null +++ b/crp_VIL/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp @@ -0,0 +1,314 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Copyright 2019 Autoware Foundation. All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Authors: Simon Thompson, Ryohsuke Mitsudome + * + */ + +#include "map_loader/lanelet2_map_visualization_node.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +namespace +{ +void insert_marker_array( + visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2) +{ + a1->markers.insert(a1->markers.end(), a2.markers.begin(), a2.markers.end()); +} + +void set_color(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, double a) +{ + cl->r = static_cast(r); + cl->g = static_cast(g); + cl->b = static_cast(b); + cl->a = static_cast(a); +} +} // namespace + +Lanelet2MapVisualizationNode::Lanelet2MapVisualizationNode(const rclcpp::NodeOptions & options) +: Node("lanelet2_map_visualization", options) +{ + using std::placeholders::_1; + + viz_lanelets_centerline_ = true; + + sub_map_bin_ = this->create_subscription( + "input/lanelet2_map", rclcpp::QoS{1}.transient_local(), + std::bind(&Lanelet2MapVisualizationNode::on_map_bin, this, _1)); + + pub_marker_ = this->create_publisher( + "output/lanelet2_map_marker", rclcpp::QoS{1}.transient_local()); +} + +void Lanelet2MapVisualizationNode::on_map_bin( + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) +{ + lanelet::LaneletMapPtr viz_lanelet_map(new lanelet::LaneletMap); + + lanelet::utils::conversion::fromBinMsg(*msg, viz_lanelet_map); + RCLCPP_INFO(this->get_logger(), "Map is loaded\n"); + + // get lanelets etc to visualize + lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(viz_lanelet_map); + lanelet::ConstLanelets road_lanelets = lanelet::utils::query::roadLanelets(all_lanelets); + lanelet::ConstLanelets shoulder_lanelets = lanelet::utils::query::shoulderLanelets(all_lanelets); + lanelet::ConstLanelets crosswalk_lanelets = + lanelet::utils::query::crosswalkLanelets(all_lanelets); + lanelet::ConstLanelets bicycle_lane_lanelets; + lanelet::ConstLineStrings3d partitions = lanelet::utils::query::getAllPartitions(viz_lanelet_map); + lanelet::ConstLineStrings3d pedestrian_polygon_markings = + lanelet::utils::query::getAllPedestrianPolygonMarkings(viz_lanelet_map); + lanelet::ConstLineStrings3d pedestrian_line_markings = + lanelet::utils::query::getAllPedestrianLineMarkings(viz_lanelet_map); + lanelet::ConstLanelets walkway_lanelets = lanelet::utils::query::walkwayLanelets(all_lanelets); + std::vector stop_lines = + lanelet::utils::query::stopLinesLanelets(road_lanelets); + std::vector aw_tl_reg_elems = + lanelet::utils::query::autowareTrafficLights(all_lanelets); + std::vector da_reg_elems = + lanelet::utils::query::detectionAreas(all_lanelets); + std::vector no_reg_elems = + lanelet::utils::query::noStoppingAreas(all_lanelets); + std::vector sb_reg_elems = + lanelet::utils::query::speedBumps(all_lanelets); + std::vector cw_reg_elems = + lanelet::utils::query::crosswalks(all_lanelets); + lanelet::ConstLineStrings3d parking_spaces = + lanelet::utils::query::getAllParkingSpaces(viz_lanelet_map); + lanelet::ConstPolygons3d parking_lots = lanelet::utils::query::getAllParkingLots(viz_lanelet_map); + lanelet::ConstPolygons3d obstacle_polygons = + lanelet::utils::query::getAllObstaclePolygons(viz_lanelet_map); + lanelet::ConstPolygons3d no_obstacle_segmentation_area = + lanelet::utils::query::getAllPolygonsByType(viz_lanelet_map, "no_obstacle_segmentation_area"); + lanelet::ConstPolygons3d no_obstacle_segmentation_area_for_run_out = + lanelet::utils::query::getAllPolygonsByType( + viz_lanelet_map, "no_obstacle_segmentation_area_for_run_out"); + lanelet::ConstPolygons3d hatched_road_markings_area = + lanelet::utils::query::getAllPolygonsByType(viz_lanelet_map, "hatched_road_markings"); + lanelet::ConstPolygons3d intersection_areas = + lanelet::utils::query::getAllPolygonsByType(viz_lanelet_map, "intersection_area"); + std::vector no_parking_reg_elems = + lanelet::utils::query::noParkingAreas(all_lanelets); + lanelet::ConstLineStrings3d curbstones = lanelet::utils::query::curbstones(viz_lanelet_map); + std::vector bus_stop_reg_elems; + + std_msgs::msg::ColorRGBA cl_road; + std_msgs::msg::ColorRGBA cl_shoulder; + std_msgs::msg::ColorRGBA cl_cross; + std_msgs::msg::ColorRGBA cl_partitions; + std_msgs::msg::ColorRGBA cl_pedestrian_markings; + std_msgs::msg::ColorRGBA cl_ll_borders; + std_msgs::msg::ColorRGBA cl_shoulder_borders; + std_msgs::msg::ColorRGBA cl_stoplines; + std_msgs::msg::ColorRGBA cl_trafficlights; + std_msgs::msg::ColorRGBA cl_detection_areas; + std_msgs::msg::ColorRGBA cl_speed_bumps; + std_msgs::msg::ColorRGBA cl_crosswalks; + std_msgs::msg::ColorRGBA cl_parking_lots; + std_msgs::msg::ColorRGBA cl_parking_spaces; + std_msgs::msg::ColorRGBA cl_lanelet_id; + std_msgs::msg::ColorRGBA cl_obstacle_polygons; + std_msgs::msg::ColorRGBA cl_no_stopping_areas; + std_msgs::msg::ColorRGBA cl_no_obstacle_segmentation_area; + std_msgs::msg::ColorRGBA cl_no_obstacle_segmentation_area_for_run_out; + std_msgs::msg::ColorRGBA cl_hatched_road_markings_area; + std_msgs::msg::ColorRGBA cl_hatched_road_markings_line; + std_msgs::msg::ColorRGBA cl_no_parking_areas; + std_msgs::msg::ColorRGBA cl_curbstones; + std_msgs::msg::ColorRGBA cl_intersection_area; + std_msgs::msg::ColorRGBA cl_bus_stop_area; + std_msgs::msg::ColorRGBA cl_bicycle_lane; + set_color(&cl_road, 0.27, 0.27, 0.27, 0.999); + set_color(&cl_shoulder, 0.15, 0.15, 0.15, 0.999); + set_color(&cl_cross, 0.27, 0.3, 0.27, 0.5); + set_color(&cl_partitions, 0.25, 0.25, 0.25, 0.999); + set_color(&cl_pedestrian_markings, 0.5, 0.5, 0.5, 0.999); + set_color(&cl_ll_borders, 0.5, 0.5, 0.5, 0.999); + set_color(&cl_shoulder_borders, 0.2, 0.2, 0.2, 0.999); + set_color(&cl_stoplines, 0.5, 0.5, 0.5, 0.999); + set_color(&cl_trafficlights, 0.5, 0.5, 0.5, 0.8); + set_color(&cl_detection_areas, 0.27, 0.27, 0.37, 0.5); + set_color(&cl_no_stopping_areas, 0.37, 0.37, 0.37, 0.5); + set_color(&cl_speed_bumps, 0.56, 0.40, 0.27, 0.5); + set_color(&cl_crosswalks, 0.80, 0.80, 0.0, 0.5); + set_color(&cl_obstacle_polygons, 0.4, 0.27, 0.27, 0.5); + set_color(&cl_parking_lots, 1.0, 1.0, 1.0, 0.2); + set_color(&cl_parking_spaces, 1.0, 1.0, 1.0, 0.3); + set_color(&cl_lanelet_id, 0.5, 0.5, 0.5, 0.999); + set_color(&cl_no_obstacle_segmentation_area, 0.37, 0.37, 0.27, 0.5); + set_color(&cl_no_obstacle_segmentation_area_for_run_out, 0.37, 0.7, 0.27, 0.5); + set_color(&cl_hatched_road_markings_area, 0.3, 0.3, 0.3, 0.5); + set_color(&cl_hatched_road_markings_line, 0.5, 0.5, 0.5, 0.999); + set_color(&cl_no_parking_areas, 0.42, 0.42, 0.42, 0.5); + set_color(&cl_curbstones, 0.1, 0.1, 0.2, 0.999); + set_color(&cl_intersection_area, 0.16, 1.0, 0.69, 0.5); + set_color(&cl_bus_stop_area, 0.863, 0.863, 0.863, 0.5); + set_color(&cl_bicycle_lane, 0.0, 0.3843, 0.6274, 0.5); + + visualization_msgs::msg::MarkerArray map_marker_array; + + insert_marker_array( + &map_marker_array, + lanelet::visualization::lineStringsAsMarkerArray(stop_lines, "stop_lines", cl_stoplines, 0.5)); + insert_marker_array( + &map_marker_array, + lanelet::visualization::lineStringsAsMarkerArray(partitions, "partitions", cl_partitions, 0.1)); + insert_marker_array( + &map_marker_array, + lanelet::visualization::laneletDirectionAsMarkerArray(shoulder_lanelets, "shoulder_")); + insert_marker_array( + &map_marker_array, lanelet::visualization::laneletDirectionAsMarkerArray(road_lanelets)); + insert_marker_array( + &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( + "crosswalk_lanelets", crosswalk_lanelets, cl_cross)); + insert_marker_array( + &map_marker_array, lanelet::visualization::pedestrianPolygonMarkingsAsMarkerArray( + pedestrian_polygon_markings, cl_pedestrian_markings)); + + insert_marker_array( + &map_marker_array, lanelet::visualization::pedestrianLineMarkingsAsMarkerArray( + pedestrian_line_markings, cl_pedestrian_markings)); + insert_marker_array( + &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( + "walkway_lanelets", walkway_lanelets, cl_cross)); + insert_marker_array( + &map_marker_array, + lanelet::visualization::obstaclePolygonsAsMarkerArray(obstacle_polygons, cl_obstacle_polygons)); + insert_marker_array( + &map_marker_array, + lanelet::visualization::detectionAreasAsMarkerArray(da_reg_elems, cl_detection_areas)); + insert_marker_array( + &map_marker_array, + lanelet::visualization::noStoppingAreasAsMarkerArray(no_reg_elems, cl_no_stopping_areas)); + insert_marker_array( + &map_marker_array, + lanelet::visualization::speedBumpsAsMarkerArray(sb_reg_elems, cl_speed_bumps)); + insert_marker_array( + &map_marker_array, + lanelet::visualization::crosswalkAreasAsMarkerArray(cw_reg_elems, cl_crosswalks)); + insert_marker_array( + &map_marker_array, + lanelet::visualization::parkingLotsAsMarkerArray(parking_lots, cl_parking_lots)); + insert_marker_array( + &map_marker_array, + lanelet::visualization::parkingSpacesAsMarkerArray(parking_spaces, cl_parking_spaces)); + insert_marker_array( + &map_marker_array, + lanelet::visualization::laneletsBoundaryAsMarkerArray( + shoulder_lanelets, cl_shoulder_borders, viz_lanelets_centerline_, "shoulder_")); + insert_marker_array( + &map_marker_array, lanelet::visualization::laneletsBoundaryAsMarkerArray( + road_lanelets, cl_ll_borders, viz_lanelets_centerline_)); + insert_marker_array( + &map_marker_array, + lanelet::visualization::autowareTrafficLightsAsMarkerArray(aw_tl_reg_elems, cl_trafficlights)); + insert_marker_array( + &map_marker_array, lanelet::visualization::generateTrafficLightRegulatoryElementIdMaker( + road_lanelets, cl_trafficlights)); + insert_marker_array( + &map_marker_array, lanelet::visualization::generateTrafficLightRegulatoryElementIdMaker( + crosswalk_lanelets, cl_trafficlights)); + insert_marker_array( + &map_marker_array, + lanelet::visualization::generateTrafficLightIdMaker(aw_tl_reg_elems, cl_trafficlights)); + insert_marker_array( + &map_marker_array, + lanelet::visualization::generateLaneletIdMarker(shoulder_lanelets, cl_lanelet_id)); + insert_marker_array( + &map_marker_array, + lanelet::visualization::generateLaneletIdMarker(road_lanelets, cl_lanelet_id)); + insert_marker_array( + &map_marker_array, lanelet::visualization::generateLaneletIdMarker( + crosswalk_lanelets, cl_lanelet_id, "crosswalk_lanelet_id")); + insert_marker_array( + &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( + "shoulder_road_lanelets", shoulder_lanelets, cl_shoulder)); + insert_marker_array( + &map_marker_array, + lanelet::visualization::laneletsAsTriangleMarkerArray("road_lanelets", road_lanelets, cl_road)); + insert_marker_array( + &map_marker_array, lanelet::visualization::noObstacleSegmentationAreaAsMarkerArray( + no_obstacle_segmentation_area, cl_no_obstacle_segmentation_area)); + insert_marker_array( + &map_marker_array, + lanelet::visualization::noObstacleSegmentationAreaForRunOutAsMarkerArray( + no_obstacle_segmentation_area_for_run_out, cl_no_obstacle_segmentation_area_for_run_out)); + + insert_marker_array( + &map_marker_array, + lanelet::visualization::hatchedRoadMarkingsAreaAsMarkerArray( + hatched_road_markings_area, cl_hatched_road_markings_area, cl_hatched_road_markings_line)); + + insert_marker_array( + &map_marker_array, + lanelet::visualization::noParkingAreasAsMarkerArray(no_parking_reg_elems, cl_no_parking_areas)); + + insert_marker_array( + &map_marker_array, + lanelet::visualization::lineStringsAsMarkerArray(curbstones, "curbstone", cl_curbstones, 0.2)); + + insert_marker_array( + &map_marker_array, lanelet::visualization::intersectionAreaAsMarkerArray( + intersection_areas, cl_intersection_area)); + + + insert_marker_array( + &map_marker_array, + lanelet::visualization::laneletDirectionAsMarkerArray(bicycle_lane_lanelets, "bicycle_lane_")); + insert_marker_array( + &map_marker_array, lanelet::visualization::laneletsBoundaryAsMarkerArray( + bicycle_lane_lanelets, cl_ll_borders /* use ll_border color */, + viz_lanelets_centerline_, "bicycle_lane_")); + insert_marker_array( + &map_marker_array, lanelet::visualization::generateLaneletIdMarker( + bicycle_lane_lanelets, cl_lanelet_id /* use lanelet_id color */)); + insert_marker_array( + &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( + "bicycle_lane_lanelets", bicycle_lane_lanelets, cl_bicycle_lane)); + + pub_marker_->publish(map_marker_array); +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(Lanelet2MapVisualizationNode) \ No newline at end of file diff --git a/crp_VIL/mpc_camera_driver b/crp_VIL/mpc_camera_driver index 56e9aff..9c4de26 160000 --- a/crp_VIL/mpc_camera_driver +++ b/crp_VIL/mpc_camera_driver @@ -1 +1 @@ -Subproject commit 56e9affd11fb1d0361e55d1d9fde4928baaccabc +Subproject commit 9c4de264a40c262b6fd3ded07e3e2794284d154e