refactor(map_based_prediction): move member functions to utils (autow…

…arefoundation#9225)

Signed-off-by: Mamoru Sobue <[email protected]>

perception/autoware_map_based_prediction/include/map_based_prediction/data_structure.hpp

@@ -15,17 +15,24 @@
 #ifndef MAP_BASED_PREDICTION__DATA_STRUCTURE_HPP_
 #define MAP_BASED_PREDICTION__DATA_STRUCTURE_HPP_
 
+#include <autoware/universe_utils/system/stop_watch.hpp>
+
+#include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
+#include <autoware_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_perception_msgs/msg/tracked_object.hpp>
+#include <autoware_perception_msgs/msg/tracked_objects.hpp>
+#include <autoware_perception_msgs/msg/traffic_light_group_array.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/twist.hpp>
 #include <std_msgs/msg/header.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 #include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_routing/LaneletPath.h>
 
 #include <unordered_map>
+#include <utility>
 #include <vector>
-
 namespace autoware::map_based_prediction
 {
 using PosePath = std::vector<geometry_msgs::msg::Pose>;
@@ -58,7 +65,7 @@ enum class Maneuver {
 struct LaneletData
 {
   lanelet::Lanelet lanelet;
-  float probability;
+  double probability;
 };
 
 struct PredictedRefPath
@@ -90,6 +97,23 @@ struct CrosswalkUserData
   autoware_perception_msgs::msg::TrackedObject tracked_object;
 };
 
+using LaneletsData = std::vector<LaneletData>;
+using ManeuverProbability = std::unordered_map<Maneuver, float>;
+using autoware::universe_utils::StopWatch;
+using autoware_map_msgs::msg::LaneletMapBin;
+using autoware_perception_msgs::msg::ObjectClassification;
+using autoware_perception_msgs::msg::PredictedObject;
+using autoware_perception_msgs::msg::PredictedObjectKinematics;
+using autoware_perception_msgs::msg::PredictedObjects;
+using autoware_perception_msgs::msg::PredictedPath;
+using autoware_perception_msgs::msg::TrackedObject;
+using autoware_perception_msgs::msg::TrackedObjectKinematics;
+using autoware_perception_msgs::msg::TrackedObjects;
+using autoware_perception_msgs::msg::TrafficLightElement;
+using autoware_perception_msgs::msg::TrafficLightGroup;
+using autoware_perception_msgs::msg::TrafficLightGroupArray;
+using LaneletPathWithPathInfo = std::pair<lanelet::routing::LaneletPath, PredictedRefPath>;
+
 }  // namespace autoware::map_based_prediction
 
 #endif  // MAP_BASED_PREDICTION__DATA_STRUCTURE_HPP_

perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp

@@ -25,16 +25,10 @@
 #include <autoware/universe_utils/ros/published_time_publisher.hpp>
 #include <autoware/universe_utils/ros/transform_listener.hpp>
 #include <autoware/universe_utils/ros/update_param.hpp>
-#include <autoware/universe_utils/ros/uuid_helper.hpp>
 #include <autoware/universe_utils/system/lru_cache.hpp>
-#include <autoware/universe_utils/system/stop_watch.hpp>
 #include <autoware/universe_utils/system/time_keeper.hpp>
 #include <rclcpp/rclcpp.hpp>
 
-#include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
-#include <autoware_perception_msgs/msg/predicted_objects.hpp>
-#include <autoware_perception_msgs/msg/tracked_objects.hpp>
-#include <autoware_perception_msgs/msg/traffic_light_group_array.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/twist.hpp>
@@ -77,25 +71,10 @@ struct hash<lanelet::routing::LaneletPath>
 }  // namespace std
 namespace autoware::map_based_prediction
 {
-using LaneletsData = std::vector<LaneletData>;
-using ManeuverProbability = std::unordered_map<Maneuver, float>;
-using autoware::universe_utils::StopWatch;
-using autoware_map_msgs::msg::LaneletMapBin;
-using autoware_perception_msgs::msg::ObjectClassification;
-using autoware_perception_msgs::msg::PredictedObject;
-using autoware_perception_msgs::msg::PredictedObjectKinematics;
-using autoware_perception_msgs::msg::PredictedObjects;
-using autoware_perception_msgs::msg::PredictedPath;
-using autoware_perception_msgs::msg::TrackedObject;
-using autoware_perception_msgs::msg::TrackedObjectKinematics;
-using autoware_perception_msgs::msg::TrackedObjects;
-using autoware_perception_msgs::msg::TrafficLightElement;
-using autoware_perception_msgs::msg::TrafficLightGroup;
-using autoware_perception_msgs::msg::TrafficLightGroupArray;
 using autoware_planning_msgs::msg::TrajectoryPoint;
 using tier4_debug_msgs::msg::StringStamped;
 using TrajectoryPoints = std::vector<TrajectoryPoint>;
-using LaneletPathWithPathInfo = std::pair<lanelet::routing::LaneletPath, PredictedRefPath>;
+
 class MapBasedPredictionNode : public rclcpp::Node
 {
 public:
@@ -202,10 +181,6 @@ class MapBasedPredictionNode : public rclcpp::Node
   //// Vehicle process
   // Lanelet process
   LaneletsData getCurrentLanelets(const TrackedObject & object);
-  bool checkCloseLaneletCondition(
-    const std::pair<double, lanelet::Lanelet> & lanelet, const TrackedObject & object);
-  float calculateLocalLikelihood(
-    const lanelet::Lanelet & current_lanelet, const TrackedObject & object) const;
   bool isDuplicated(
     const std::pair<double, lanelet::ConstLanelet> & target_lanelet,
     const LaneletsData & lanelets_data);

perception/autoware_map_based_prediction/include/map_based_prediction/utils.hpp

@@ -30,6 +30,7 @@
 #include <tf2/utils.h>
 
 #include <deque>
+#include <memory>
 #include <string>
 #include <unordered_map>
 #include <unordered_set>
@@ -94,6 +95,39 @@ PredictedObjectKinematics convertToPredictedKinematics(
 
 PredictedObject convertToPredictedObject(const TrackedObject & tracked_object);
 
+double calculateLocalLikelihood(
+  const lanelet::Lanelet & current_lanelet, const TrackedObject & object,
+  const double sigma_lateral_offset, const double sigma_yaw_angle_deg);
+
+bool isDuplicated(
+  const std::pair<double, lanelet::ConstLanelet> & target_lanelet,
+  const LaneletsData & lanelets_data);
+
+bool isDuplicated(
+  const PredictedPath & predicted_path, const std::vector<PredictedPath> & predicted_paths);
+
+bool checkCloseLaneletCondition(
+  const std::pair<double, lanelet::Lanelet> & lanelet, const TrackedObject & object,
+  const std::unordered_map<std::string, std::deque<ObjectData>> & road_users_history,
+  const double dist_threshold_for_searching_lanelet,
+  const double delta_yaw_threshold_for_searching_lanelet);
+
+// NOTE: These two functions are copied from the route_handler package.
+lanelet::Lanelets getRightOppositeLanelets(
+  const std::shared_ptr<lanelet::LaneletMap> & lanelet_map_ptr,
+  const lanelet::ConstLanelet & lanelet);
+
+lanelet::Lanelets getLeftOppositeLanelets(
+  const std::shared_ptr<lanelet::LaneletMap> & lanelet_map_ptr,
+  const lanelet::ConstLanelet & lanelet);
+
+LaneletsData getCurrentLanelets(
+  const TrackedObject & object, lanelet::LaneletMapPtr lanelet_map_ptr,
+  const std::unordered_map<std::string, std::deque<ObjectData>> & road_users_history,
+  const double dist_threshold_for_searching_lanelet,
+  const double delta_yaw_threshold_for_searching_lanelet, const double sigma_lateral_offset,
+  const double sigma_yaw_angle_deg);
+
 }  // namespace utils
 
 }  // namespace autoware::map_based_prediction