diff --git a/.github/workflows/clang-tidy-differential.yaml b/.github/workflows/clang-tidy-differential.yaml index e2e4fc478e0ee..60337cc572e7a 100644 --- a/.github/workflows/clang-tidy-differential.yaml +++ b/.github/workflows/clang-tidy-differential.yaml @@ -38,7 +38,7 @@ jobs: - name: Get modified files id: get-modified-files run: | - echo "changed_files=$(git diff --name-only "origin/${{ github.base_ref }}"...HEAD | grep -E '\.(cpp|hpp)$' || true)" >> $GITHUB_OUTPUT + echo "changed_files=$(git diff --name-only "origin/${{ github.base_ref }}"...HEAD | grep -E '\.(cpp|hpp)$' | tr '\n' ' ' || true)" >> $GITHUB_OUTPUT shell: bash - name: Run clang-tidy diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp index e6421b2af84bb..876797a58df25 100644 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp @@ -100,7 +100,9 @@ class LaneDepartureCheckerNode : public rclcpp::Node // Publisher autoware::universe_utils::DebugPublisher debug_publisher_{this, "~/debug"}; - autoware::universe_utils::ProcessingTimePublisher processing_time_publisher_{this}; + autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{ + this, "~/debug/processing_time_ms_diag"}; + rclcpp::Publisher::SharedPtr processing_time_publisher_; // Timer rclcpp::TimerBase::SharedPtr timer_; diff --git a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index 427cf0898470e..dde37b03ffecc 100644 --- a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -170,6 +170,8 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o lane_departure_checker_->setParam(param_, vehicle_info); // Publisher + processing_time_publisher_ = + this->create_publisher("~/debug/processing_time_ms", 1); // Nothing // Diagnostic Updater @@ -347,7 +349,11 @@ void LaneDepartureCheckerNode::onTimer() } processing_time_map["Total"] = stop_watch.toc("Total"); - processing_time_publisher_.publish(processing_time_map); + processing_diag_publisher_.publish(processing_time_map); + tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = get_clock()->now(); + processing_time_msg.data = processing_time_map["Total"]; + processing_time_publisher_->publish(processing_time_msg); } rcl_interfaces::msg::SetParametersResult LaneDepartureCheckerNode::onParameter( diff --git a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp index 058eb45bfaaff..902790f260e5e 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp @@ -271,7 +271,7 @@ class MPC */ std::pair executeOptimization( const MPCMatrix & mpc_matrix, const VectorXd & x0, const double prediction_dt, - const MPCTrajectory & trajectory, const double current_velocity); + const MPCTrajectory & trajectory); /** * @brief Resample the trajectory with the MPC resampling time. @@ -386,8 +386,7 @@ class MPC * @param reference_trajectory The reference trajectory. * @param current_velocity current velocity of ego. */ - VectorXd calcSteerRateLimitOnTrajectory( - const MPCTrajectory & trajectory, const double current_velocity) const; + VectorXd calcSteerRateLimitOnTrajectory(const MPCTrajectory & trajectory) const; //!< @brief logging with warn and return false template diff --git a/control/autoware_mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp index c8e2da6daf7f4..ea97e9e6d5f39 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc.cpp @@ -76,9 +76,8 @@ bool MPC::calculateMPC( const auto mpc_matrix = generateMPCMatrix(mpc_resampled_ref_trajectory, prediction_dt); // solve Optimization problem - const auto [success_opt, Uex] = executeOptimization( - mpc_matrix, x0_delayed, prediction_dt, mpc_resampled_ref_trajectory, - current_kinematics.twist.twist.linear.x); + const auto [success_opt, Uex] = + executeOptimization(mpc_matrix, x0_delayed, prediction_dt, mpc_resampled_ref_trajectory); if (!success_opt) { return fail_warn_throttle("optimization failed. Stop MPC."); } @@ -544,8 +543,7 @@ MPCMatrix MPC::generateMPCMatrix( * [ -au_lim * dt ] < [uN-uN-1] < [ au_lim * dt ] (*N... DIM_U) */ std::pair MPC::executeOptimization( - const MPCMatrix & m, const VectorXd & x0, const double prediction_dt, const MPCTrajectory & traj, - const double current_velocity) + const MPCMatrix & m, const VectorXd & x0, const double prediction_dt, const MPCTrajectory & traj) { VectorXd Uex; @@ -578,7 +576,7 @@ std::pair MPC::executeOptimization( VectorXd ub = VectorXd::Constant(DIM_U_N, m_steer_lim); // max steering angle // steering angle rate limit - VectorXd steer_rate_limits = calcSteerRateLimitOnTrajectory(traj, current_velocity); + VectorXd steer_rate_limits = calcSteerRateLimitOnTrajectory(traj); VectorXd ubA = steer_rate_limits * prediction_dt; VectorXd lbA = -steer_rate_limits * prediction_dt; ubA(0) = m_raw_steer_cmd_prev + steer_rate_limits(0) * m_ctrl_period; @@ -730,8 +728,7 @@ double MPC::calcDesiredSteeringRate( return steer_rate; } -VectorXd MPC::calcSteerRateLimitOnTrajectory( - const MPCTrajectory & trajectory, const double current_velocity) const +VectorXd MPC::calcSteerRateLimitOnTrajectory(const MPCTrajectory & trajectory) const { const auto interp = [&](const auto & steer_rate_limit_map, const auto & current) { std::vector reference, limits; @@ -765,13 +762,6 @@ VectorXd MPC::calcSteerRateLimitOnTrajectory( return reference.back(); }; - // when the vehicle is stopped, no steering rate limit. - constexpr double steer_rate_lim = 5.0; - const bool is_vehicle_stopped = std::fabs(current_velocity) < 0.01; - if (is_vehicle_stopped) { - return steer_rate_lim * VectorXd::Ones(m_param.prediction_horizon); - } - // calculate steering rate limit VectorXd steer_rate_limits = VectorXd::Zero(m_param.prediction_horizon); for (int i = 0; i < m_param.prediction_horizon; ++i) { diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 9291d538b022f..5805ef7db9f86 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -64,7 +64,8 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro { public: /// \param node Reference to the node used only for the component and parameter initialization. - explicit PidLongitudinalController(rclcpp::Node & node); + explicit PidLongitudinalController( + rclcpp::Node & node, std::shared_ptr diag_updater); private: struct Motion @@ -236,8 +237,8 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro std::shared_ptr m_last_running_time{std::make_shared(clock_->now())}; // Diagnostic - - diagnostic_updater::Updater diagnostic_updater_; + std::shared_ptr + diag_updater_{}; // Diagnostic updater for publishing diagnostic data. struct DiagnosticData { double trans_deviation{0.0}; // translation deviation between nearest point and current_pose diff --git a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 78c7ccf832514..93496c85c741b 100644 --- a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -27,14 +27,16 @@ namespace autoware::motion::control::pid_longitudinal_controller { -PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) +PidLongitudinalController::PidLongitudinalController( + rclcpp::Node & node, std::shared_ptr diag_updater) : node_parameters_(node.get_node_parameters_interface()), clock_(node.get_clock()), - logger_(node.get_logger().get_child("longitudinal_controller")), - diagnostic_updater_(&node) + logger_(node.get_logger().get_child("longitudinal_controller")) { using std::placeholders::_1; + diag_updater_ = diag_updater; + // parameters timer m_longitudinal_ctrl_period = node.get_parameter("ctrl_period").as_double(); @@ -432,7 +434,7 @@ trajectory_follower::LongitudinalOutput PidLongitudinalController::run( publishDebugData(ctrl_cmd, control_data); // diagnostic - diagnostic_updater_.force_update(); + diag_updater_->force_update(); return output; } @@ -1150,8 +1152,8 @@ void PidLongitudinalController::updateDebugVelAcc(const ControlData & control_da void PidLongitudinalController::setupDiagnosticUpdater() { - diagnostic_updater_.setHardwareID("autoware_pid_longitudinal_controller"); - diagnostic_updater_.add("control_state", this, &PidLongitudinalController::checkControlState); + diag_updater_->setHardwareID("autoware_pid_longitudinal_controller"); + diag_updater_->add("control_state", this, &PidLongitudinalController::checkControlState); } void PidLongitudinalController::checkControlState( diff --git a/control/autoware_trajectory_follower_node/src/controller_node.cpp b/control/autoware_trajectory_follower_node/src/controller_node.cpp index c1e5fe646cdaa..d5167f5096786 100644 --- a/control/autoware_trajectory_follower_node/src/controller_node.cpp +++ b/control/autoware_trajectory_follower_node/src/controller_node.cpp @@ -57,7 +57,8 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control switch (longitudinal_controller_mode) { case LongitudinalControllerMode::PID: { longitudinal_controller_ = - std::make_shared(*this); + std::make_shared( + *this, diag_updater_); break; } default: diff --git a/control/autoware_vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp b/control/autoware_vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp index 46130fc8f9941..66dde51780ffa 100644 --- a/control/autoware_vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp +++ b/control/autoware_vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp @@ -294,7 +294,12 @@ class PubSubNode : public rclcpp::Node const auto max_lat_acc_lim = *std::max_element(lat_acc_lim.begin(), lat_acc_lim.end()); const auto max_lat_jerk_lim = *std::max_element(lat_jerk_lim.begin(), lat_jerk_lim.end()); + // This test is designed to verify that the filter is applied correctly. However, if topic + // communication is delayed, the allowable range of change in the command values between the + // sender and receiver of the topic will vary, making the results dependent on processing time. + // We define the allowable error margin to account for this. constexpr auto threshold_scale = 1.1; + // Output command must be smaller than maximum limit. // TODO(Horibe): check for each velocity range. if (std::abs(lon_vel) > 0.01) { @@ -433,7 +438,7 @@ TEST_P(TestFixture, CheckFilterForSinCmd) // << cmd_generator_.p_.steering.freq << " * dt + " << cmd_generator_.p_.steering.bias // << ")" << std::endl; - for (size_t i = 0; i < 100; ++i) { + for (size_t i = 0; i < 30; ++i) { auto start_time = std::chrono::steady_clock::now(); const bool reset_clock = (i == 0); @@ -449,7 +454,13 @@ TEST_P(TestFixture, CheckFilterForSinCmd) std::chrono::milliseconds elapsed = std::chrono::duration_cast(end_time - start_time); - std::chrono::milliseconds sleep_duration = std::chrono::milliseconds{10} - elapsed; + // The value determines the period of the topic. The filter logic of vehicle_cmd_gate depends on + // real-time, and if the time from publishing to subscribing becomes too long, this test will + // fail (the test specification itself should be improved). To prevent processing bottlenecks, + // please set this value appropriately. It is set to 30ms because it occasionally fails at 10ms. + constexpr int running_ms = 30; + + std::chrono::milliseconds sleep_duration = std::chrono::milliseconds{running_ms} - elapsed; if (sleep_duration.count() > 0) { std::this_thread::sleep_for(sleep_duration); } diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index 228b4c89f95d3..373e343ad64fa 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -1,5 +1,8 @@ + + + @@ -10,15 +13,13 @@ + - + + + - - - - - @@ -43,10 +44,21 @@ - - + + + + + + + + + + + + + @@ -56,35 +68,58 @@ + + + + + - + + + + + + + + - + + + + + + + + - + + + - + + - + + @@ -118,11 +153,12 @@ - + + + - @@ -133,6 +169,7 @@ + @@ -141,9 +178,9 @@ - + - + @@ -152,7 +189,7 @@ - + @@ -175,10 +212,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -208,8 +273,14 @@ - + + + + + + + @@ -250,8 +321,12 @@ - + + + + + @@ -264,32 +339,41 @@ - - - - - - - - - - - - + + + + + + + + + + + + + + + + - - - - - + + + + + + + - + - + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml index 3c8480038f320..4c288aa632182 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml @@ -1,17 +1,11 @@ - - - - - - - + @@ -38,6 +32,14 @@ + + + + + + + + - - - - - - - - - - - @@ -230,7 +220,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml index ab34cc49dd2f6..98e8ec7c8ed4b 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml @@ -1,18 +1,19 @@ - - - - + + + + + @@ -21,7 +22,7 @@ - + @@ -41,7 +42,7 @@ - + @@ -77,7 +78,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml index 819a6337138a1..38ee946201f7e 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml @@ -1,22 +1,10 @@ - + + - - - - - - - - - - - - - - + @@ -43,7 +31,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/tracker_based_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/tracker_based_detector.launch.xml index 0efe7444fcba5..b85d4b02847b3 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/tracker_based_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/tracker_based_detector.launch.xml @@ -1,10 +1,12 @@ + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml index 85b404ba89b1f..d4a9e40f29d97 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml @@ -44,9 +44,9 @@ - - - + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml index 16ec8f8fbd573..6ea745a8bae2c 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml @@ -47,16 +47,16 @@ - - - - - + + + + + - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml index b8172daa616fa..9bc4aabc39b20 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml @@ -1,5 +1,8 @@ + + + @@ -13,50 +16,84 @@ - - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 8f0513bcd5446..5d8d7563e7a7c 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -124,6 +124,9 @@ /> + + + @@ -189,6 +192,7 @@ + @@ -255,7 +259,9 @@ + + diff --git a/perception/lidar_centerpoint/test/test_postprocess_kernel.cpp b/perception/lidar_centerpoint/test/test_postprocess_kernel.cpp index fde4fcbee3d6c..e75aff416b8aa 100644 --- a/perception/lidar_centerpoint/test/test_postprocess_kernel.cpp +++ b/perception/lidar_centerpoint/test/test_postprocess_kernel.cpp @@ -94,12 +94,12 @@ TEST_F(PostprocessKernelTest, SingleDetectionTest) constexpr float detection_x = 70.f; constexpr float detection_y = -38.4f; constexpr float detection_z = 1.0; - constexpr float detection_log_w = std::log(7.0); - constexpr float detection_log_l = std::log(1.0); - constexpr float detection_log_h = std::log(2.0); + const float detection_log_w = std::log(7.0); + const float detection_log_l = std::log(1.0); + const float detection_log_h = std::log(2.0); constexpr float detection_yaw = M_PI_4; - constexpr float detection_yaw_sin = std::sin(detection_yaw); - constexpr float detection_yaw_cos = std::sin(detection_yaw); + const float detection_yaw_sin = std::sin(detection_yaw); + const float detection_yaw_cos = std::sin(detection_yaw); constexpr float detection_vel_x = 5.0; constexpr float detection_vel_y = -5.0; @@ -240,9 +240,9 @@ TEST_F(PostprocessKernelTest, CircleNMSTest) constexpr float detection_x = 70.f; constexpr float detection_y = -38.4f; constexpr float detection_z = 1.0; - constexpr float detection_log_w = std::log(7.0); - constexpr float detection_log_l = std::log(1.0); - constexpr float detection_log_h = std::log(2.0); + const float detection_log_w = std::log(7.0); + const float detection_log_l = std::log(1.0); + const float detection_log_h = std::log(2.0); constexpr float detection_yaw1_sin = 0.0; constexpr float detection_yaw1_cos = 1.0; constexpr float detection_yaw2_sin = 1.0; diff --git a/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp b/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp index 1bc548fa7a951..c6bd93b7a9371 100644 --- a/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp +++ b/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp @@ -137,22 +137,8 @@ LinearMotionTracker::LinearMotionTracker( } else { P_v_xy = R * P_v_xy_local * R.transpose(); } - // acceleration covariance often written in object frame - const bool has_acceleration_covariance = - false; // currently message does not have acceleration covariance - if (has_acceleration_covariance) { - // const auto ax_cov = - // object.kinematics.acceleration_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; // This - // is future update - // const auto ay_cov = - // object.kinematics.acceleration_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; // This - // is future update - // Eigen::Matrix2d P_a_xy_local; - // P_a_xy_local << ax_cov, 0.0, 0.0, ay_cov; - P_a_xy = R * P_a_xy_local * R.transpose(); - } else { - P_a_xy = R * P_a_xy_local * R.transpose(); - } + + P_a_xy = R * P_a_xy_local * R.transpose(); // put value in P matrix // use block description. This assume x,y,vx,vy,ax,ay in this order diff --git a/perception/tensorrt_yolox/src/tensorrt_yolox.cpp b/perception/tensorrt_yolox/src/tensorrt_yolox.cpp index ad8297052719a..34155c3477003 100644 --- a/perception/tensorrt_yolox/src/tensorrt_yolox.cpp +++ b/perception/tensorrt_yolox/src/tensorrt_yolox.cpp @@ -1279,7 +1279,6 @@ void TrtYoloX::getColorizedMask( int height = mask.rows; if ((cmask.cols != mask.cols) || (cmask.rows != mask.rows)) { throw std::runtime_error("input and output image have difference size."); - return; } for (int y = 0; y < height; y++) { for (int x = 0; x < width; x++) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md index 33abc8eaf40ed..b27723ab2066f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md @@ -485,11 +485,6 @@ title Filtering flow for vehicle type objects start partition isSatisfiedWithVehicleCodition() { -if(object is force avoidance target ?) then (yes) -#FF006C :return true; -stop -else (\n no) - if(isNeverAvoidanceTarget()) then (yes) #00FFB1 :return false; stop @@ -537,7 +532,6 @@ endif endif endif endif -endif #00FFB1 :return false; stop } diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json index 3e6f0ef1f61a3..266a4cf8a23ab 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json @@ -17,6 +17,7 @@ "default": "4.0" }, "path_generation_method": { + "type": "string", "enum": ["shift_line_base", "optimization_base", "both"], "description": "Path generation method.", "default": "shift_line_base" diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp index bf9dc30145cd3..12cb59ac46195 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp @@ -49,8 +49,10 @@ void DynamicObstacleStopModule::init(rclcpp::Node & node, const std::string & mo node.create_publisher("~/" + ns_ + "/debug_markers", 1); virtual_wall_publisher_ = node.create_publisher("~/" + ns_ + "/virtual_walls", 1); - processing_time_publisher_ = std::make_shared( - &node, "~/debug/" + ns_ + "/processing_time_ms"); + processing_diag_publisher_ = std::make_shared( + &node, "~/debug/" + ns_ + "/processing_time_ms_diag"); + processing_time_publisher_ = node.create_publisher( + "~/debug/" + ns_ + "/processing_time_ms", 1); using autoware::universe_utils::getOrDeclareParameter; auto & p = params_; @@ -187,7 +189,11 @@ VelocityPlanningResult DynamicObstacleStopModule::plan( processing_times["footprints"] = footprints_duration_us / 1000; processing_times["collisions"] = collisions_duration_us / 1000; processing_times["Total"] = total_time_us / 1000; - processing_time_publisher_->publish(processing_times); + processing_diag_publisher_->publish(processing_times); + tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = clock_->now(); + processing_time_msg.data = processing_times["Total"]; + processing_time_publisher_->publish(processing_time_msg); return result; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp index aeeaabeb9b510..af63b50bceb39 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp @@ -50,7 +50,7 @@ size_t calculateStartIndex( /// @param[in] start_idx starting index of the input trajectory /// @param[in] factor factor used for downsampling /// @return downsampled trajectory -TrajectoryPoints downsampleTrajectory( +TrajectoryPoints downsample_trajectory( const TrajectoryPoints & trajectory, const size_t start_idx, const int factor); /// @brief recalculate the steering angle of the trajectory @@ -123,16 +123,6 @@ std::vector calculate_slowd const std::vector & projections, const std::vector & footprints, ProjectionParameters & projection_params, const VelocityParameters & velocity_params, autoware::motion_utils::VirtualWalls & virtual_walls); - -/// @brief copy the velocity profile of a downsampled trajectory to the original trajectory -/// @param[in] downsampled_trajectory downsampled trajectory -/// @param[in] trajectory input trajectory -/// @param[in] start_idx starting index of the downsampled trajectory relative to the input -/// @param[in] factor downsampling factor -/// @return input trajectory with the velocity profile of the downsampled trajectory -TrajectoryPoints copyDownsampledVelocity( - const TrajectoryPoints & downsampled_traj, TrajectoryPoints trajectory, const size_t start_idx, - const int factor); } // namespace autoware::motion_velocity_planner::obstacle_velocity_limiter #endif // OBSTACLE_VELOCITY_LIMITER_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp index 5a4f03f6f20a6..efbdcdaf464e2 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp @@ -23,6 +23,7 @@ #include #include +#include #include #include #include @@ -53,8 +54,10 @@ void ObstacleVelocityLimiterModule::init(rclcpp::Node & node, const std::string node.create_publisher("~/" + ns_ + "/debug_markers", 1); virtual_wall_publisher_ = node.create_publisher("~/" + ns_ + "/virtual_walls", 1); - processing_time_publisher_ = std::make_shared( - &node, "~/debug/" + ns_ + "/processing_time_ms"); + processing_diag_publisher_ = std::make_shared( + &node, "~/debug/" + ns_ + "/processing_time_ms_diag"); + processing_time_publisher_ = node.create_publisher( + "~/debug/" + ns_ + "/processing_time_ms", 1); const auto vehicle_info = vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); vehicle_lateral_offset_ = static_cast(vehicle_info.max_lateral_offset_m); @@ -157,7 +160,7 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan( const auto end_idx = obstacle_velocity_limiter::calculateEndIndex( original_traj_points, start_idx, preprocessing_params_.max_length, preprocessing_params_.max_duration); - auto downsampled_traj_points = obstacle_velocity_limiter::downsampleTrajectory( + auto downsampled_traj_points = downsample_trajectory( original_traj_points, start_idx, end_idx, preprocessing_params_.downsample_factor); obstacle_velocity_limiter::ObstacleMasks obstacle_masks; const auto preprocessing_us = stopwatch.toc("preprocessing"); @@ -226,7 +229,11 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan( processing_times["obstacles"] = obstacles_us / 1000; processing_times["slowdowns"] = slowdowns_us / 1000; processing_times["Total"] = total_us / 1000; - processing_time_publisher_->publish(processing_times); + processing_diag_publisher_->publish(processing_times); + tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = clock_->now(); + processing_time_msg.data = processing_times["Total"]; + processing_time_publisher_->publish(processing_time_msg); return result; } } // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp index f4b1304d67e5e..73e05bed88c37 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp @@ -55,17 +55,6 @@ size_t calculateEndIndex( return idx; } -TrajectoryPoints downsampleTrajectory( - const TrajectoryPoints & trajectory, const size_t start_idx, const size_t end_idx, - const int factor) -{ - if (factor < 1) return trajectory; - TrajectoryPoints downsampled_traj; - downsampled_traj.reserve((end_idx - start_idx) / factor); - for (size_t i = start_idx; i <= end_idx; i += factor) downsampled_traj.push_back(trajectory[i]); - return downsampled_traj; -} - void calculateSteeringAngles(TrajectoryPoints & trajectory, const double wheel_base) { auto prev_point = trajectory.front(); @@ -82,16 +71,4 @@ void calculateSteeringAngles(TrajectoryPoints & trajectory, const double wheel_b std::atan2(wheel_base * d_heading, point.longitudinal_velocity_mps * dt); } } - -TrajectoryPoints copyDownsampledVelocity( - const TrajectoryPoints & downsampled_traj, TrajectoryPoints trajectory, const size_t start_idx, - const int factor) -{ - const auto size = std::min(downsampled_traj.size(), trajectory.size()); - for (size_t i = 0; i < size; ++i) { - trajectory[start_idx + i * factor].longitudinal_velocity_mps = - downsampled_traj[i].longitudinal_velocity_mps; - } - return trajectory; -} } // namespace autoware::motion_velocity_planner::obstacle_velocity_limiter diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.hpp index cb9fe40e64907..e098ee6abe822 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.hpp @@ -37,31 +37,11 @@ size_t calculateEndIndex( const TrajectoryPoints & trajectory, const size_t start_idx, const double max_length, const double max_duration); -/// @brief downsample a trajectory, reducing its number of points by the given factor -/// @param[in] trajectory input trajectory -/// @param[in] start_idx starting index of the input trajectory -/// @param[in] end_idx ending index of the input trajectory -/// @param[in] factor factor used for downsampling -/// @return downsampled trajectory -TrajectoryPoints downsampleTrajectory( - const TrajectoryPoints & trajectory, const size_t start_idx, const size_t end_idx, - const int factor); - /// @brief recalculate the steering angle of the trajectory /// @details uses the change in headings for calculation /// @param[inout] trajectory input trajectory /// @param[in] wheel_base wheel base of the vehicle void calculateSteeringAngles(TrajectoryPoints & trajectory, const double wheel_base); - -/// @brief copy the velocity profile of a downsampled trajectory to the original trajectory -/// @param[in] downsampled_trajectory downsampled trajectory -/// @param[in] trajectory input trajectory -/// @param[in] start_idx starting index of the downsampled trajectory relative to the input -/// @param[in] factor downsampling factor -/// @return input trajectory with the velocity profile of the downsampled trajectory -TrajectoryPoints copyDownsampledVelocity( - const TrajectoryPoints & downsampled_traj, TrajectoryPoints trajectory, const size_t start_idx, - const int factor); } // namespace autoware::motion_velocity_planner::obstacle_velocity_limiter #endif // TRAJECTORY_PREPROCESSING_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp index 18a37e74df9aa..8facb77932aa5 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -25,6 +25,7 @@ #include #include +#include #include #include #include @@ -57,8 +58,10 @@ void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name) node.create_publisher("~/" + ns_ + "/debug_markers", 1); virtual_wall_publisher_ = node.create_publisher("~/" + ns_ + "/virtual_walls", 1); - processing_time_publisher_ = std::make_shared( - &node, "~/debug/" + ns_ + "/processing_time_ms"); + processing_diag_publisher_ = std::make_shared( + &node, "~/debug/" + ns_ + "/processing_time_ms_diag"); + processing_time_publisher_ = node.create_publisher( + "~/debug/" + ns_ + "/processing_time_ms", 1); } void OutOfLaneModule::init_parameters(rclcpp::Node & node) { @@ -158,9 +161,11 @@ VelocityPlanningResult OutOfLaneModule::plan( stopwatch.tic(); out_of_lane::EgoData ego_data; ego_data.pose = planner_data->current_odometry.pose.pose; - ego_data.trajectory_points = ego_trajectory_points; - ego_data.first_trajectory_idx = + const auto start_idx = autoware::motion_utils::findNearestSegmentIndex(ego_trajectory_points, ego_data.pose.position); + ego_data.trajectory_points = + downsample_trajectory(ego_trajectory_points, start_idx, ego_trajectory_points.size(), 10); + ego_data.first_trajectory_idx = 0; ego_data.velocity = planner_data->current_odometry.twist.twist.linear.x; ego_data.max_decel = planner_data->velocity_smoother_->getMinDecel(); stopwatch.tic("calculate_trajectory_footprints"); @@ -306,7 +311,11 @@ VelocityPlanningResult OutOfLaneModule::plan( processing_times["calc_slowdown_points"] = calc_slowdown_points_us / 1000; processing_times["insert_slowdown_points"] = insert_slowdown_points_us / 1000; processing_times["Total"] = total_time_us / 1000; - processing_time_publisher_->publish(processing_times); + processing_diag_publisher_->publish(processing_times); + tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = clock_->now(); + processing_time_msg.data = processing_times["Total"]; + processing_time_publisher_->publish(processing_time_msg); return result; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp index bf0be64a0880d..9bd662af697ea 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp @@ -23,6 +23,7 @@ #include #include +#include #include #include @@ -45,7 +46,8 @@ class PluginModuleInterface rclcpp::Logger logger_ = rclcpp::get_logger(""); rclcpp::Publisher::SharedPtr debug_publisher_; rclcpp::Publisher::SharedPtr virtual_wall_publisher_; - std::shared_ptr processing_time_publisher_; + std::shared_ptr processing_diag_publisher_; + rclcpp::Publisher::SharedPtr processing_time_publisher_; autoware::motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator{}; }; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/trajectory_preprocessing.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/trajectory_preprocessing.hpp new file mode 100644 index 0000000000000..95728c4738c5b --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/trajectory_preprocessing.hpp @@ -0,0 +1,46 @@ +// Copyright 2024 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 AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__TRAJECTORY_PREPROCESSING_HPP_ +#define AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__TRAJECTORY_PREPROCESSING_HPP_ + +#include + +#include + +namespace autoware::motion_velocity_planner +{ +/// @brief downsample a trajectory, reducing its number of points by the given factor +/// @param[in] trajectory input trajectory +/// @param[in] start_idx starting index of the input trajectory +/// @param[in] end_idx ending index of the input trajectory +/// @param[in] factor factor used for downsampling +/// @return downsampled trajectory +std::vector downsample_trajectory( + const std::vector & trajectory, + const size_t start_idx, const size_t end_idx, const int factor) +{ + if (factor < 1) { + return trajectory; + } + std::vector downsampled_traj; + downsampled_traj.reserve((end_idx - start_idx) / factor + 1); + for (size_t i = start_idx; i <= end_idx; i += factor) { + downsampled_traj.push_back(trajectory[i]); + } + return downsampled_traj; +} +} // namespace autoware::motion_velocity_planner + +#endif // AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__TRAJECTORY_PREPROCESSING_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml index d2a3e305180d5..e428719f6e32f 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml @@ -27,6 +27,7 @@ lanelet2_extension libboost-dev rclcpp + tier4_debug_msgs tier4_planning_msgs visualization_msgs diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml index c00e0dd856200..aad25d5fb20a1 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml @@ -38,6 +38,7 @@ tf2_eigen tf2_geometry_msgs tf2_ros + tier4_debug_msgs tier4_planning_msgs visualization_msgs diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index da997dcfbc6e4..6578c5cfaca65 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -82,6 +82,8 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & velocity_factor_publisher_ = this->create_publisher( "~/output/velocity_factors", 1); + processing_time_publisher_ = this->create_publisher( + "~/debug/total_time/processing_time_ms", 1); // Parameters smooth_velocity_before_planning_ = declare_parameter("smooth_velocity_before_planning"); @@ -105,8 +107,6 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & std::bind(&MotionVelocityPlannerNode::on_set_param, this, std::placeholders::_1)); logger_configure_ = std::make_unique(this); - published_time_publisher_ = - std::make_unique(this); } void MotionVelocityPlannerNode::on_load_plugin( @@ -284,10 +284,14 @@ void MotionVelocityPlannerNode::on_trajectory( lk.unlock(); trajectory_pub_->publish(output_trajectory_msg); - published_time_publisher_->publish_if_subscribed( + published_time_publisher_.publish_if_subscribed( trajectory_pub_, output_trajectory_msg.header.stamp); processing_times["Total"] = stop_watch.toc("Total"); - processing_time_publisher_.publish(processing_times); + processing_diag_publisher_.publish(processing_times); + tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = get_clock()->now(); + processing_time_msg.data = processing_times["Total"]; + processing_time_publisher_->publish(processing_time_msg); } void MotionVelocityPlannerNode::insert_stop( diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp index 04e3e6b02c7b0..8debb9cbedf00 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp @@ -32,6 +32,7 @@ #include #include #include +#include #include #include @@ -96,7 +97,9 @@ class MotionVelocityPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_viz_pub_; rclcpp::Publisher::SharedPtr velocity_factor_publisher_; - autoware::universe_utils::ProcessingTimePublisher processing_time_publisher_{this}; + autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{this}; + rclcpp::Publisher::SharedPtr processing_time_publisher_; + autoware::universe_utils::PublishedTimePublisher published_time_publisher_{this}; // parameters rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr set_param_callback_; @@ -140,8 +143,6 @@ class MotionVelocityPlannerNode : public rclcpp::Node autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points); std::unique_ptr logger_configure_; - - std::unique_ptr published_time_publisher_; }; } // namespace autoware::motion_velocity_planner diff --git a/system/default_ad_api/src/interface.cpp b/system/default_ad_api/src/interface.cpp index d53d12afcc499..e0009d5bdc382 100644 --- a/system/default_ad_api/src/interface.cpp +++ b/system/default_ad_api/src/interface.cpp @@ -21,7 +21,7 @@ InterfaceNode::InterfaceNode(const rclcpp::NodeOptions & options) : Node("interf { const auto on_interface_version = [](auto, auto res) { res->major = 1; - res->minor = 3; + res->minor = 4; res->patch = 0; }; diff --git a/system/default_ad_api/test/node/interface_version.py b/system/default_ad_api/test/node/interface_version.py index 63a79e8722be3..10d00d1dbad30 100644 --- a/system/default_ad_api/test/node/interface_version.py +++ b/system/default_ad_api/test/node/interface_version.py @@ -30,7 +30,7 @@ if response.major != 1: exit(1) -if response.minor != 3: +if response.minor != 4: exit(1) if response.patch != 0: exit(1)