diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp index 5dd17ddc6974..b842261d56cf 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp @@ -17,6 +17,8 @@ #include +#include +#include #include namespace tier4_autoware_utils @@ -27,7 +29,7 @@ class InterProcessPollingSubscriber { private: typename rclcpp::Subscription::SharedPtr subscriber_; - std::optional data_; + typename T::SharedPtr data_; public: explicit InterProcessPollingSubscriber( @@ -43,33 +45,21 @@ class InterProcessPollingSubscriber [node]([[maybe_unused]] const typename T::ConstSharedPtr msg) { assert(false); }, noexec_subscription_options); if (qos.get_rmw_qos_profile().depth > 1) { - RCLCPP_WARN( - node->get_logger(), + throw std::invalid_argument( "InterProcessPollingSubscriber will be used with depth > 1, which may cause inefficient " "serialization while updateLatestData()"); } }; - bool updateLatestData() + typename T::ConstSharedPtr takeData() { + auto new_data = std::make_shared(); rclcpp::MessageInfo message_info; - T tmp; - // The queue size (QoS) must be 1 to get the last message data. - bool is_latest_message_consumed = false; - // pop the queue until latest data - while (subscriber_->take(tmp, message_info)) { - is_latest_message_consumed = true; + const bool success = subscriber_->take(*new_data, message_info); + if (success) { + data_ = new_data; } - if (is_latest_message_consumed) { - data_ = tmp; - } - return data_.has_value(); - }; - const T & getData() const - { - if (!data_.has_value()) { - throw std::runtime_error("Bad_optional_access in class InterProcessPollingSubscriber"); - } - return data_.value(); + + return data_; }; }; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index c368265ea0f6..a819f000412b 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -55,15 +55,19 @@ class ObstacleCruisePlannerNode : public rclcpp::Node const std::vector & traj_points, const vehicle_info_util::VehicleInfo & vehicle_info, const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin = 0.0) const; - std::vector convertToObstacles(const std::vector & traj_points) const; + std::vector convertToObstacles( + const Odometry & odometry, const PredictedObjects & objects, + const std::vector & traj_points) const; std::tuple, std::vector, std::vector> determineEgoBehaviorAgainstObstacles( + const Odometry & odometry, const PredictedObjects & objecrts, const std::vector & traj_points, const std::vector & obstacles); std::vector decimateTrajectoryPoints( - const std::vector & traj_points) const; + const Odometry & odometry, const std::vector & traj_points) const; std::optional createStopObstacle( - const std::vector & traj_points, const std::vector & traj_polys, - const Obstacle & obstacle, const double precise_lateral_dist) const; + const Odometry & odometry, const std::vector & traj_points, + const std::vector & traj_polys, const Obstacle & obstacle, + const double precise_lateral_dist) const; bool isStopObstacle(const uint8_t label) const; bool isInsideCruiseObstacle(const uint8_t label) const; bool isOutsideCruiseObstacle(const uint8_t label) const; @@ -88,12 +92,14 @@ class ObstacleCruisePlannerNode : public rclcpp::Node bool isObstacleCrossing( const std::vector & traj_points, const Obstacle & obstacle) const; double calcCollisionTimeMargin( - const std::vector & collision_points, + const Odometry & odometry, const std::vector & collision_points, const std::vector & traj_points, const bool is_driving_forward) const; std::optional createSlowDownObstacle( - const std::vector & traj_points, const Obstacle & obstacle, - const double precise_lat_dist); - PlannerData createPlannerData(const std::vector & traj_points) const; + const Odometry & odometry, const std::vector & traj_points, + const Obstacle & obstacle, const double precise_lat_dist); + PlannerData createPlannerData( + const Odometry & odometry, const AccelWithCovarianceStamped & acc, + const std::vector & traj_points) const; void checkConsistency( const rclcpp::Time & current_time, const PredictedObjects & predicted_objects, diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 43400acb86bd..94e9707d55c4 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -484,12 +484,17 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr msg) { - if ( - !ego_odom_sub_.updateLatestData() || !objects_sub_.updateLatestData() || - !acc_sub_.updateLatestData()) { + const auto ego_odom_ptr = ego_odom_sub_.takeData(); + const auto objects_ptr = objects_sub_.takeData(); + const auto acc_ptr = acc_sub_.takeData(); + if (!ego_odom_ptr || !objects_ptr || !acc_ptr) { return; } + const auto & ego_odom = *ego_odom_ptr; + const auto & objects = *objects_ptr; + const auto & acc = *acc_ptr; + const auto traj_points = motion_utils::convertToTrajectoryPointArray(*msg); // check if subscribed variables are ready if (traj_points.empty()) { @@ -506,14 +511,14 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms // (1) with a proper label // (2) in front of ego // (3) not too far from trajectory - const auto target_obstacles = convertToObstacles(traj_points); + const auto target_obstacles = convertToObstacles(ego_odom, objects, traj_points); // 2. Determine ego's behavior against each obstacle from stop, cruise and slow down. const auto & [stop_obstacles, cruise_obstacles, slow_down_obstacles] = - determineEgoBehaviorAgainstObstacles(traj_points, target_obstacles); + determineEgoBehaviorAgainstObstacles(ego_odom, objects, traj_points, target_obstacles); // 3. Create data for planning - const auto planner_data = createPlannerData(traj_points); + const auto planner_data = createPlannerData(ego_odom, acc, traj_points); // 4. Stop planning const auto stop_traj_points = planner_ptr_->generateStopTrajectory(planner_data, stop_obstacles); @@ -629,15 +634,16 @@ std::vector ObstacleCruisePlannerNode::createOneStepPolygons( } std::vector ObstacleCruisePlannerNode::convertToObstacles( + const Odometry & odometry, const PredictedObjects & objects, const std::vector & traj_points) const { stop_watch_.tic(__func__); - const auto obj_stamp = rclcpp::Time(objects_sub_.getData().header.stamp); + const auto obj_stamp = rclcpp::Time(objects.header.stamp); const auto & p = behavior_determination_param_; std::vector target_obstacles; - for (const auto & predicted_object : objects_sub_.getData().objects) { + for (const auto & predicted_object : objects.objects) { const auto & object_id = tier4_autoware_utils::toHexString(predicted_object.object_id).substr(0, 4); const auto & current_obstacle_pose = @@ -655,8 +661,7 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( } // 2. Check if the obstacle is in front of the ego. - const size_t ego_idx = - ego_nearest_param_.findIndex(traj_points, ego_odom_sub_.getData().pose.pose); + const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, odometry.pose.pose); const auto ego_to_obstacle_distance = calcDistanceToFrontVehicle(traj_points, ego_idx, current_obstacle_pose.pose.position); if (!ego_to_obstacle_distance) { @@ -745,14 +750,15 @@ bool ObstacleCruisePlannerNode::isFrontCollideObstacle( std::tuple, std::vector, std::vector> ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( + const Odometry & odometry, const PredictedObjects & objects, const std::vector & traj_points, const std::vector & obstacles) { stop_watch_.tic(__func__); // calculated decimated trajectory points and trajectory polygon - const auto decimated_traj_points = decimateTrajectoryPoints(traj_points); + const auto decimated_traj_points = decimateTrajectoryPoints(odometry, traj_points); const auto decimated_traj_polys = - createOneStepPolygons(decimated_traj_points, vehicle_info_, ego_odom_sub_.getData().pose.pose); + createOneStepPolygons(decimated_traj_points, vehicle_info_, odometry.pose.pose); debug_data_ptr_->detection_polygons = decimated_traj_polys; // determine ego's behavior from stop, cruise and slow down @@ -777,14 +783,14 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( cruise_obstacles.push_back(*cruise_obstacle); continue; } - const auto stop_obstacle = - createStopObstacle(decimated_traj_points, decimated_traj_polys, obstacle, precise_lat_dist); + const auto stop_obstacle = createStopObstacle( + odometry, decimated_traj_points, decimated_traj_polys, obstacle, precise_lat_dist); if (stop_obstacle) { stop_obstacles.push_back(*stop_obstacle); continue; } const auto slow_down_obstacle = - createSlowDownObstacle(decimated_traj_points, obstacle, precise_lat_dist); + createSlowDownObstacle(odometry, decimated_traj_points, obstacle, precise_lat_dist); if (slow_down_obstacle) { slow_down_obstacles.push_back(*slow_down_obstacle); continue; @@ -810,7 +816,7 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( slow_down_condition_counter_.removeCounterUnlessUpdated(); // Check target obstacles' consistency - checkConsistency(objects_sub_.getData().header.stamp, objects_sub_.getData(), stop_obstacles); + checkConsistency(objects.header.stamp, objects, stop_obstacles); // update previous obstacles prev_stop_obstacles_ = stop_obstacles; @@ -826,13 +832,12 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( } std::vector ObstacleCruisePlannerNode::decimateTrajectoryPoints( - const std::vector & traj_points) const + const Odometry & odometry, const std::vector & traj_points) const { const auto & p = behavior_determination_param_; // trim trajectory - const size_t ego_seg_idx = - ego_nearest_param_.findSegmentIndex(traj_points, ego_odom_sub_.getData().pose.pose); + const size_t ego_seg_idx = ego_nearest_param_.findSegmentIndex(traj_points, odometry.pose.pose); const size_t traj_start_point_idx = ego_seg_idx; const auto trimmed_traj_points = std::vector(traj_points.begin() + traj_start_point_idx, traj_points.end()); @@ -1151,8 +1156,9 @@ ObstacleCruisePlannerNode::createCollisionPointsForOutsideCruiseObstacle( } std::optional ObstacleCruisePlannerNode::createStopObstacle( - const std::vector & traj_points, const std::vector & traj_polys, - const Obstacle & obstacle, const double precise_lat_dist) const + const Odometry & odometry, const std::vector & traj_points, + const std::vector & traj_polys, const Obstacle & obstacle, + const double precise_lat_dist) const { const auto & p = behavior_determination_param_; const auto & object_id = obstacle.uuid.substr(0, 4); @@ -1203,7 +1209,7 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( } const double collision_time_margin = - calcCollisionTimeMargin(collision_points, traj_points, is_driving_forward_); + calcCollisionTimeMargin(odometry, collision_points, traj_points, is_driving_forward_); if (p.collision_time_margin < collision_time_margin) { RCLCPP_INFO_EXPRESSION( get_logger(), enable_debug_info_, @@ -1216,7 +1222,7 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( // calculate collision points with trajectory with lateral stop margin const auto traj_polys_with_lat_margin = createOneStepPolygons( - traj_points, vehicle_info_, ego_odom_sub_.getData().pose.pose, p.max_lat_margin_for_stop); + traj_points, vehicle_info_, odometry.pose.pose, p.max_lat_margin_for_stop); const auto collision_point = polygon_utils::getCollisionPoint( traj_points, traj_polys_with_lat_margin, obstacle, is_driving_forward_, vehicle_info_); @@ -1231,8 +1237,8 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( } std::optional ObstacleCruisePlannerNode::createSlowDownObstacle( - const std::vector & traj_points, const Obstacle & obstacle, - const double precise_lat_dist) + const Odometry & odometry, const std::vector & traj_points, + const Obstacle & obstacle, const double precise_lat_dist) { const auto & object_id = obstacle.uuid.substr(0, 4); const auto & p = behavior_determination_param_; @@ -1286,7 +1292,7 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl // calculate collision points with trajectory with lateral stop margin // NOTE: For additional margin, hysteresis is not divided by two. const auto traj_polys_with_lat_margin = createOneStepPolygons( - traj_points, vehicle_info_, ego_odom_sub_.getData().pose.pose, + traj_points, vehicle_info_, odometry.pose.pose, p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down); std::vector front_collision_polygons; @@ -1446,11 +1452,11 @@ bool ObstacleCruisePlannerNode::isObstacleCrossing( } double ObstacleCruisePlannerNode::calcCollisionTimeMargin( - const std::vector & collision_points, + const Odometry & odometry, const std::vector & collision_points, const std::vector & traj_points, const bool is_driving_forward) const { - const auto & ego_pose = ego_odom_sub_.getData().pose.pose; - const double ego_vel = ego_odom_sub_.getData().twist.twist.linear.x; + const auto & ego_pose = odometry.pose.pose; + const double ego_vel = odometry.twist.twist.linear.x; const double time_to_reach_collision_point = [&]() { const double abs_ego_offset = is_driving_forward @@ -1477,14 +1483,15 @@ double ObstacleCruisePlannerNode::calcCollisionTimeMargin( } PlannerData ObstacleCruisePlannerNode::createPlannerData( + const Odometry & odometry, const AccelWithCovarianceStamped & acc, const std::vector & traj_points) const { PlannerData planner_data; planner_data.current_time = now(); planner_data.traj_points = traj_points; - planner_data.ego_pose = ego_odom_sub_.getData().pose.pose; - planner_data.ego_vel = ego_odom_sub_.getData().twist.twist.linear.x; - planner_data.ego_acc = acc_sub_.getData().accel.accel.linear.x; + planner_data.ego_pose = odometry.pose.pose; + planner_data.ego_vel = odometry.twist.twist.linear.x; + planner_data.ego_acc = acc.accel.accel.linear.x; planner_data.is_driving_forward = is_driving_forward_; return planner_data; }