Collision detection

packages/geom/include/geom/bounding_box.h

@@ -0,0 +1,12 @@
+#pragma once
+
+#include "geom/vector.h"
+
+namespace truck::geom {
+
+struct BoundingBox {
+    Vec2 min;
+    Vec2 max;
+};
+
+}  // namespace truck::geom

packages/geom/include/geom/polygon.h

@@ -1,8 +1,9 @@
 #pragma once
 
-#include "geom/vector.h"
-#include "geom/triangle.h"
+#include "geom/bounding_box.h"
 #include "geom/segment.h"
+#include "geom/triangle.h"
+#include "geom/vector.h"
 
 #include <vector>
 
@@ -28,6 +29,7 @@ struct Polygon : public std::vector<Vec2> {
     bool isConvex() const noexcept;
     Orientation orientation() const noexcept;
     Segments segments() const noexcept;
+    BoundingBox minBoundingBox() const noexcept;
 };
 
 using Polygons = std::vector<Polygon>;

packages/geom/src/polygon.cpp

@@ -139,7 +139,7 @@ Segments Polygon::segments() const noexcept {
     segments[0].begin = {points.back().x, points.back().y};
     segments[0].end = {points[0].x, points[0].y};
 
-    for (size_t i = 1; i < points.size(); ++i) {
+    for (auto i = 1; i < points.size(); ++i) {
         Vec2 begin = {points[i - 1].x, points[i - 1].y};
         Vec2 end = {points[i].x, points[i].y};
         segments.emplace_back(begin, end);
@@ -148,4 +148,31 @@ Segments Polygon::segments() const noexcept {
     return segments;
 }
 
+BoundingBox Polygon::minBoundingBox() const noexcept {
+    const auto points = *this;
+
+    geom::Vec2 min = points[0];
+    geom::Vec2 max = points[0];
+
+    for (auto i = 1; i < points.size(); ++i) {
+        if (points[i].x < min.x) {
+            min.x = points[i].x;
+        }
+
+        if (points[i].y < min.y) {
+            min.y = points[i].y;
+        }
+
+        if (points[i].x > max.x) {
+            max.x = points[i].x;
+        }
+
+        if (points[i].y > max.y) {
+            max.y = points[i].y;
+        }
+    }
+
+    return {min, max};
+}
+
 }  // namespace truck::geom

packages/model/config/model.yaml

@@ -43,6 +43,11 @@ tf_static:
     translation: { x: 0.25, y: 0.05, z: -0.04 }
     rotation: { x: 0.0, y: 0.0, z: 0.0, w: 1.0 }
 
+  - frame_id: "base"
+    child_frame_id: "rear_axle"
+    translation: { x: 0.2025, y: 0.0, z: 0.0 }
+    rotation: { x: 0.0, y: 0.0, z: 0.0, w: 1.0 }
+
   - frame_id: "camera_link"
     child_frame_id: "camera_gyro_frame"
     translation: { x: -0.01602, y: -0.03022, z: 0.0074 }

packages/model/include/model/model.h

@@ -2,6 +2,7 @@
 
 #include "common/math.h"
 #include "geom/angle.h"
+#include "geom/polygon.h"
 #include "model/params.h"
 
 #include <tf2_ros/buffer.h>
@@ -90,6 +91,8 @@ class Model {
     WheelVelocity rearTwistToWheelVelocity(Twist rear_twist) const;
     double linearVelocityToMotorRPS(double velocity) const;
     double motorRPStoLinearVelocity(double rps) const;
+    geom::Polygon rearPoseToShapePolygon(const geom::Pose rear_pose) const;
+    geom::Polygon basePoseToShapePolygon(const geom::Pose base_pose) const;
 
     tf2_msgs::msg::TFMessage getTfStaticMsg() const;
     tf2::Transform getLatestTranform(const std::string& source, const std::string& target) const;

packages/model/src/model.cpp

@@ -173,6 +173,25 @@ double Model::motorRPStoLinearVelocity(double rps) const {
     return rps * params_.wheel.radius * M_PI * params_.gear_ratio * 2;
 }
 
+geom::Polygon Model::rearPoseToShapePolygon(const geom::Pose rear_pose) const {
+    const auto x = rear_pose.pos.x;
+    const auto y = rear_pose.pos.y;
+    const auto yaw = rear_pose.dir.vec();
+
+    const auto dir = params_.shape.length * yaw;
+    const auto norm = params_.shape.width / 2 * yaw;
+    const geom::Vec2 a(x - norm.y, y + norm.x);
+    const geom::Vec2 b(x + norm.y, y - norm.x);
+    return {a, a + dir, b + dir, b};
+}
+
+geom::Polygon Model::basePoseToShapePolygon(const geom::Pose base_pose) const {
+    const geom::Pose rear_pose = {
+        base_pose.pos - params_.wheel_base.base_to_rear * base_pose.dir, base_pose.dir};
+
+    return rearPoseToShapePolygon(rear_pose);
+}
+
 double Model::gearRatio() const { return params_.gear_ratio; }
 
 const Shape& Model::shape() const { return params_.shape; }

packages/simulator_2d/CMakeLists.txt

@@ -14,12 +14,14 @@ find_package(rosgraph_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(nlohmann_json REQUIRED)
 find_package(map REQUIRED)
+find_package(Boost REQUIRED)
 
 add_library(
   simulator_engine SHARED src/simulator_engine.cpp src/truck_state.cpp
+                          src/simulation_map.cpp
 )
 
-ament_target_dependencies(simulator_engine model geom map tf2)
+ament_target_dependencies(simulator_engine model geom map tf2 Boost)
 
 target_include_directories(
   simulator_engine

packages/simulator_2d/README.md

@@ -7,13 +7,13 @@ The calculations are based on the [Ackeramnn model](../../doc/ackermann_vehicle.
 
 | Name | Description | Arguments | Return type |
 | --- | --- | --- | --- |
-| **resetBase** | Sets the model center state | const geom::Pose& pose, double middle_steering, double linear_velocity | void |
-| **resetMap** | Sets the model center state | const std::string& path | void |
-| **eraseMap** | Sets the model center state |  | void |
-| **getTruckState** | Sets the model center state |  | TruckState |
-| **setBaseControl** | Sets the model center state | double velocity, double acceleration, double curvature | void |
-| **setBaseControl** | Sets the model center state | double velocity, double curvature | void |
-| **advance** | Sets the model center state | double seconds = 1.0 | void |
+| **resetBase** | Sets the model center state, resets the simulation state | const geom::Pose& pose, double middle_steering, double linear_velocity | void |
+| **resetMap** | Sets the obstacle map, resets the simulation state | const std::string& path | void |
+| **eraseMap** | Clears the obstacle map, resets the simulation state |  | void |
+| **getTruckState** | Returns the simulation state |  | TruckState |
+| **setBaseControl** | Sets the control command | double velocity, double acceleration, double curvature | void |
+| **setBaseControl** | Sets the control command and selects the acceleration | double velocity, double curvature | void |
+| **advance** | Advances the simulation for a given time | double seconds = 1.0 | void |
 
 ## Truck State API
 

packages/simulator_2d/include/simulator_2d/simulation_map.h

@@ -0,0 +1,44 @@
+#include "geom/pose.h"
+#include "geom/polygon.h"
+#include "geom/segment.h"
+#include "model/model.h"
+
+#include <boost/geometry.hpp>
+
+#include <string>
+#include <vector>
+
+namespace truck::simulator {
+
+namespace bg = boost::geometry;
+namespace bgi = boost::geometry::index;
+
+using RTreePoint = bg::model::point<double, 2, bg::cs::cartesian>;
+using RTreeSegment = bg::model::segment<RTreePoint>;
+using RTreeBox = bg::model::box<RTreePoint>;
+using RTreeIndexedSegment = std::pair<RTreeSegment, size_t>;
+using RTreeIndexedSegments = std::vector<RTreeIndexedSegment>;
+using RTree = bgi::rtree<RTreeIndexedSegment, bgi::rstar<16>>;
+
+class SimulationMap {
+  public:
+    SimulationMap(double precision = 1e-8);
+
+    void resetMap(const std::string& path);
+    void eraseMap();
+    bool checkForCollisions(const geom::Polygon& shape_polygon) const;
+    std::vector<float> getLidarRanges(
+        const geom::Pose& lidar_pose, const model::Lidar& lidar) const;
+
+  private:
+    void initializeRTree();
+
+    struct Parameters {
+        double precision;
+    } params_;
+
+    geom::Segments obstacles_;
+    RTree rtree_;
+};
+
+}  // namespace truck::simulator

packages/simulator_2d/include/simulator_2d/simulator_engine.h

@@ -1,3 +1,5 @@
+#include "simulator_2d/simulation_map.h"
+#include "simulator_2d/status_code.h"
 #include "simulator_2d/truck_state.h"
 
 #include "model/model.h"
@@ -45,6 +47,8 @@ class SimulatorEngine {
     void resetRear(double x, double y, double yaw, double steering, double linear_velocity);
     void resetRear();
 
+    void checkForCollisions();
+
     geom::Pose getOdomBasePose() const;
     model::Steering getTargetSteering() const;
     std::vector<float> getLidarRanges(const geom::Pose& odom_base_pose) const;
@@ -67,10 +71,7 @@ class SimulatorEngine {
         double integration_step_6;
         double inverse_integration_step;
         double inverse_wheelbase_length;
-        geom::Vec2 base_to_lidar;
-        int lidar_rays_number;
-        geom::AngleVec2 lidar_angle_min;
-        geom::Angle lidar_angle_increment;
+        geom::Vec2 rear_to_lidar;
         geom::Vec2 rear_to_imu_translation;
         tf2::Transform base_to_hyro_rotation;
     } cache_;
@@ -84,11 +85,13 @@ class SimulatorEngine {
 
     rclcpp::Time time_;
 
+    StatusCode status_;
+
     State rear_ax_state_;
 
     std::unique_ptr<model::Model> model_ = nullptr;
 
-    geom::Segments obstacles_;
+    SimulationMap map_;
 };
 
 }  // namespace truck::simulator

packages/simulator_2d/include/simulator_2d/status_code.h

@@ -0,0 +1,7 @@
+#pragma once
+
+namespace truck::simulator {
+
+enum class StatusCode { IN_PROGRESS, OK, COLLISION };
+
+}  // namespace truck::simulator

packages/simulator_2d/include/simulator_2d/truck_state.h

@@ -1,3 +1,5 @@
+#include "simulator_2d/status_code.h"
+
 #include "model/model.h"
 #include "geom/angle.h"
 #include "geom/pose.h"
@@ -10,6 +12,7 @@ namespace truck::simulator {
 
 class TruckState {
   public:
+    StatusCode status() const;
     rclcpp::Time time() const;
     geom::Pose odomBasePose() const;
     model::Steering currentSteering() const;
@@ -23,6 +26,7 @@ class TruckState {
     geom::Vec3 gyroAngularVelocity() const;
     geom::Vec3 accelLinearAcceleration() const;
 
+    TruckState& status(StatusCode status);
     TruckState& time(const rclcpp::Time& time);
     TruckState& odomBasePose(const geom::Pose& pose);
     TruckState& currentSteering(const model::Steering& current_steering);
@@ -38,6 +42,7 @@ class TruckState {
 
   private:
     struct Cache {
+        StatusCode status;
         rclcpp::Time time;
         geom::Pose base_odom_pose;
         model::Steering current_steering;

packages/simulator_2d/package.xml

@@ -21,6 +21,7 @@
   <depend>nlohmann_json</depend>
   <depend>map</depend>
   <depend>sensor_msgs</depend>
+  <depend>Boost</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>