Track robot trajectories

roboteam_world/observer/CMakeLists.txt

@@ -27,6 +27,7 @@ add_library(observer STATIC
         src/filters/vision/Camera.cpp
         src/filters/vision/CameraMap.cpp
         src/filters/vision/RobotFeedbackFilter.cpp
+        src/filters/vision/RobotTrajectorySegment.cpp
         )
 
 target_link_libraries(observer

roboteam_world/observer/include/observer/data/RobotParameters.h

@@ -16,6 +16,12 @@ class RobotParameters {
     [[nodiscard]] proto::RobotParameters toProto() const;
     static RobotParameters from_default();
     static RobotParameters from_rtt2020();
+
+    [[nodiscard]] double getRadius() const;
+    [[nodiscard]] double getHeight() const;
+    [[nodiscard]] double centerToFrontDistance() const;
+    [[nodiscard]] double getFrontWidth() const;
+
 private:
     double radius;
     double height;

roboteam_world/observer/include/observer/filters/vision/RobotTrajectorySegment.h

@@ -0,0 +1,33 @@
+//
+// Created by rolf on 30-7-23.
+//
+
+#ifndef RTT_ROBOTEAM_WORLD_OBSERVER_SRC_FILTERS_VISION_ROBOTTRAJECTORYSEGMENT_H
+#define RTT_ROBOTEAM_WORLD_OBSERVER_SRC_FILTERS_VISION_ROBOTTRAJECTORYSEGMENT_H
+
+#include <roboteam_utils/RobotShape.h>
+namespace rtt {
+
+    //This class contains the data necessary to describe a robot travelling at a constant velocity
+    //For a small period of time.
+    //This is an approximation of the robots' behavior used for computing ball collisions
+    struct RobotTrajectorySegment {
+        RobotTrajectorySegment(RobotShape  startPos,
+                               Vector2 vel,
+                               double angVel,
+                               double dt,
+                               bool isBlue,
+                               int id
+                               );
+        RobotShape startPos; //includes robots' starting angle
+        Vector2 vel;
+        double angVel;
+        double dt;
+
+        bool isBlue;
+        int id;
+    };
+
+}
+
+#endif  // RTT_ROBOTEAM_WORLD_OBSERVER_SRC_FILTERS_VISION_ROBOTTRAJECTORYSEGMENT_H

roboteam_world/observer/include/observer/filters/vision/WorldFilter.h

@@ -7,6 +7,7 @@
 
 #include "observer/filters/vision/ball/BallFilter.h"
 #include "observer/filters/vision/robot/RobotFilter.h"
+#include "observer/filters/vision/RobotTrajectorySegment.h"
 #include "DetectionFrame.h"
 #include "observer/parameters/RobotParameterDatabase.h"
 #include "RobotFeedbackFilter.h"
@@ -43,7 +44,7 @@ class WorldFilter {
   robotMap blue;
   robotMap yellow;
   std::vector<BallFilter> balls;
-
+  std::vector<rtt::RobotTrajectorySegment> robotTrajectories;
   RobotParameters blueParams;
   RobotParameters yellowParams;
 
@@ -56,6 +57,8 @@ class WorldFilter {
   void processFrame(const DetectionFrame& frame);
   void processRobots(const DetectionFrame& frame, bool blueBots);
   void processBalls(const DetectionFrame& frame);
+  void getRobotTrajectories(int cameraID);
+  void getTeamRobotTrajectories(bool isBlue, int cameraID);
   [[nodiscard]] std::vector<FilteredRobot> getHealthiestRobotsMerged(bool blueBots, Time time)  const;
   [[nodiscard]] std::vector<FilteredRobot> oneCameraHealthyRobots(bool blueBots, int camera_id, Time time) const;
   void addRobotPredictionsToMessage(proto::World& world, Time time) const;

roboteam_world/observer/include/observer/filters/vision/robot/CameraRobotFilter.h

@@ -5,6 +5,8 @@
 #ifndef RTT_ROBOTEAM_WORLD_OBSERVER_SRC_FILTERS_VISION_CAMERAROBOTFILTER_H_
 #define RTT_ROBOTEAM_WORLD_OBSERVER_SRC_FILTERS_VISION_CAMERAROBOTFILTER_H_
 
+#include "observer/data/RobotParameters.h"
+#include "observer/filters/vision/RobotTrajectorySegment.h"
 #include "observer/filters/vision/CameraObjectFilter.h"
 #include "observer/filters/vision/PosVelFilter2D.h"
 #include "RobotOrientationFilter.h"
@@ -31,7 +33,7 @@ class CameraRobotFilter : public CameraObjectFilter{
 
   [[nodiscard]] bool justUpdated() const;
 
-  //RobotTrajectory getLastFrameTrajectory() const; //TODO fix
+  [[nodiscard]] rtt::RobotTrajectorySegment getTrajectory(const RobotParameters& params) const;
 
  private:
 

roboteam_world/observer/include/observer/filters/vision/robot/RobotFilter.h

@@ -5,9 +5,12 @@
 #ifndef RTT_ROBOTFILTER_H
 #define RTT_ROBOTFILTER_H
 
+#include <optional>
 #include "CameraRobotFilter.h"
+#include "observer/filters/vision/RobotTrajectorySegment.h"
 #include "observer/filters/vision/robot/RobotObservation.h"
-#include <optional>
+#include "observer/data/RobotParameters.h"
+
 class RobotFilter  {
  public:
   explicit RobotFilter(const RobotObservation &observation);
@@ -45,7 +48,7 @@ class RobotFilter  {
    * @return the estimate of the robot on the given camera at time t, if a filter which tracks the robot on that camera exists
    */
   [[nodiscard]] std::optional<FilteredRobot> getRobot(int cameraID, Time time) const;
-  //std::optional<RobotTrajectorySegment> getLastFrameTrajectory(int cameraID, const RobotParameters& parameters) const;
+  std::optional<rtt::RobotTrajectorySegment> getTrajectory(int cameraID, const RobotParameters& parameters) const;
  private:
   TeamRobotID id;
   std::map<int,CameraRobotFilter> cameraFilters;

roboteam_world/observer/src/data/RobotParameters.cpp

@@ -4,6 +4,7 @@
 
 
 #include "../../include/observer/data/RobotParameters.h"
+#include <cmath>
 
 RobotParameters RobotParameters::from_default() {
     RobotParameters parameters; //default constructor should be roughly correct
@@ -44,3 +45,16 @@ RobotParameters::RobotParameters(double radius, double height, double frontWidth
                                                        dribblerWidth{dribblerWidth}, angleOffset{angleOffset} {
 
 }
+double RobotParameters::getRadius() const {
+    return radius;
+}
+double RobotParameters::getHeight() const {
+    return height;
+}
+double RobotParameters::getFrontWidth() const {
+    return frontWidth;
+}
+double RobotParameters::centerToFrontDistance() const {
+    //Pythagoras
+    return sqrt(radius*radius - 0.25*frontWidth*frontWidth);
+}

roboteam_world/observer/src/filters/vision/RobotTrajectorySegment.cpp

@@ -0,0 +1,23 @@
+//
+// Created by rolf on 30-7-23.
+//
+
+#include <utility>
+
+#include "observer/filters/vision/RobotTrajectorySegment.h"
+namespace rtt {
+
+    RobotTrajectorySegment::RobotTrajectorySegment(RobotShape  startPos,
+                                                   Vector2 vel,
+                                                   double angVel,
+                                                   double dt,
+                                                   bool isBlue,
+                                                   int id) :
+          startPos{std::move(startPos)},
+          vel{vel},
+          angVel{angVel},
+          dt{dt},
+          isBlue{isBlue},
+          id{id}{
+    }
+}

roboteam_world/observer/src/filters/vision/WorldFilter.cpp

@@ -28,7 +28,7 @@ void WorldFilter::process(const std::vector<proto::SSL_DetectionFrame> &frames,
 
   std::vector<DetectionFrame> detectionFrames;
   for (const auto &protoFrame: frames) {
-    detectionFrames.emplace_back(DetectionFrame(protoFrame));
+    detectionFrames.emplace_back(protoFrame);
   }
   //Sort by time
   std::sort(detectionFrames.begin(), detectionFrames.end(),
@@ -45,12 +45,12 @@ void WorldFilter::process(const std::vector<proto::SSL_DetectionFrame> &frames,
   //This can also be caused by other teams running e.g. their simulators internally and accidentally broadcasting onto the network
   detectionFrames.erase(std::remove_if(detectionFrames.begin(), detectionFrames.end(),
                                        [](const DetectionFrame &frame) { return frame.dt < 0.0; }),detectionFrames.end());
-  for(const auto& frame : detectionFrames){
-      lastCaptureTimes[frame.cameraID] = frame.timeCaptured;
-  }
   for (const auto &frame : detectionFrames) {
     processFrame(frame);
   }
+  for(const auto& frame : detectionFrames){
+    lastCaptureTimes[frame.cameraID] = frame.timeCaptured;
+  }
 }
 
 
@@ -201,6 +201,9 @@ void WorldFilter::addRobotPredictionsToMessage(proto::World &world, Time time) c
   }
 }
 void WorldFilter::processBalls(const DetectionFrame &frame) {
+  //First, we get the robots trajectories
+  getRobotTrajectories(frame.cameraID);
+
   std::vector<CameraGroundBallPrediction> predictions(balls.size());
   //get predictions from cameras
   for (std::size_t i = 0; i < balls.size(); ++i) {
@@ -234,6 +237,24 @@ void WorldFilter::addBallPredictionsToMessage(proto::World &world, Time time) co
     world.mutable_ball()->CopyFrom(bestBall.asWorldBall());
   }
 }
-
-
-
+void WorldFilter::getRobotTrajectories(int cameraID) {
+  //We do not return a value here because from performance benchmarks
+  //we observed that making it a member prevents a lot of allocations in observer.
+  //Thus, we prefer to use a member whose capacity is re-used the next frame instead
+  robotTrajectories.clear();
+  getTeamRobotTrajectories(false,cameraID);
+  getTeamRobotTrajectories(true,cameraID);
+}
+void WorldFilter::getTeamRobotTrajectories(bool isBlue, int cameraID) {
+  const robotMap & robots = isBlue ? blue : yellow;
+  const RobotParameters& params = isBlue ?  blueParams : yellowParams;
+  for(const auto& oneIDFilters : robots){
+    //TODO: now we consider all robots with one ID to be valid. In case there is multiple, we may want to only take the 'best' robot here
+    for(const auto& bot : oneIDFilters.second){
+      auto trajectory = bot.getTrajectory(cameraID,params);
+      if(trajectory.has_value()){
+        robotTrajectories.push_back(trajectory.value());
+      }
+    }
+  }
+}

roboteam_world/observer/src/filters/vision/robot/CameraRobotFilter.cpp

@@ -105,4 +105,27 @@ void CameraRobotFilter::updatePreviousInfo() {
   previousPosition = RobotPos(positionFilter.getPosition(),angleFilter.getPosition());
 
   previousTime = positionFilter.lastUpdated();
-}
+}
+rtt::RobotTrajectorySegment CameraRobotFilter::getTrajectory(const RobotParameters &params) const {
+  auto pos = positionFilter.getPosition();
+
+  Time currentTime = positionFilter.lastUpdated();
+  double dt = (currentTime-previousTime).asSeconds();
+
+  Eigen::Vector2d deltaPosition = pos-previousPosition.position;
+  rtt::Vector2 deltaPos(deltaPosition.x(),deltaPosition.y());
+  rtt::Vector2 vel = deltaPos / dt;
+
+  rtt::Angle currentAngle = angleFilter.getPosition();
+  double w = (currentAngle - previousPosition.angle).getValue() / dt;
+
+  if(dt == 0.0){ //Protect against division by zero; can happen if robots are initialized
+      w = 0.0;
+      vel = rtt::Vector2(0.0,0.0);
+  }
+
+  rtt::Vector2 previousPos(previousPosition.position.x(),previousPosition.position.y());
+  rtt::RobotShape startPos(previousPos,params.centerToFrontDistance(),params.getRadius(),previousPosition.angle);
+
+  return {startPos,vel,w,dt,robot.team == TeamColor::BLUE,static_cast<int>(robot.robot_id.robotID)};
+}

roboteam_world/observer/src/filters/vision/robot/RobotFilter.cpp

@@ -125,5 +125,14 @@ std::optional<FilteredRobot> RobotFilter::getRobot(int cameraID, Time time) cons
   }
 }
 
+std::optional<rtt::RobotTrajectorySegment> RobotFilter::getTrajectory(int cameraID, const RobotParameters& parameters) const{
+  //TODO: check robot health here
+  auto cameraFilter = cameraFilters.find(cameraID);
+  if(cameraFilter != cameraFilters.end()){
+    return cameraFilter->second.getTrajectory(parameters);
+  }
+  return std::nullopt;
+}
+