From 00b59921254fbfa8dc119659e4c2daa666f8d839 Mon Sep 17 00:00:00 2001 From: rolf Date: Sun, 30 Jul 2023 13:48:15 +0200 Subject: [PATCH 1/2] Fix typo --- roboteam_world/observer/src/filters/vision/VisionFilter.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/roboteam_world/observer/src/filters/vision/VisionFilter.cpp b/roboteam_world/observer/src/filters/vision/VisionFilter.cpp index 2810b18ef..9deb90833 100644 --- a/roboteam_world/observer/src/filters/vision/VisionFilter.cpp +++ b/roboteam_world/observer/src/filters/vision/VisionFilter.cpp @@ -9,9 +9,9 @@ proto::World VisionFilter::process(const std::vector & processDetections(packets,geometry_updated,robotData); //TODO for now not extrapolating because grsim sends packets from 1970... - Time extroplatedToTime = getExtrapolationTimeForPolicy(); + Time extropalatedToTime = getExtrapolationTimeForPolicy(); - return worldFilter.getWorldPrediction(extroplatedToTime); + return worldFilter.getWorldPrediction(extropalatedToTime); } bool VisionFilter::processGeometry(const std::vector& packets) { bool newGeometry = false; From 7013611fba4558b147fb73533642280050ed5103 Mon Sep 17 00:00:00 2001 From: rolf Date: Sun, 30 Jul 2023 15:15:40 +0200 Subject: [PATCH 2/2] Track robot trajectories --- roboteam_world/observer/CMakeLists.txt | 1 + .../include/observer/data/RobotParameters.h | 6 ++++ .../filters/vision/RobotTrajectorySegment.h | 33 +++++++++++++++++ .../observer/filters/vision/WorldFilter.h | 5 ++- .../filters/vision/robot/CameraRobotFilter.h | 4 ++- .../filters/vision/robot/RobotFilter.h | 7 ++-- .../observer/src/data/RobotParameters.cpp | 14 ++++++++ .../filters/vision/RobotTrajectorySegment.cpp | 23 ++++++++++++ .../src/filters/vision/WorldFilter.cpp | 35 +++++++++++++++---- .../vision/robot/CameraRobotFilter.cpp | 25 ++++++++++++- .../src/filters/vision/robot/RobotFilter.cpp | 9 +++++ 11 files changed, 150 insertions(+), 12 deletions(-) create mode 100644 roboteam_world/observer/include/observer/filters/vision/RobotTrajectorySegment.h create mode 100644 roboteam_world/observer/src/filters/vision/RobotTrajectorySegment.cpp diff --git a/roboteam_world/observer/CMakeLists.txt b/roboteam_world/observer/CMakeLists.txt index 0fd32569f..b71a34d72 100644 --- a/roboteam_world/observer/CMakeLists.txt +++ b/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 diff --git a/roboteam_world/observer/include/observer/data/RobotParameters.h b/roboteam_world/observer/include/observer/data/RobotParameters.h index 7b142c895..30a82ffdc 100644 --- a/roboteam_world/observer/include/observer/data/RobotParameters.h +++ b/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; diff --git a/roboteam_world/observer/include/observer/filters/vision/RobotTrajectorySegment.h b/roboteam_world/observer/include/observer/filters/vision/RobotTrajectorySegment.h new file mode 100644 index 000000000..23a3b0fe7 --- /dev/null +++ b/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 +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 diff --git a/roboteam_world/observer/include/observer/filters/vision/WorldFilter.h b/roboteam_world/observer/include/observer/filters/vision/WorldFilter.h index 851c3386c..b3e7620b4 100644 --- a/roboteam_world/observer/include/observer/filters/vision/WorldFilter.h +++ b/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 balls; - + std::vector 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 getHealthiestRobotsMerged(bool blueBots, Time time) const; [[nodiscard]] std::vector oneCameraHealthyRobots(bool blueBots, int camera_id, Time time) const; void addRobotPredictionsToMessage(proto::World& world, Time time) const; diff --git a/roboteam_world/observer/include/observer/filters/vision/robot/CameraRobotFilter.h b/roboteam_world/observer/include/observer/filters/vision/robot/CameraRobotFilter.h index feac53420..5398e87de 100644 --- a/roboteam_world/observer/include/observer/filters/vision/robot/CameraRobotFilter.h +++ b/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: diff --git a/roboteam_world/observer/include/observer/filters/vision/robot/RobotFilter.h b/roboteam_world/observer/include/observer/filters/vision/robot/RobotFilter.h index 5d7cc614f..5353f87f1 100644 --- a/roboteam_world/observer/include/observer/filters/vision/robot/RobotFilter.h +++ b/roboteam_world/observer/include/observer/filters/vision/robot/RobotFilter.h @@ -5,9 +5,12 @@ #ifndef RTT_ROBOTFILTER_H #define RTT_ROBOTFILTER_H +#include #include "CameraRobotFilter.h" +#include "observer/filters/vision/RobotTrajectorySegment.h" #include "observer/filters/vision/robot/RobotObservation.h" -#include +#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 getRobot(int cameraID, Time time) const; - //std::optional getLastFrameTrajectory(int cameraID, const RobotParameters& parameters) const; + std::optional getTrajectory(int cameraID, const RobotParameters& parameters) const; private: TeamRobotID id; std::map cameraFilters; diff --git a/roboteam_world/observer/src/data/RobotParameters.cpp b/roboteam_world/observer/src/data/RobotParameters.cpp index 6901b8c9c..efe343b00 100644 --- a/roboteam_world/observer/src/data/RobotParameters.cpp +++ b/roboteam_world/observer/src/data/RobotParameters.cpp @@ -4,6 +4,7 @@ #include "../../include/observer/data/RobotParameters.h" +#include 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); +} diff --git a/roboteam_world/observer/src/filters/vision/RobotTrajectorySegment.cpp b/roboteam_world/observer/src/filters/vision/RobotTrajectorySegment.cpp new file mode 100644 index 000000000..0d16dc1b1 --- /dev/null +++ b/roboteam_world/observer/src/filters/vision/RobotTrajectorySegment.cpp @@ -0,0 +1,23 @@ +// +// Created by rolf on 30-7-23. +// + +#include + +#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}{ + } +} \ No newline at end of file diff --git a/roboteam_world/observer/src/filters/vision/WorldFilter.cpp b/roboteam_world/observer/src/filters/vision/WorldFilter.cpp index d34cc4ea7..321c4a376 100644 --- a/roboteam_world/observer/src/filters/vision/WorldFilter.cpp +++ b/roboteam_world/observer/src/filters/vision/WorldFilter.cpp @@ -28,7 +28,7 @@ void WorldFilter::process(const std::vector &frames, std::vector 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 &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 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()); + } + } + } +} diff --git a/roboteam_world/observer/src/filters/vision/robot/CameraRobotFilter.cpp b/roboteam_world/observer/src/filters/vision/robot/CameraRobotFilter.cpp index 79d80a2a7..d9e4787e0 100644 --- a/roboteam_world/observer/src/filters/vision/robot/CameraRobotFilter.cpp +++ b/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(); -} \ No newline at end of file +} +rtt::RobotTrajectorySegment CameraRobotFilter::getTrajectory(const RobotParameters ¶ms) 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(robot.robot_id.robotID)}; +} diff --git a/roboteam_world/observer/src/filters/vision/robot/RobotFilter.cpp b/roboteam_world/observer/src/filters/vision/robot/RobotFilter.cpp index 0093675e2..7a0f1e403 100644 --- a/roboteam_world/observer/src/filters/vision/robot/RobotFilter.cpp +++ b/roboteam_world/observer/src/filters/vision/robot/RobotFilter.cpp @@ -125,5 +125,14 @@ std::optional RobotFilter::getRobot(int cameraID, Time time) cons } } +std::optional 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; +} +