epuck_robot.h

Go to the documentation of this file.

#ifndef EPUCK_ROBOT_H
#define EPUCK_ROBOT_H
 
 
#include "bitrl//bitrl_config.h"
 
#ifdef BITRL_WEBOTS
 
#include "rlenvs/rigid_bodies/body_rotation.h"
#include "rlenvs/rigid_bodies/body_translation.h"
#include "rlenvs/rlenvs_types_v2.h"
 
#include <webots/DistanceSensor.hpp>
#include <webots/Motor.hpp>
#include <webots/PositionSensor.hpp>
#include <webots/Robot.hpp>
 
// we need a supervisor as we want to
// be able to reset the simulation
#include <webots/Supervisor.hpp>
 
#include <memory>
#include <ostream>
 
namespace rlenvscpp
{
namespace rigid_bodies
{
namespace webots_robots
{
 
struct EpuckRobotProperties
{
 
    static constexpr uint_t DISTANCE_SENSORS_NUMBER = 8;
 
    static constexpr uint_t LEFT_POSITION_SENSOR_ID = 0;
 
    static constexpr uint_t RIGHT_POSITION_SENSOR_ID = 1;
 
    static constexpr uint_t LEFT_MOTOR_ID = 0;
 
    static constexpr uint_t RIGHT_MOTOR_ID = 0;
 
    static constexpr real_t WHEEL_RADIUS = 0.025;
 
    static constexpr real_t AXLE_LENGTH = 0.052;
 
    static constexpr real_t MAX_FWD_SPEED = 0.25;
 
    static constexpr real_t MAX_BWD_SPEED = 0.25;
 
    static constexpr real_t MAX_ROTATION_SPEED = 6.28;
};
 
struct EpuckOdometry
{
 
    real_t dl;
 
    real_t dr;
 
    real_t da;
 
    EpuckOdometry(real_t dl_, real_t dr_, real_t da_);
 
    std::ostream &print(std::ostream &out) const noexcept;
};
 
inline EpuckOdometry::EpuckOdometry(real_t dl_, real_t dr_, real_t da_) : dl(dl_), dr(dr_), da(da_)
{
}
 
inline std::ostream &operator<<(std::ostream &out, const EpuckOdometry &info)
{
    return info.print(out);
}
 
class EpuckRobot final : protected EpuckRobotProperties
{
  public:
    static const std::string name;
 
    static constexpr uint_t DEFAULT_SIM_TIME_STEP = 32;
 
    using EpuckRobotProperties::AXLE_LENGTH;
    using EpuckRobotProperties::DISTANCE_SENSORS_NUMBER;
    using EpuckRobotProperties::LEFT_POSITION_SENSOR_ID;
    using EpuckRobotProperties::MAX_BWD_SPEED;
    using EpuckRobotProperties::MAX_FWD_SPEED;
    using EpuckRobotProperties::MAX_ROTATION_SPEED;
    using EpuckRobotProperties::RIGHT_POSITION_SENSOR_ID;
    using EpuckRobotProperties::WHEEL_RADIUS;
 
    EpuckRobot();
 
    std::shared_ptr<webots::Supervisor> get_webots_robot_prt() { return robot_; }
 
    void activate_motor(uint_t motor_id, real_t init_velocity);
 
    void activate_position_sensor(uint_t sensor_id, uint_t time_step);
 
    void activate_proximity_sensor(const std::string &name, uint_t time_step);
 
    int_t step(uint_t time_step) { return robot_->step(time_step); }
 
    int_t begin_step(uint_t time_step) { return robot_->stepBegin(time_step); }
 
    int_t end_step() { return robot_->stepEnd(); }
 
    void reset();
 
    void reload() { robot_->worldReload(); }
 
    void stop_simulation() { robot_->simulationQuit(EXIT_SUCCESS); }
 
    void pause_simulation()
    {
        robot_->simulationSetMode(webots::Supervisor::SIMULATION_MODE_PAUSE);
    }
 
    EpuckOdometry compute_odometry() const;
 
    uint_t get_basic_time_step() const { return robot_->getBasicTimeStep(); }
 
    real_t get_distance_value_from_sensor(const std::string &sensor_name) const;
 
    std::vector<real_t> read_distance_sensors() const;
 
    real_t get_motor_velocity(uint_t mid) const { return motors_[mid]->getVelocity(); }
 
    RBTranslation get_position() const;
 
    RBRotation get_rotation() const;
 
    webots::Motor *get_motor(uint_t m) { return motors_[m]; }
    webots::PositionSensor *get_position_sensor(uint_t m) { return position_sensors_[m]; }
 
  private:
    std::shared_ptr<webots::Supervisor> robot_;
 
    std::vector<webots::PositionSensor *> position_sensors_;
 
    std::vector<webots::Motor *> motors_;
 
    std::vector<webots::DistanceSensor *> proximity_sensors_;
};
 
} // namespace webots_robots
} // namespace rigid_bodies
} // namespace rlenvscpp
 
#endif
#endif