feat: check vehicle path

src/autoware_practice_evaluator/launch/evaluator.launch.xml

@@ -3,5 +3,6 @@
   <node pkg="autoware_practice_evaluator" exec="evaluator" name="evaluator">
     <param name="judge_file" value="$(var judge_file)"/>
     <remap from="~/pose" to="/simulator/ground_truth/pose"/>
+    <remap from="~/path" to="/simulator/ground_truth/path"/>
   </node>
 </launch>

src/autoware_practice_evaluator/package.xml

@@ -12,6 +12,7 @@
   <depend>autoware_practice_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>libboost-dev</depend>
+  <depend>nav_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>visualization_msgs</depend>

src/autoware_practice_evaluator/src/condition.cpp

@@ -14,7 +14,7 @@
 
 #include "condition.hpp"
 
-#include <boost/geometry/algorithms/within.hpp>
+#include <boost/geometry/algorithms/intersects.hpp>
 
 #include <algorithm>
 #include <string>
@@ -143,7 +143,7 @@ SuccessArea::SuccessArea(YAML::Node yaml)
 
 TriState SuccessArea::update(const JudgeInput & data)
 {
-  return boost::geometry::within(data.point, area_) ? TriState::Success : TriState::Judging;
+  return boost::geometry::intersects(data.path, area_) ? TriState::Success : TriState::Judging;
 }
 
 std::vector<RvizMarker> SuccessArea::visualize()
@@ -161,7 +161,7 @@ FailureArea::FailureArea(YAML::Node yaml)
 
 TriState FailureArea::update(const JudgeInput & data)
 {
-  return boost::geometry::within(data.point, area_) ? TriState::Failure : TriState::Judging;
+  return boost::geometry::intersects(data.path, area_) ? TriState::Failure : TriState::Judging;
 }
 
 std::vector<RvizMarker> FailureArea::visualize()

src/autoware_practice_evaluator/src/condition.hpp

@@ -38,6 +38,7 @@ enum class TriState : int
 struct JudgeInput
 {
   Point point;
+  LineString path;
 };
 
 class Condition

src/autoware_practice_evaluator/src/evaluator.cpp

@@ -23,6 +23,7 @@ Evaluator::Evaluator(const rclcpp::NodeOptions & options) : Node("evaluator", op
 {
   using std::placeholders::_1;
   sub_pose_ = create_subscription<PoseStamped>("~/pose", rclcpp::QoS(1), std::bind(&Evaluator::on_pose, this, _1));
+  sub_path_ = create_subscription<VehiclePath>("~/path", rclcpp::QoS(1), std::bind(&Evaluator::on_path, this, _1));
   pub_result_ = create_publisher<JudgeStatus>("~/result", rclcpp::QoS(1));
   pub_markers_ = create_publisher<MarkerArray>("~/markers", rclcpp::QoS(1));
 
@@ -38,6 +39,19 @@ void Evaluator::on_pose(const PoseStamped & msg)
 {
   data_.point.x = msg.pose.position.x;
   data_.point.y = msg.pose.position.y;
+  data_.point.z = msg.pose.position.z;
+}
+
+void Evaluator::on_path(const VehiclePath & msg)
+{
+  data_.path.clear();
+  for (const auto & pose : msg.poses) {
+    Point point;
+    point.x = pose.pose.position.x;
+    point.y = pose.pose.position.y;
+    point.z = pose.pose.position.z;
+    data_.path.push_back(point);
+  }
 }
 
 void Evaluator::on_timer()

src/autoware_practice_evaluator/src/evaluator.hpp

@@ -20,6 +20,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <autoware_practice_msgs/msg/judge_status.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
+#include <nav_msgs/msg/path.hpp>
 
 #include <memory>
 #include <vector>
@@ -34,13 +35,16 @@ class Evaluator : public rclcpp::Node
 
 private:
   using PoseStamped = geometry_msgs::msg::PoseStamped;
+  using VehiclePath = nav_msgs::msg::Path;
   using JudgeStatus = autoware_practice_msgs::msg::JudgeStatus;
 
   void on_pose(const PoseStamped & msg);
+  void on_path(const VehiclePath & msg);
   void on_timer();
 
   rclcpp::TimerBase::SharedPtr timer_;
   rclcpp::Subscription<PoseStamped>::SharedPtr sub_pose_;
+  rclcpp::Subscription<VehiclePath>::SharedPtr sub_path_;
   rclcpp::Publisher<JudgeStatus>::SharedPtr pub_result_;
   rclcpp::Publisher<MarkerArray>::SharedPtr pub_markers_;
 

src/autoware_practice_evaluator/src/geometry.hpp

@@ -16,6 +16,7 @@
 #define GEOMETRY_HPP_
 
 #include <boost/geometry/geometries/geometries.hpp>
+#include <boost/geometry/geometries/register/linestring.hpp>
 #include <boost/geometry/geometries/register/point.hpp>
 #include <boost/geometry/geometries/register/ring.hpp>
 #include <geometry_msgs/msg/point.hpp>
@@ -26,13 +27,20 @@ namespace autoware_practice_evaluator
 {
 
 using Point = geometry_msgs::msg::Point;
-using Polygon = std::vector<Point>;
-
 Point Point2(double x, double y);
 
+struct Polygon : public std::vector<Point>
+{
+};
+
+struct LineString : public std::vector<Point>
+{
+};
+
 }  // namespace autoware_practice_evaluator
 
-BOOST_GEOMETRY_REGISTER_POINT_3D(autoware_practice_evaluator::Point, double, boost::geometry::cs::cartesian, x, y, z)  // NOLINT
-BOOST_GEOMETRY_REGISTER_RING(autoware_practice_evaluator::Polygon)                                                     // NOLINT
+BOOST_GEOMETRY_REGISTER_POINT_2D(autoware_practice_evaluator::Point, double, boost::geometry::cs::cartesian, x, y)  // NOLINT
+BOOST_GEOMETRY_REGISTER_RING(autoware_practice_evaluator::Polygon)                                                  // NOLINT
+BOOST_GEOMETRY_REGISTER_LINESTRING(autoware_practice_evaluator::LineString)                                         // NOLINT
 
 #endif  // GEOMETRY_HPP_

src/autoware_practice_simulator/config/default.param.yaml

@@ -1,4 +1,4 @@
 /**:
   ros__parameters:
     rate: 10.0
-    resolution: 60.0
+    time_resolution: 0.02

src/autoware_practice_simulator/package.xml

@@ -12,6 +12,7 @@
   <depend>autoware_auto_control_msgs</depend>
   <depend>autoware_auto_vehicle_msgs</depend>
   <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>tf2</depend>

src/autoware_practice_simulator/src/simulator.cpp

@@ -49,33 +49,55 @@ Simulator::Simulator(const rclcpp::NodeOptions & options) : Node("simulator", op
   // Init ROS interface.
   {
     tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
-    pub_pose_ = create_publisher<PoseStamped>("~/ground_truth/pose", rclcpp::QoS(1));
     pub_markers_ = create_publisher<MarkerArray>("~/markers", rclcpp::QoS(1));
+    pub_pose_ = create_publisher<PoseStamped>("~/ground_truth/pose", rclcpp::QoS(1));
+    pub_path_ = create_publisher<VehiclePath>("~/ground_truth/path", rclcpp::QoS(1));
   }
 
-  // Init simulation timer.
+  // Init simulation settings.
   {
-    rate_sim_ = 20.0;
-    rate_pub_ = 10.0;
-    timer_sim_ = rclcpp::create_timer(this, get_clock(), rclcpp::Rate(rate_sim_).period(), [this] { on_timer_sim(); });
-    timer_pub_ = rclcpp::create_timer(this, get_clock(), rclcpp::Rate(rate_pub_).period(), [this] { on_timer_pub(); });
+    time_resolution_ = declare_parameter<double>("time_resolution");
+    last_time_ = now();
   }
-}
 
-void Simulator::on_timer_sim()
-{
-  const auto dt = 1.0 / rate_sim_;
-  controller_->update(dt, kinematics_->state());
-  kinematics_->update(dt, controller_->input());
+  // Init simulation timer.
+  {
+    const auto period = rclcpp::Rate(declare_parameter<double>("rate")).period();
+    timer_ = rclcpp::create_timer(this, get_clock(), period, [this] { on_timer(); });
+  }
 }
 
-void Simulator::on_timer_pub()
+void Simulator::on_timer()
 {
   const auto stamp = now();
+  execute(stamp);
   publish(stamp);
   controller_->publish(stamp);
 }
 
+void Simulator::execute(const rclcpp::Time & stamp)
+{
+  const auto create_pose_stamped = [](const rclcpp::Time & stamp, const VehicleState & state)
+  {
+    PoseStamped msg;
+    msg.header.stamp = stamp;
+    msg.header.frame_id = "map";
+    msg.pose.position.x = state.point.x;
+    msg.pose.position.y = state.point.y;
+    msg.pose.position.z = 0.0;
+    msg.pose.orientation = yaw_to_quaternion(state.angle);
+    return msg;
+  };
+
+  path_.clear();
+  while (last_time_ < stamp) {
+    controller_->update(time_resolution_, kinematics_->state());
+    kinematics_->update(time_resolution_, controller_->input());
+    path_.push_back(create_pose_stamped(last_time_, kinematics_->state()));
+    last_time_ += rclcpp::Duration::from_seconds(time_resolution_);
+  }
+}
+
 void Simulator::publish(const rclcpp::Time & stamp)
 {
   const auto specs = kinematics_->specs();
@@ -107,6 +129,15 @@ void Simulator::publish(const rclcpp::Time & stamp)
     pub_pose_->publish(msg);
   }
 
+  // Vehicle path.
+  {
+    VehiclePath msg;
+    msg.header.stamp = stamp;
+    msg.header.frame_id = "map";
+    msg.poses = path_;
+    pub_path_->publish(msg);
+  }
+
   // Vehicle markers.
   {
     const auto base_x = specs.overhang[0] + specs.wheel_base;

src/autoware_practice_simulator/src/simulator.hpp

@@ -19,39 +19,43 @@
 #include "kinematics.hpp"
 
 #include <rclcpp/rclcpp.hpp>
+#include <nav_msgs/msg/path.hpp>
 #include <visualization_msgs/msg/marker.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
 #include <tf2_ros/transform_broadcaster.h>
 
 #include <memory>
+#include <vector>
 
 namespace autoware_practice_simulator
 {
 
-using visualization_msgs::msg::Marker;
-using visualization_msgs::msg::MarkerArray;
-
 class Simulator : public rclcpp::Node
 {
 public:
   explicit Simulator(const rclcpp::NodeOptions & options);
 
 private:
-  void on_timer_sim();
-  void on_timer_pub();
+  using Marker = visualization_msgs::msg::Marker;
+  using MarkerArray = visualization_msgs::msg::MarkerArray;
+  using VehiclePath = nav_msgs::msg::Path;
+
+  void on_timer();
+  void execute(const rclcpp::Time & stamp);
   void publish(const rclcpp::Time & stamp);
 
   std::unique_ptr<VehicleKinematics> kinematics_;
   std::unique_ptr<VehicleController> controller_;
   std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
-  rclcpp::TimerBase::SharedPtr timer_sim_;
-  rclcpp::TimerBase::SharedPtr timer_pub_;
-  rclcpp::Publisher<PoseStamped>::SharedPtr pub_pose_;
+  rclcpp::TimerBase::SharedPtr timer_;
   rclcpp::Publisher<MarkerArray>::SharedPtr pub_markers_;
+  rclcpp::Publisher<PoseStamped>::SharedPtr pub_pose_;
+  rclcpp::Publisher<VehiclePath>::SharedPtr pub_path_;
+  rclcpp::Time last_time_;
+  double time_resolution_;
 
-  double rate_sim_;
-  double rate_pub_;
+  std::vector<PoseStamped> path_;
 };
 
 }  // namespace autoware_practice_simulator