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/crp_msgs/msg/Scenario.msg b/crp_APL/crp_msgs/msg/Scenario.msg index 32daade..fe1041c 100644 --- a/crp_APL/crp_msgs/msg/Scenario.msg +++ b/crp_APL/crp_msgs/msg/Scenario.msg @@ -3,5 +3,4 @@ std_msgs/Header header autoware_perception_msgs/PredictedObjects local_moving_objects autoware_perception_msgs/PredictedObjects local_obstacles crp_msgs/PathWithTrafficRules[] paths -nav_msgs/OccupancyGrid free_space -float32 maximum_speed \ No newline at end of file +nav_msgs/OccupancyGrid free_space \ 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_APL/plan_behavior_planning/include/plan_behavior_planning/behaviorPlanner.hpp b/crp_APL/plan_behavior_planning/include/plan_behavior_planning/behaviorPlanner.hpp index b3a6a9a..2ebb09a 100644 --- a/crp_APL/plan_behavior_planning/include/plan_behavior_planning/behaviorPlanner.hpp +++ b/crp_APL/plan_behavior_planning/include/plan_behavior_planning/behaviorPlanner.hpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include @@ -23,17 +24,20 @@ class BehaviorPlanner : public rclcpp::Node void routeCallback(const autoware_planning_msgs::msg::LaneletRoute::SharedPtr msg); void scenarioCallback(const crp_msgs::msg::Scenario::SharedPtr msg); void egoCallback(const crp_msgs::msg::Ego::SharedPtr msg); + void behaviorCallback(const crp_msgs::msg::Behavior::SharedPtr msg); void loop(); rclcpp::Subscription::SharedPtr m_sub_route; rclcpp::Subscription::SharedPtr m_sub_scenario; rclcpp::Subscription::SharedPtr m_sub_ego; + rclcpp::Subscription::SharedPtr m_sub_behavior_; rclcpp::Publisher::SharedPtr m_pub_strategy; rclcpp::Publisher::SharedPtr m_pub_target_space; rclcpp::TimerBase::SharedPtr timer_; + float m_maximum_speed; }; } // namespace apl diff --git a/crp_APL/plan_behavior_planning/src/behaviorPlanner.cpp b/crp_APL/plan_behavior_planning/src/behaviorPlanner.cpp index 416a1f0..15d4696 100644 --- a/crp_APL/plan_behavior_planning/src/behaviorPlanner.cpp +++ b/crp_APL/plan_behavior_planning/src/behaviorPlanner.cpp @@ -10,6 +10,10 @@ crp::apl::BehaviorPlanner::BehaviorPlanner() : Node("behavior_planner") m_sub_ego = this->create_subscription( "ego", 10, std::bind(&BehaviorPlanner::egoCallback, this, std::placeholders::_1)); + m_sub_behavior_ = this->create_subscription( + "/ui/behavior", 10, + std::bind(&BehaviorPlanner::behaviorCallback, this, std::placeholders::_1)); + m_pub_strategy = this->create_publisher("plan/strategy", 10); m_pub_target_space = this->create_publisher("plan/target_space", 10); @@ -32,10 +36,11 @@ void crp::apl::BehaviorPlanner::scenarioCallback(const crp_msgs::msg::Scenario:: if (msg->paths.size() > 0) targetSpaceMsg.path = msg->paths[0]; - for (tier4_planning_msgs::msg::PathPointWithLaneId & pathPoint : targetSpaceMsg.path.path.points) - { - pathPoint.point.longitudinal_velocity_mps = std::min(msg->maximum_speed, pathPoint.point.longitudinal_velocity_mps); - } + + for (tier4_planning_msgs::msg::PathPointWithLaneId & pathPoint : targetSpaceMsg.path.path.points) + { + pathPoint.point.longitudinal_velocity_mps = std::min(m_maximum_speed, pathPoint.point.longitudinal_velocity_mps); + } targetSpaceMsg.free_space = msg->free_space; @@ -50,6 +55,13 @@ void crp::apl::BehaviorPlanner::egoCallback(const crp_msgs::msg::Ego::SharedPtr return; } + +void crp::apl::BehaviorPlanner::behaviorCallback(const crp_msgs::msg::Behavior::SharedPtr msg) +{ + m_maximum_speed = msg->target_speed.data; +} + + void crp::apl::BehaviorPlanner::loop() { // possible Scenrios: "off", "laneFollowWithSpeedAdjust", "laneFollow", "speedAdjust" diff --git a/crp_APL/prcp_situation_analysis/include/prcp_situation_analysis/scenarioFusion.hpp b/crp_APL/prcp_situation_analysis/include/prcp_situation_analysis/scenarioFusion.hpp index 3de8aa5..b6f721e 100644 --- a/crp_APL/prcp_situation_analysis/include/prcp_situation_analysis/scenarioFusion.hpp +++ b/crp_APL/prcp_situation_analysis/include/prcp_situation_analysis/scenarioFusion.hpp @@ -25,14 +25,12 @@ class ScenarioFusion : public rclcpp::Node void localObstaclesCallback(const autoware_perception_msgs::msg::PredictedObjects::SharedPtr msg); void localPathCallback(const tier4_planning_msgs::msg::PathWithLaneId::SharedPtr msg); void localDrivableSurfaceCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg); - void behaviorCallback(const crp_msgs::msg::Behavior::SharedPtr msg); void publishCallback(); rclcpp::Subscription::SharedPtr m_sub_localMovingObjects_; rclcpp::Subscription::SharedPtr m_sub_localObstacles_; rclcpp::Subscription::SharedPtr m_sub_localPath_; rclcpp::Subscription::SharedPtr m_sub_localDrivableSurface_; - rclcpp::Subscription::SharedPtr m_sub_behavior_; rclcpp::Publisher::SharedPtr m_pub_scenario_; diff --git a/crp_APL/prcp_situation_analysis/src/scenarioFusion.cpp b/crp_APL/prcp_situation_analysis/src/scenarioFusion.cpp index 8139187..7d2e9bb 100644 --- a/crp_APL/prcp_situation_analysis/src/scenarioFusion.cpp +++ b/crp_APL/prcp_situation_analysis/src/scenarioFusion.cpp @@ -3,9 +3,6 @@ crp::apl::ScenarioFusion::ScenarioFusion() : Node("scenario_fusion") { - m_sub_behavior_ = this->create_subscription( - "/cai/behavior", 10, - std::bind(&ScenarioFusion::behaviorCallback, this, std::placeholders::_1)); m_sub_localMovingObjects_ = this->create_subscription( "/cai/local_moving_objects", 10, std::bind(&ScenarioFusion::localMovingObjectsCallback, this, std::placeholders::_1)); @@ -31,11 +28,6 @@ void crp::apl::ScenarioFusion::publishCallback() } -void crp::apl::ScenarioFusion::behaviorCallback(const crp_msgs::msg::Behavior::SharedPtr msg) -{ - m_scenario.maximum_speed = msg->target_speed.data; -} - void crp::apl::ScenarioFusion::localMovingObjectsCallback(const autoware_perception_msgs::msg::PredictedObjects::SharedPtr msg) { m_scenario.header = msg->header; diff --git a/crp_APL/ui/layouts/CRP Dasboard.json b/crp_APL/ui/layouts/CRP Dasboard.json index a81c502..1ad566a 100644 --- a/crp_APL/ui/layouts/CRP Dasboard.json +++ b/crp_APL/ui/layouts/CRP Dasboard.json @@ -103,6 +103,15 @@ "topicName": "/lexus3/control_reinit", "datatype": "std_msgs/msg/Bool" }, + "Publish!32kth13": { + "buttonText": "Publish", + "buttonTooltip": "", + "advancedView": true, + "value": "{\n \"target_speed\": { \"data\": 11.4\n },\n \"curve_driving_style\": { \"data\": 0\n }\n}", + "topicName": "/ui/behavior", + "datatype": "crp_msgs/msg/Behavior", + "foxglovePanelTitle": "Behavior" + }, "Gauge!1dii1on": { "path": "/ego.twist.twist.linear.x", "minValue": 0, @@ -453,7 +462,11 @@ }, "second": { "first": "RawMessages!2larpau", - "second": "Publish!3eqe9hn", + "second": { + "first": "Publish!3eqe9hn", + "second": "Publish!32kth13", + "direction": "row" + }, "direction": "row", "splitPercentage": 28.950826562211713 }, diff --git a/crp_VIL/lanelet_handler/CMakeLists.txt b/crp_VIL/lanelet_handler/CMakeLists.txt index 39337ab..ce4e0b8 100644 --- a/crp_VIL/lanelet_handler/CMakeLists.txt +++ b/crp_VIL/lanelet_handler/CMakeLists.txt @@ -9,18 +9,20 @@ 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) + 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/mcap_rec/launch/lexus_record.launch.py b/crp_VIL/mcap_rec/launch/recordLexus.launch.py similarity index 100% rename from crp_VIL/mcap_rec/launch/lexus_record.launch.py rename to crp_VIL/mcap_rec/launch/recordLexus.launch.py diff --git a/crp_VIL/mpc_camera_driver b/crp_VIL/mpc_camera_driver index 9c4de26..56e9aff 160000 --- a/crp_VIL/mpc_camera_driver +++ b/crp_VIL/mpc_camera_driver @@ -1 +1 @@ -Subproject commit 9c4de264a40c262b6fd3ded07e3e2794284d154e +Subproject commit 56e9affd11fb1d0361e55d1d9fde4928baaccabc diff --git a/doc/SystemSpecification.pdf b/doc/SystemSpecification.pdf index 67be25f..42e80fc 100644 Binary files a/doc/SystemSpecification.pdf and b/doc/SystemSpecification.pdf differ diff --git a/doc/SystemSpecification.tex b/doc/SystemSpecification.tex index f22985c..00b4f7b 100644 --- a/doc/SystemSpecification.tex +++ b/doc/SystemSpecification.tex @@ -514,6 +514,35 @@ \subsubsection{Inner workings} \item Yaw rate $\psi= v_\xi tan(\frac{\alpha_f}{L_w})$, where $\psi$ is the yaw rate, $\alpha_f$ is the front road-wheel angle and $L_w$ is the wheelbase. \end{itemize} + +\subsection{duro\_gps\_launcher} +\subsubsection{Purpose} +The purpose of this package is to launch the duro GPS driver. The driver is in a separate repository included as a subrepository. It also contains a node (duro\_topic\_converter) that converts the duro GPS topics to the predefined CRP topics. +\subsubsection{Usage} +The package only contains the launcher for the driver. This can be started as follows: +\begin{lstlisting} + ros2 launch duro_gps_launcher duro.launch.py +\end{lstlisting} +\subsubsection{Launch parameters} +The parameters of the launcher are given in Table \ref{tab:duro_gps_launcher}. The default values are set for the Lexus vehicle. +\begin{table}[!h] + \captionsetup{justification=centering} + \normalsize + \caption{\label{tab:duro_gps_launcher} Duro gps launcher parameters.} + \begin{tabular}{| l | l | l |} + \hline + \textbf{Name} & \textbf{Default value} & \textbf{Description} \\ + \hline + duro\_namespace & gps/duro & Namespace for the Novatel GPS \\ + \hline + duro\_ip & 192.168.10.11 & IP address of the Duro GPS \\ + \hline + duro\_port & 5555 & Port of the Novatel GPS \\ + \hline + \end{tabular} +\end{table} + + \subsection{novatel\_gps\_launcher} \subsubsection{Purpose} The purpose of this package is to launch the novatel GPS driver. The driver is in a separate repository included as a subrepository. It also contains a node (novatel\_topic\_converter) that converts the novatel GPS topics to the predefined CRP topics. @@ -578,6 +607,19 @@ \subsubsection{Launch parameters} \end{tabular} \end{table} + +\subsection{mcap\_rec} +\subsubsection{Purpose} +This package contains scipts for recording all the necessary topics of the project. +\subsubsection{Usage} +The package can be used by the provided launchers: +\begin{lstlisting} + ros2 launch mcap_rec recordLexus.launch.py tag:=name_of_the_recording +\end{lstlisting} +\subsubsection{Recorded params} +Each launcher has a different preset for which topics to record. The recorded topics are defined in the etc folder in txt files. + + \subsection{nissan\_can\_driver} \subsubsection{Purpose} The purpose of this package is to publish the vehicle status (speed, steering) to topics and receive control commands from topics to control the vehicle. @@ -598,7 +640,8 @@ \subsubsection{Launch parameters} \hline \textbf{Name} & \textbf{Default value} & \textbf{Description} \\ \hline - autoware\_control\_input & True & Whether to use autoware or standard type input message for control \\ + autoware\_control\_input & True & Whether to use autoware or standard type \\ + & & input message for control \\ \hline kvaser\_hardware\_id & 11162 & HW ID of the kvaser device used for CAN communication \\ \hline diff --git a/external/map_loader/CMakeLists.txt b/external/map_loader/CMakeLists.txt new file mode 100644 index 0000000..c6b1ead --- /dev/null +++ b/external/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/external/map_loader/README.md b/external/map_loader/README.md new file mode 100644 index 0000000..bc3eb80 --- /dev/null +++ b/external/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/external/map_loader/config/lanelet2_map_loader.param.yaml b/external/map_loader/config/lanelet2_map_loader.param.yaml new file mode 100755 index 0000000..48d392a --- /dev/null +++ b/external/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/external/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp b/external/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp new file mode 100644 index 0000000..049d714 --- /dev/null +++ b/external/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/external/map_loader/launch/lanelet2_map_loader.launch.xml b/external/map_loader/launch/lanelet2_map_loader.launch.xml new file mode 100644 index 0000000..e71906b --- /dev/null +++ b/external/map_loader/launch/lanelet2_map_loader.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/external/map_loader/package.xml b/external/map_loader/package.xml new file mode 100644 index 0000000..b86183c --- /dev/null +++ b/external/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/external/map_loader/schema/lanelet2_map_loader.schema.json b/external/map_loader/schema/lanelet2_map_loader.schema.json new file mode 100644 index 0000000..a55050e --- /dev/null +++ b/external/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/external/map_loader/script/map_hash_generator b/external/map_loader/script/map_hash_generator new file mode 100755 index 0000000..5fb5755 --- /dev/null +++ b/external/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/external/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/external/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp new file mode 100644 index 0000000..60e34f8 --- /dev/null +++ b/external/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/launch/crp_launcher/launch/core.launch.py b/launch/crp_launcher/launch/core.launch.py new file mode 100644 index 0000000..efa4491 --- /dev/null +++ b/launch/crp_launcher/launch/core.launch.py @@ -0,0 +1,118 @@ +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.conditions import LaunchConfigurationEquals +from launch.launch_description_sources import PythonLaunchDescriptionSource, AnyLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory +from os.path import join + + +def generate_launch_description(): + + # ARGS + + # sensor abstraction + vehicle_tire_angle_topic_arg = DeclareLaunchArgument( + 'vehicle_tire_angle_topic', + default_value='/sensing/vehicle/tire_angle', + description='Length of the scenario in meters') + local_path_length_arg = DeclareLaunchArgument( + 'local_path_length', + default_value='70.0', + description='Length of the scenario in meters') + + # NODES + + sensor_abstraction = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + join( + get_package_share_directory('prcp_sensor_abstraction'), + 'launch', + 'sensor_abstraction.launch.py') + ) + ) + + environmental_fusion = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + join( + get_package_share_directory('prcp_situation_analysis'), + 'launch', + 'environmental_fusion.launch.py') + ) + ) + + behavior_planning = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + join( + get_package_share_directory('plan_behavior_planning'), + 'launch', + 'behavior_planning.launch.py' + ) + ) + ) + + motion_planning = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + join( + get_package_share_directory('plan_motion_planning'), + 'launch', + 'motion_planning.launch.py' + ) + ) + ) + + planner_lat_lane_follow_ldm = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + join( + get_package_share_directory('plan_lat_lane_follow_ldm'), + 'launch', + 'plan_lat_lane_follow_ldm.launch.py' + ) + ) + ) + + vehicle_control = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + join( + get_package_share_directory('ctrl_vehicle_control'), + 'launch', + 'ctrl_vehicle_control.launch.py') + ) + ) + + vehicle_control_lat = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + join( + get_package_share_directory('ctrl_vehicle_control_lat_compensatory'), + 'launch', + 'ctrl_vehicle_control_lat_compensatory.launch.py') + ) + ) + + vehicle_control_long = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + join( + get_package_share_directory('ctrl_vehicle_control_long'), + 'launch', + 'ctrl_vehicle_control_long.launch.py') + ) + ) + + return LaunchDescription([ + # args + vehicle_tire_angle_topic_arg, + vehicle_tire_angle_topic_arg, + local_path_length_arg, + + # nodes + sensor_abstraction, + environmental_fusion, + behavior_planning, + motion_planning, + vehicle_control, + vehicle_control_lat, + vehicle_control_long, + + planner_lat_lane_follow_ldm + ]) diff --git a/launch/crp_launcher/launch/lexus.launch.py b/launch/crp_launcher/launch/lexus.launch.py index 5b3c6c7..ef2e876 100644 --- a/launch/crp_launcher/launch/lexus.launch.py +++ b/launch/crp_launcher/launch/lexus.launch.py @@ -77,6 +77,18 @@ def generate_launch_description(): default_value='70.0', description='Length of the scenario in meters') + + # CORE + + crp_core = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + join( + get_package_share_directory('crp_launcher'), + 'launch', + 'core.launch.py') + ) + ) + # NODES novatel_gps = IncludeLaunchDescription( @@ -198,63 +210,6 @@ def generate_launch_description(): ) ) - sensor_abstraction = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - join( - get_package_share_directory('prcp_sensor_abstraction'), - 'launch', - 'sensor_abstraction.launch.py') - ) - ) - - environmental_fusion = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - join( - get_package_share_directory('prcp_situation_analysis'), - 'launch', - 'environmental_fusion.launch.py') - ) - ) - - behavior_planning = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - join( - get_package_share_directory('plan_behavior_planning'), - 'launch', - 'behavior_planning.launch.py' - ) - ) - ) - - motion_planning = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - join( - get_package_share_directory('plan_motion_planning'), - 'launch', - 'motion_planning.launch.py' - ) - ) - ) - - planner_lat_lane_follow_ldm = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - join( - get_package_share_directory('plan_lat_lane_follow_ldm'), - 'launch', - 'plan_lat_lane_follow_ldm.launch.py' - ) - ) - ) - - vehicle_control = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - join( - get_package_share_directory('ctrl_vehicle_control'), - 'launch', - 'ctrl_vehicle_control.launch.py') - ) - ) - lexus_speed_control = IncludeLaunchDescription( PythonLaunchDescriptionSource( join( @@ -264,24 +219,6 @@ def generate_launch_description(): ) ) - vehicle_control_lat = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - join( - get_package_share_directory('ctrl_vehicle_control_lat_compensatory'), - 'launch', - 'ctrl_vehicle_control_lat_compensatory.launch.py') - ) - ) - - vehicle_control_long = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - join( - get_package_share_directory('ctrl_vehicle_control_long'), - 'launch', - 'ctrl_vehicle_control_long.launch.py') - ) - ) - return LaunchDescription([ # args select_gps_arg, @@ -300,6 +237,9 @@ def generate_launch_description(): vehicle_tire_angle_topic_arg, local_path_length_arg, + # core + crp_core, + # vehicle nodes novatel_gps, duro_gps, @@ -316,13 +256,4 @@ def generate_launch_description(): # nodes lanelet_file_loader, - sensor_abstraction, - environmental_fusion, - behavior_planning, - motion_planning, - vehicle_control, - vehicle_control_lat, - vehicle_control_long, - - planner_lat_lane_follow_ldm ]) diff --git a/launch/crp_launcher/package.xml b/launch/crp_launcher/package.xml index 02f9695..eb7cca5 100644 --- a/launch/crp_launcher/package.xml +++ b/launch/crp_launcher/package.xml @@ -9,22 +9,13 @@ ament_cmake - - novatel_gps_launcher tf2_ros - kvaser_interface - lanelet_handler prcp_sensor_abstraction prcp_situation_analysis plan_behavior_planning plan_motion_planning plan_lat_lane_follow_ldm ctrl_vehicle_control - - nissan_bringup - - lexus_bringup - pacmod3 ament_cmake diff --git a/scripts/build_all.sh b/scripts/build_all.sh index b5d7abe..cded75e 100755 --- a/scripts/build_all.sh +++ b/scripts/build_all.sh @@ -3,6 +3,36 @@ cd $script_dir while [ ! -e "src/" ]; do cd ../ done -echo $(pwd) +echo "Build location: "$(pwd) -colcon build --symlink-install \ No newline at end of file +# build core +trap 'exit' INT # trap ctrl-c +$script_dir/build_core.sh +trap - INT + +# build lexus packages +packages=( + duro_gps_driver + duro_gps_launcher + kvaser_interface + lanelet_handler + lexus_bringup + mcap_rec + mpc_camera_driver + novatel_gps_msgs + novatel_gps_driver + novatel_gps_launcher + pacmod_extender + pacmod_interface +) + +packages_string="" +for package in "${packages[@]}"; do + packages_string+="$package " +done + +packages_paths=$(colcon list --packages-up-to $packages_string -p) + +rosdep install --from-paths $packages_paths --ignore-src -r -y + +colcon build --packages-select $packages_string \ No newline at end of file diff --git a/scripts/build_core.sh b/scripts/build_core.sh index a227490..03685d5 100755 --- a/scripts/build_core.sh +++ b/scripts/build_core.sh @@ -3,9 +3,17 @@ cd $script_dir while [ ! -e "src/" ]; do cd ../ done -echo $(pwd) +echo "Build location: "$(pwd) -colcon build --symlink-install --packages-select \ +rosdep install --from-paths $script_dir/../crp_APL $script_dir/../crp_CIL --ignore-src -r -y +colcon build --packages-select \ +tier4_planning_msgs \ +autoware_common_msgs \ +autoware_planning_msgs \ +autoware_perception_msgs \ +autoware_control_msgs \ +autoware_localization_msgs \ +autoware_map_msgs \ crp_msgs \ prcp_sensor_abstraction \ prcp_situation_analysis \ @@ -18,4 +26,6 @@ plan_lon_intelligent_speed_adjust \ plan_motion_planning \ crp_launcher \ ctrl_vehicle_control \ -test_node \ No newline at end of file +ctrl_vehicle_control_lat_compensatory \ +ctrl_vehicle_control_long \ +test_node