robot.h

Go to the documentation of this file.

#ifndef ROBOT_H
#define ROBOT_H
 
#include <vector>
#include <cmath>
#include <set>
#include <queue>
#include <chrono>
#include <SDL2/SDL.h>
#include <box2d/box2d.h>
 
#include "utils.h"
#include "render.h"
#include "colormaps.h"
#include "spogobot.h"
#include "objects.h"

std::string log_current_robot();
 

enum class ShapeType {
    Circle,   
    Ellipse,  
    Polygon   
};
 

class MsgSuccessRate {
public:
    virtual double operator()(double msg_size, double p_send, double cluster_size) const = 0;

    virtual ~MsgSuccessRate() = default;
};

class DynamicMsgSuccessRate : public MsgSuccessRate {
public:
    DynamicMsgSuccessRate(double alpha = 0.000001, double beta = 3.0708, double gamma = 2.3234, double delta = 1.1897);

    double operator()(double msg_size, double p_send, double cluster_size) const override;
 
private:
    double alpha_; 
    double beta_;  
    double gamma_; 
    double delta_; 
};

class ConstMsgSuccessRate : public MsgSuccessRate {
public:
    explicit ConstMsgSuccessRate(double value);

    double operator()(double msg_size, double p_send, double cluster_size) const override;
 
private:
    double const_value_; 
};

MsgSuccessRate* msg_success_rate_factory(Configuration const& config);
 
 

class PogobotObject : public PhysicalObject {
 
public:

    PogobotObject(uint16_t _id, float _x, float _y,
           ObjectGeometry& geom,
           size_t _userdatasize,
           float _communication_radius = 80.0f,
           std::unique_ptr<MsgSuccessRate> _msg_success_rate = std::make_unique<ConstMsgSuccessRate>(0.5),
           float _temporal_noise_stddev = 0.0f,
           float _linear_damping = 0.0f, float _angular_damping = 0.0f,
           float _density = 10.0f, float _friction = 0.3f, float _restitution = 0.5f,
           float _max_linear_speed = 100.0f, float _max_angular_speed = 1.0f,
           float _linear_noise_stddev = 0.0f, float _angular_noise_stddev = 0.0f,
           bool _rotate_LEDs_45_deg = false,
           std::pair<int16_t, int16_t> angular_systematic_bias_domain = {0, 0},
           std::pair<int16_t, int16_t> photosensors_systematic_bias_domain = {0, 0},
           float _photosensors_noise_stddev = 0.0f,
           std::string const& _category = "robots",
           bool dummy = false);

    PogobotObject(Simulation* simulation, uint16_t _id, float _x, float _y,
           size_t _userdatasize, Configuration const& config,
           std::string const& _category = "robots");

    virtual void create_serialization_fields(DataLogger* data_logger) override;

    virtual void serialize_base_values(DataLogger* data_logger, double t) override;
 
 
 
    //virtual ~Robot();
 
    // Base info
    size_t userdatasize;
    void* data = nullptr;                
    void (*user_init)(void) = nullptr;   
    void (*user_step)(void) = nullptr;   
    bool _enable_user_steps = true;       
    void (*callback_export_data)(void) = nullptr;        

    virtual void launch_user_step(float t) override;
 
    // C-code accessible values
    uint32_t pogobot_ticks = 0;                 
    uint8_t main_loop_hz = 60;                  
    uint8_t max_nb_processed_msg_per_tick = 3;  
    void (*msg_rx_fn)(message_t*) = nullptr;    
    bool (*msg_tx_fn)(void) = nullptr;          
    int8_t error_codes_led_idx = 3;             
    time_reference_t _global_timer;             
    time_reference_t timer_main_loop;           
    uint32_t _current_time_milliseconds = 0;    
    uint32_t _error_code_initial_time = 0;      
    uint8_t percent_msgs_sent_per_ticks = 20;   
    uint32_t nb_msgs_sent = 0;                  
    uint32_t nb_msgs_recv = 0;                  
 
    // Time-related utilities
    std::set<time_reference_t*> stop_watches;   

    void register_stop_watch(time_reference_t* sw);

    void enable_stop_watches();

    void disable_stop_watches();

    virtual void update_time();
 
 
    // Physical information
    float radius;                   
    float left_motor_speed  = 0;    
    float right_motor_speed = 0;    
    bool show_lateral_leds = false; 
 

    virtual void render(SDL_Renderer*, b2WorldId) const override;

    virtual void render_communication_channels(SDL_Renderer*, b2WorldId) const;

    virtual void set_motor(motor_id motor, int speed);

    virtual b2Vec2 get_IR_emitter_position(ir_direction dir) const;

    virtual float get_IR_emitter_angle(ir_direction dir) const;

    virtual b2Vec2 get_photosensor_position(uint8_t sensor_number) const;
 
    // LED control
    std::vector<color_t> leds = std::vector<color_t>(5, {0, 0, 0}); 
 
    // Neighbors and messaging
    std::vector<std::vector<PogobotObject*>> neighbors{ir_all+1};  
    std::queue<message_t> messages; 
    float communication_radius;                       
    std::unique_ptr<MsgSuccessRate> msg_success_rate; 

    void send_to_neighbors(ir_direction dir, short_message_t *const message);

    void send_to_neighbors(ir_direction dir, message_t *const message);

    void receive_message(message_t *const message, PogobotObject* source);

    bool enable_user_steps() const { return _enable_user_steps; }
 

    void sleep_µs(uint64_t microseconds);
 
    uint64_t current_time_microseconds = 0LL;                        

    uint8_t motor_dir_mem[3] = {0, 1, 0};
    uint8_t motor_dir[3] = {0, 1, 0};
    uint16_t motor_power_mem[3] = {512, 512, 0};
 
protected:
    virtual void do_init([[maybe_unused]] b2WorldId world_id) override;

    void create_robot_body(b2WorldId world_id);

    void initialize_time();
 
    // Temporal information
    float temporal_noise = 0;
    float temporal_noise_stddev;
 
    // Physical information
    float max_linear_speed;
    float max_angular_speed;
    float linear_noise_stddev;
    float angular_noise_stddev;
    bool rotate_LEDs_45_deg;
 
    // Dummy?
    bool dummy = false;

    virtual void parse_configuration(Configuration const& config, Simulation* simulation) override;

    virtual void initialize_photosensors_bias(std::pair<int16_t, int16_t>& domain);

    virtual void initialize_angular_bias(std::pair<int16_t, int16_t>& domain);
 
 
public:
    // Photosensors noise and biaises
    std::vector<int16_t> photosensors_biases = {0, 0, 0}; 
    float photosensors_noise_stddev = 0.0f;     
 
    int16_t angular_bias = 0;
};
 

class PogobjectObject : public PogobotObject {
public:
    PogobjectObject(uint16_t _id, float _x, float _y,
           ObjectGeometry& geom,
           size_t _userdatasize,
           float _communication_radius = 80.0f,
           std::unique_ptr<MsgSuccessRate> _msg_success_rate = std::make_unique<ConstMsgSuccessRate>(0.5),
           float _temporal_noise_stddev = 0.0f,
           float _linear_damping = 0.0f, float _angular_damping = 0.0f,
           float _density = 10.0f, float _friction = 0.3f, float _restitution = 0.5f,
           std::string const& _category = "robots");

    PogobjectObject(Simulation* simulation, uint16_t _id, float _x, float _y,
           size_t _userdatasize, Configuration const& config,
           std::string const& _category = "robots");

    virtual void set_motor(motor_id motor, int speed) override;

    virtual b2Vec2 get_IR_emitter_position(ir_direction dir) const override;

    virtual void render(SDL_Renderer*, b2WorldId) const override;
 
protected:
    virtual void do_init([[maybe_unused]] b2WorldId world_id) override;
};
 

class Pogowall : public PogobotObject {
public:
    Pogowall(uint16_t _id, float _x, float _y,
           ObjectGeometry& geom,
           size_t _userdatasize,
           float _communication_radius = 80.0f,
           std::unique_ptr<MsgSuccessRate> _msg_success_rate = std::make_unique<ConstMsgSuccessRate>(0.5),
           float _temporal_noise_stddev = 0.0f,
           float _linear_damping = 0.0f, float _angular_damping = 0.0f,
           float _density = 10.0f, float _friction = 0.3f, float _restitution = 0.5f,
           float _max_linear_speed = 100.0f, float _max_angular_speed = 1.0f,
           float _linear_noise_stddev = 0.0f, float _angular_noise_stddev = 0.0f,
           std::string const& _category = "robots");

    Pogowall(Simulation* simulation, uint16_t _id, float _x, float _y,
           size_t _userdatasize, Configuration const& config,
           std::string const& _category = "robots");

    virtual void set_motor([[maybe_unused]] motor_id motor, [[maybe_unused]] int speed) override { }

    virtual b2Vec2 get_IR_emitter_position(ir_direction dir) const override;

    virtual float get_IR_emitter_angle(ir_direction dir) const override;

    virtual void render(SDL_Renderer*, b2WorldId) const override { }

    virtual void render_communication_channels(SDL_Renderer*, b2WorldId) const override { }

    virtual bool is_tangible() const override { return false; }

    virtual void move([[maybe_unused]] float x, [[maybe_unused]] float y, [[maybe_unused]] float theta = NAN) override { }
 
protected:
    virtual void do_init([[maybe_unused]] b2WorldId world_id) override;
};
 

class MembraneObject : public Pogowall { // PogobotObject {
public:
    struct Dot   { b2BodyId  body_id; };
    struct Joint { b2JointId joint_id; };
 

    MembraneObject(uint16_t _id, float _x, float _y,
           ObjectGeometry& geom,
           size_t _userdatasize,
           float _communication_radius = 80.0f,
           std::unique_ptr<MsgSuccessRate> _msg_success_rate = std::make_unique<ConstMsgSuccessRate>(0.5),
           float _temporal_noise_stddev = 0.0f,
           float _linear_damping = 0.0f, float _angular_damping = 0.0f,
           float _density = 10.0f, float _friction = 0.3f, float _restitution = 0.5f,
           float _max_linear_speed = 100.0f, float _max_angular_speed = 1.0f,
           float _linear_noise_stddev = 0.0f, float _angular_noise_stddev = 0.0f,
           unsigned int _num_dots = 100, float _dot_radius = 10.0f, int _cross_span = 3,
           float _stiffness = 30.f,
           bool _serialize_dot_pose = false,
           std::string _colormap = "rainbow",
           std::string const& _category = "robots");

    MembraneObject(Simulation* simulation, uint16_t _id, float _x, float _y,
           size_t _userdatasize, Configuration const& config,
           std::string const& _category = "robots");

    virtual void create_serialization_fields(DataLogger* data_logger) override;

    virtual void serialize_base_values(DataLogger* data_logger, double t) override;

    virtual void set_motor([[maybe_unused]] motor_id motor, [[maybe_unused]] int speed) override { }

    virtual b2Vec2 get_position() const override;

    virtual void render(SDL_Renderer*, b2WorldId) const override;

    virtual bool is_tangible() const override { return true; }

    virtual void move([[maybe_unused]] float x, [[maybe_unused]] float y, [[maybe_unused]] float theta = NAN) override;

    virtual arena_polygons_t generate_contours(std::size_t points_per_contour = 0) const override;
 
 
protected:
    virtual void do_init([[maybe_unused]] b2WorldId world_id) override;

    void create_robot_body(b2WorldId world_id);

    virtual void parse_configuration(Configuration const& config, Simulation* simulation) override;
 
    /* helper – create one distance joint with the usual parameters */
    void make_distance_joint(b2WorldId world_id,
                             b2BodyId  a,
                             b2BodyId  b,
                             float     stiffness_scale = 1.0f);
 
    // Physical information
    int num_dots;
    std::vector<size_t> size_contours;
    float dot_radius;
    int cross_span;
    float stiffness;
    bool serialize_dot_pose;
    std::string colormap;
    std::vector<Dot> dots;
    std::vector<Joint> joints;
};
 
 
class RectMembraneObject : public MembraneObject {
public:
    struct Rect {
        b2BodyId body_id;
        float half_length;
        float half_thickness;
    };
 
    struct Hinge {
        b2JointId joint_id;
    };
 
    RectMembraneObject(uint16_t _id, float _x, float _y,
        ObjectGeometry& geom,
        size_t _userdatasize,
        float _communication_radius = 80.0f,
        std::unique_ptr<MsgSuccessRate> _msg_success_rate = std::make_unique<ConstMsgSuccessRate>(0.5),
        float _temporal_noise_stddev = 0.0f,
        float _linear_damping = 0.0f, float _angular_damping = 0.0f,
        float _density = 10.0f, float _friction = 0.3f, float _restitution = 0.5f,
        float _max_linear_speed = 100.0f, float _max_angular_speed = 1.0f,
        float _linear_noise_stddev = 0.0f, float _angular_noise_stddev = 0.0f,
        unsigned int _num_dots = 100,
        float _rect_thickness = 10.0f,
        int _cross_span = 3,
        float _stiffness = 30.0f,
        bool _serialize_dot_pose = false,
        std::string _colormap = "rainbow",
        std::string const& _category = "robots");
 
    RectMembraneObject(Simulation* simulation, uint16_t _id, float _x, float _y,
        size_t _userdatasize, Configuration const& config,
        std::string const& _category = "robots");

    virtual void serialize_base_values(DataLogger* data_logger, double t) override;
 
    virtual b2Vec2 get_position() const override;
    virtual void render(SDL_Renderer* renderer, b2WorldId world_id) const override;
    virtual void move(float x, float y, float theta = NAN) override;
    virtual arena_polygons_t generate_contours(std::size_t points_per_contour = 0) const override;
 
protected:
    virtual void do_init(b2WorldId world_id) override;
    virtual void parse_configuration(Configuration const& config, Simulation* simulation) override;
 
    void create_rect_membrane(b2WorldId world_id);
    void make_revolute_joint(b2WorldId world_id, b2BodyId a, b2BodyId b, b2Vec2 anchor_world);
    void make_center_distance_joint(b2WorldId world_id, b2BodyId a, b2BodyId b, float stiffness_scale = 1.0f);
 
    float rect_thickness;
    std::vector<Rect> rects;
    std::vector<Hinge> hinges;
};
 

class ActiveObject : public PogobotObject {
public:
    ActiveObject(uint16_t _id, float _x, float _y,
           ObjectGeometry& geom,
           size_t _userdatasize,
           float _communication_radius = 80.0f,
           std::unique_ptr<MsgSuccessRate> _msg_success_rate = std::make_unique<ConstMsgSuccessRate>(0.5),
           float _temporal_noise_stddev = 0.0f,
           float _linear_damping = 0.0f, float _angular_damping = 0.0f,
           float _density = 10.0f, float _friction = 0.3f, float _restitution = 0.5f,
           float _max_linear_speed = 100.0f, float _max_angular_speed = 1.0f,
           float _linear_noise_stddev = 0.0f, float _angular_noise_stddev = 0.0f,
           std::string _colormap = "rainbow",
           std::string const& _category = "active_objects");

    ActiveObject(Simulation* simulation, uint16_t _id, float _x, float _y,
           size_t _userdatasize, Configuration const& config,
           std::string const& _category = "active_objects");

    virtual void set_motor([[maybe_unused]] motor_id motor, [[maybe_unused]] int speed) override { }

    virtual void render(SDL_Renderer*, b2WorldId) const override;

    virtual void render_communication_channels(SDL_Renderer*, b2WorldId) const override { }
 
//    /**
//     * @brief Move the object to a given coordinate
//     *
//     * @param x X coordinate.
//     * @param y Y coordinate.
//     * @param theta Orientation, in rad.
//     */
//    virtual void move(float x, float y, float theta = NAN) override;
 
 
protected:
    std::string colormap;

    virtual void parse_configuration(Configuration const& config, Simulation* simulation) override;
};
 
 
 
 
//extern Robot* current_robot;
extern PogobotObject* current_robot;
extern int UserdataSize;
extern void* mydata;
extern uint64_t sim_starting_time_microseconds;
 

uint64_t get_current_time_microseconds();
 
#endif
 
 
// MODELINE "{{{1
// vim:expandtab:softtabstop=4:shiftwidth=4:fileencoding=utf-8
// vim:foldmethod=marker