wak_sensor_vars.h

// Walk and kick: Sensor variables
// Author: Philipp Allgeuer <pallgeuer@ais.uni-bonn.de>
 
 // Ensure header is only included once
#ifndef WAK_SENSOR_VARS_H
#define WAK_SENSOR_VARS_H

// Includes
#include <walk_and_kick/wak_common.h>
#include <walk_and_kick/wak_config.h>
#include <walk_and_kick/wak_ros_interface.h>
#include <walk_and_kick/wak_gc_vars.h>
#include <walk_and_kick/wak_tc_vars.h>
#include <walk_and_kick/wak_utils.h>
#include <rc_utils/math_funcs.h>
#include <rc_utils/ros_time.h>

// Walk and kick namespace
namespace walk_and_kick
{
    class SensorVars
    {
    public:
        //
        // General
        //

        // Constructor
        SensorVars(WAKConfig& config, WAKRosInterface& RI);

        // Config parameters
        WAKConfig& config;

        // Field dimensions
        const FieldDimensions field;

        // Update function
        void update(const ros::Time& now);

        //
        // Sensor variables
        //

        // Current cycle ROS time
        ros::Time now;

        // Robot state variables
        float timeStanding;      // The duration for which the robot has been in the standing state (zero if not standing right now)

        // Game controller variables
        GCVars GC;               // Note: All uses of this variable should be preceded by checking listenToGC
        bool GCDataFresh;        // Boolean flag whether the game controller base data is fresh
        bool GCExtraDataFresh;   // Boolean flag whether the game controller extra data is fresh
        bool listenToGC;         // Boolean flag whether the robot should listen to the game controller

        // Configuration variables
        bool isPenaltyShoot;     // Boolean flag whether the game is currently a penalty shootout, as opposed to normal soccer gameplay
        bool isPenaltyTaker;     // Boolean flag whether, assuming the game is a penalty shootout, we are the attacker
        KickoffType kickoffType; // Kickoff type (e.g. KT_ATTACK, KT_DEFEND, KT_DROPBALL, KT_MANUAL)
        bool playOnYellow;       // Boolean flag whether the robot should play on the positive yellow goal
        int goalSign;            // Sign of the goal the robot should play on (+1 = Positive yellow goal, -1 = Negative blue goal)
        bool playAsCyan;         // Boolean flag whether the robot is playing as the cyan team

        // Play state variables
        GameCommand gameCommand; // Game command (e.g. CMD_PLAY, CMD_STOP, CMD_POSE)
        bool gameCommandIsNew;   // Boolean flag whether the game command is new in this cycle
        GameRole gameRole;       // Game role (e.g. ROLE_FIELDPLAYER, ROLE_GOALIE)
        PlayState playState;     // Play state (e.g. PS_STOP, PS_READY, PS_SET, PS_BEGIN_PLAY, PS_PLAY)
        bool playStateIsNew;     // Boolean flag whether the play state is new in this cycle (true also for example if the play state didn't change, but the game command is new)
        float timePlaying;       // Time since the play state was last *not* PS_BEGIN_PLAY or PS_PLAY

        // Localisation variables
        float compassHeading;    // Heading of the robot measured by the compass in the range (-pi,pi] (0 is towards the positive yellow goal and CCW from there is positive)
        float robotPoseConf;     // Current confidence of the robot's estimated position on the field (always current)
        bool haveRobotPose;      // Boolean flag whether we are currently localised (always current)
        bool robotPoseIsNew;     // Boolean flag whether haveRobotPose turned from false to true in this cycle (always current)
        float robotPoseTimeAgo;  // Time since the robot pose was last valid (last valid)
        float robotPoseDur;      // Duration for which the robot has been/was localised the current/last time the robot was localised (last valid)
        Vec3f robotPose;         // Global position and orientation of the robot on the field, where x points to the positive yellow goal, y to the left, and z is a CCW rotation relative to the direction of the positive goal (last valid)
        Vec2f robotPose2D;       // Global position of the robot on the field, where x points to the positive yellow goal, and y to the left (last valid)

        // Ball variables
        float ballConf;          // Current confidence of the ball (always current)
        bool haveBall;           // Boolean flag whether we currently have a ball (always current)
        bool ballIsNew;          // Boolean flag whether haveBall turned from false to true in this cycle (always current)
        float ballTimeAgo;       // Time since the ball was last valid (last valid)
        float ballDur;           // Duration for which the ball has been/was valid the current/last time it was valid (last valid)
        Vec2f ballDir;           // Egocentric vector from the robot to the ball (last valid)
        float ballAngle;         // Egocentric angle to the ball, measured CCW from straight ahead (last valid)
        float ballDist;          // Distance from the robot to the ball (last valid)
        bool ballStable;         // Boolean flag whether the ball is currently close and valid, and has been valid for long enough that it is considered to be stable (always current)
        bool haveBallPose;       // Boolean flag whether we currently have a ball pose (always current)
        bool ballPoseIsNew;      // Boolean flag whether haveBallPose turned from false to true in this cycle (always current)
        float ballPoseTimeAgo;   // Time since the ball pose was last valid (last valid)
        float ballPoseDur;       // Duration for which the ball pose has been/was valid the current/last time it was valid (last valid)
        Vec2f ballPose;          // Global position of the ball on the field (last valid)
        bool ballPoseStable;     // Boolean flag whether the ball pose is currently close and valid, and has been valid for long enough that it is considered to be stable (always current)

        // Goal variables
        GoalPostList goalPosts;  // List of currently detected goal posts, exactly as published by the vision

        // Obstacle variables
        ObstacleList obstacles;  // List of currently detected obstacles, exactly as published by the vision
        Obstacle obstClosest;    // The closest detected obstacle to the robot

        // Extended play state variables
        bool ballHasMoved;       // Boolean flag whether the ball has moved away from the field centre since the kickoff
        bool ballInPlay;         // Boolean flag whether the ball is in play
        bool directGoalAllowed;  // Boolean flag whether a direct kick of the ball into the goals is allowed and will result in a score

        // Team communication variables
        TCVars TC;               // Note: All uses of this variable should be preceded by checking listenToTC
        bool TCDataAvailable;    // Boolean flag whether fresh team communications data is available from at least one robot
        bool listenToTC;         // Boolean flag whether the robot should listen to the team communications

        // Miscellaneous variables
        DiveDirection diveDecision;

        //
        // Helper functions
        //

        // Functions to check the role of the robot
        bool isInvalidRole() const { return !gameRoleValid(gameRole); }
        bool isFieldPlayer() const { return gameRoleIsFieldPlayer(gameRole); }
        bool isGoalie() const { return gameRoleIsGoalie(gameRole); }

        // Robot state functions
        bool isRelaxed() const { return RI.stateRelaxed(); }
        bool isIniting() const { return RI.stateIniting(); }
        bool isStanding() const { return RI.stateStanding(); }
        bool isWalking() const { return RI.stateWalking(); }
        bool isKicking() const { return RI.stateKicking(); }
        bool isFallen() const { return RI.stateFallen(); }

    private:
        // ROS interface
        WAKRosInterface& RI;

        // Play state set function
        void setPlayState(PlayState state, const ros::Time& toldTime = ros::Time());

        // Play state timeout management functions
        void clearPlayStateTimeout() { rc_utils::zeroRosTime(m_playStateTimeout); }
        void setPlayStateTimeout(const ros::Time& timeout) { m_playStateTimeout = timeout; }
        bool havePlayStateTimeout() const { return !m_playStateTimeout.isZero(); }
        bool playStateWouldTimeOut(const ros::Time& timeout) const { return (!timeout.isZero() && now >= timeout); }
        bool playStateTimedOut() const { return playStateWouldTimeOut(m_playStateTimeout); }

        // Play state timeout condition functions
        bool timeoutTimeout(float timeSinceLastTold) const { return (timeSinceLastTold >= config.gcTimeoutTimeoutLast()); }
        bool timeoutReady() const { return (kickoffType == KT_MANUAL || !havePlayStateTimeout() || playStateTimedOut()); }
        bool timeoutSet(float timeSinceFirstIn, float timeSinceFirstTold, float timeSinceLastTold) const;
        bool timeoutBeginPlay(float timeSinceFirstTold) const { return (listenToGC || kickoffType == KT_MANUAL || timeSinceFirstTold >= rc_utils::coerceMin(config.sWaitTime() - config.waitTimeReduction(), 0.0f)); }

        // Play state variables
        ros::Time m_playStateFirstIn;
        ros::Time m_playStateFirstTold;
        ros::Time m_playStateLastTold;
        ros::Time m_playStateTimeout;
        ros::Time m_lastTimeNotPlaying;

        // Internal localisation variables
        ros::Time m_robotPoseTimeFirst;
        ros::Time m_robotPoseTimeLast;

        // Internal ball variables
        ros::Time m_ballTimeFirst;
        ros::Time m_ballTimeLast;
        ros::Time m_ballPoseTimeFirst;
        ros::Time m_ballPoseTimeLast;

        // Robot state variables
        int m_standingCount;

        // Ball has moved detection variables
        TheWorm m_ballHasMovedWorm;

        // Miscellaneous variables
        cycle_t m_updateCycle;
    };
}

#endif
// EOF