C++ API and example

1. Introduction

This page exist in order to extract the examples from the Doxygen documentation, Please have look at the end of this page there are all the examples.

2. C++ API and example

template<int QUEUE_SIZE, int NB_ACTUATORS, class STATE, class EXTENDED_STATE>
class o80::BackEnd
#include <back_end.hpp>

The backend computes the desired state for each actuator, based on the time series of commands filled up by a frontend. The backend writes information to the time series of observations.

tparam QUEUE_SIZE

number of commands that can be hosted in the command queue at any point of time. Exceptions will be thrown if more commands are queued.

tparam NB_ACTUATORS

number of actuators of the robot

tparam STATE

class encapsulating the state of an actuator of the robot

tparam EXTENDED_STATE

class encapsulating supplementary arbitrary information

Public Types

typedef time_series::MultiprocessTimeSeries<Observation<NB_ACTUATORS, STATE, EXTENDED_STATE>> ObservationsTimeSeries

Multiprocess time series hosting observations

typedef time_series::MultiprocessTimeSeries<int> CompletedCommandsTimeSeries

multiprocess times series hosting the commands id that have been processed by the backend

Public Functions

BackEnd(std::string segment_id, bool new_commands_observations = false)
Parameters
  • segment_id – should be the same for the backend and the frontend

  • new_commands_observations – (default false). If true, information will be writen in the observation only when the desired state of any actuator changed (when false: an observation is writen for each iteration)

~BackEnd()

delete the shared memory segments

const States<NB_ACTUATORS, STATE> &pulse(const TimePoint &time_now, const States<NB_ACTUATORS, STATE> &current_states, const EXTENDED_STATE &extended_state, bool iteration_update = true, long int current_iteration = -1)

The backend iterates once.

Parameters
  • time_now – : current time stamp in nanoseconds

  • current_states – : current state for each actuator

  • extended_state – : arbitrary information to be added to the Observation that will be writen to the observations time series.

Returns

: the desired states for each actuator, based on the current queue of commands

bool is_active()

returns false if a the last iteration, the previous desired states was returned as such (i.e.

no command is active)

void purge()

purge all the commands currently queued in the controllers, including the ones currently running.

The purge occurs when the method iterate is called.

const States<NB_ACTUATORS, STATE> &initial_states() const

returns the first observed states provided (via the first call to the pulse function.

May be usefull for the implementation of reset functions

Private Functions

bool iterate(const TimePoint &time_now, const States<NB_ACTUATORS, STATE> &current_states, bool iteration_update = true, long int current_iteration = -1)

Private Members

std::string segment_id_
ObservationsTimeSeries observations_
ControllersManager<NB_ACTUATORS, QUEUE_SIZE, STATE> controllers_manager_
States<NB_ACTUATORS, STATE> desired_states_
States<NB_ACTUATORS, STATE> initial_states_
bool first_iteration_
long int iteration_
FrequencyMeasure frequency_measure_
double observed_frequency_
bool new_commands_observations_
bool reapplied_desired_states_
CompletedCommandsTimeSeries waiting_for_completion_
CompletedCommandsTimeSeries completion_reported_
class o80::BoolState : public o80::SensorState
#include <bool_state.hpp>

A State encapsulating a boolean

Public Functions

BoolState()
BoolState(bool value)
void set(bool value)
bool get() const
std::string to_string() const
template<class Archive>
inline void serialize(Archive &archive)

Private Members

bool status_
class o80::Burster
#include <burster.hpp>

Class managing the bursting mode of BackEnd and Standalone. Expected usage:

Burster burster(segment_id);
while(true) {
    std::cout << "iterate!" << std::endl;
    burster.pulse()
}
The code above will wait until the method “burst” of a related FrontEnd is called, which triggers one iteration to occur.

Public Functions

Burster(std::string segment_id)
~Burster()
bool pulse()
Burster(std::segment_id)
~Burster()
void pulse()

Public Static Functions

static void clear_memory(std::string segment_id)

If an instance of Burster has not been cleanly exited (i.e. destructor not called), wipe the related shared memory (hanging at startup may otherwise occuring)

static void turn_on(std::string segment_id)

Turn on the bursting mode of the burster, i.e. it will iterate only when receiving a signal from the frontend.

static void turn_off(std::string segment_id)

Disable the bursting mode, i.e. the pulse method will always return immediately.

static void clear_memory(std::segment_id)

Private Functions

long int get_nb_bursts() const
void reset_nb_bursts()
bool should_run() const
void update_nb_bursts()
void reset_nb_bursts()

Private Members

std::string segment_id_
long int nb_bursts_
long int nb_iterated_
bool running_
std::shared_ptr<synchronizer::Follower> follower_
std::string object_id_
uint nb_bursts_
uint nb_iterated_
synchronizer::Follower follower_
class o80::BursterClient
#include <burster.hpp>

Client of Burster, i.e. used for commanding an instance of Burster of the same segment_id to perform a serie of pulses. Used internally by Frontend’s instances.

Public Types

typedef std::shared_ptr<synchronizer::Leader> LeaderPtr

Public Functions

BursterClient(std::string segment_id)
void burst(int nb_iterations)
void final_burst()

Private Functions

void set_bursting(int nb_iterations)

Private Members

synchronizer::Leader leader_
std::string segment_id_
class o80::Direct
#include <command_types.hpp>

Private Functions

template<class Archive>
inline void serialize(Archive &archive)
template<class IN, class OUT>
class o80::Driver
#include <driver.hpp>

Public Types

typedef IN DRIVER_IN
typedef OUT DRIVER_OUT

Public Functions

virtual void start() = 0
virtual void stop() = 0
virtual void set(const IN &in) = 0
virtual OUT get() = 0
class o80::Duration_us
#include <command_types.hpp>

! for interpolating toward the desired state during a specified duration

Public Functions

inline Duration_us()
inline Duration_us(long int _value)
template<class Archive>
inline void serialize(Archive &archive)

Public Members

long int value

Public Static Functions

static inline Duration_us seconds(long int value)
static inline Duration_us milliseconds(long int value)
static inline Duration_us microseconds(long int value)
static inline Duration_us nanoseconds(long int value)
class o80::FrequencyManager
#include <frequency_manager.hpp>

class for imposing a frequency to a process

Public Functions

FrequencyManager(double frequency)
Parameters

frequency – frequency to impose

long int wait()

will sleep for the duration required so that the the period that passed since the last call matches the desired frequency

Private Members

Nanoseconds period_
TimePoint previous_time_
timespec req_
class o80::FrequencyMeasure
#include <frequency_measure.hpp>

A class for evaluating the frequency at which a process run.

Public Functions

FrequencyMeasure()
double tick()
Returns

the frequency corresponding to the duration that passed since the previous call to tick

Private Members

TimePoint previous_time_
template<int QUEUE_SIZE, int NB_ACTUATORS, class ROBOT_STATE, class EXTENDED_STATE>
class o80::FrontEnd
#include <front_end.hpp>

A frontend sends commands to a related backend and read observations writen by this same backend.

tparam QUEUE_SIZE

size of the commands and observations time series

tparam NB_ACTUATORS

number of actuators of the robot

tparam ROBOT_STATE

class encapsulating the state of an actuator of the robot

tparam EXTENDED_STATE

class encapsulating supplementary arbitrary information

Public Types

typedef time_series::MultiprocessTimeSeries<Command<ROBOT_STATE>> CommandsTimeSeries

multiprocess time series hosting commands shared with the backend

typedef time_series::TimeSeries<Command<ROBOT_STATE>> BufferCommandsTimeSeries

time series buffering commands before their transfer to the commands time series

typedef time_series::MultiprocessTimeSeries<int> CompletedCommandsTimeSeries

multiprocess times series hosting the commands id that have been processed by the backend

typedef time_series::MultiprocessTimeSeries<Observation<NB_ACTUATORS, ROBOT_STATE, EXTENDED_STATE>> ObservationsTimeSeries

multprocess time series hosting the observations writen by the backend

typedef std::vector<Observation<NB_ACTUATORS, ROBOT_STATE, EXTENDED_STATE>> Observations

vector of observations

typedef std::shared_ptr<BursterClient> BursterClientPtr

for bursting mode support

Public Functions

FrontEnd(std::string segment_id)
Parameters

segment_id – should be the same for the backend and the frontend

~FrontEnd()
int get_nb_actuators() const

returns the number of actuators

bool observations_since(time_series::Index iteration, Observations &push_back_to)

Read from the shared memory all the observations starting from the specified iteration until the newest iteration and update the observations vector with them.

Parameters
  • iteration[in] iteration number of the backend

  • push_back_to[out] vector of observations to be updated.

bool update_latest_observations(size_t nb_items, Observations &push_back_to)

Read from the shared memory the latest nb_items observations update the observations vector with them.

Parameters
  • iteration[in] iteration number of the backend

  • push_back_to[out] vector of observations to be updated.

Observations get_observations_since(time_series::Index iteration)

Returns a vector of observations containing all observations starting from the specified iteration until the latest iteration

Parameters

iteration – iteration number

Observations get_latest_observations(size_t nb_items)

Returns a vector of observations containing the nb_items latest observations.

Parameters

iteration – number of observations to read

Observation<NB_ACTUATORS, ROBOT_STATE, EXTENDED_STATE> wait_for_next()

waiting for the next observation to be writen by the backend, then returning it. During the first call to this method, the current iteration is initialized as reference iteration, then the reference iteration will be increase by one at each call

bool backend_is_active()

if returns true: during its latest iteration, the backend did not reapply the previous desired states (i.e. at least one command was active), if false, the backend reapplied the previous desired state (no active command)

void reset_next_index()

reset the reference iteration used by the “wait_for_next” method to the current iteration number

void add_command(int nb_actuator, ROBOT_STATE target_state, Iteration target_iteration, Mode mode)

add a command to the buffer commands time series.

void add_command(int nb_actuator, ROBOT_STATE target_state, Duration_us duration, Mode mode)

add a command to the buffer commands time series.

void add_command(int nb_actuators, ROBOT_STATE target_state, Mode mode)

add a command to the buffer commands time series.

void add_command(int nb_actuator, ROBOT_STATE target_state, Speed speed, Mode mode)

add a command to the buffer commands time series.

void add_reinit_command()

add to each actuator an overwriting command with the initial state (as returned by the initial_states method) as target state

void purge() const

requests the backend to purge its command queues (including current commands) during next iteration

Observation<NB_ACTUATORS, ROBOT_STATE, EXTENDED_STATE> burst(int nb_iterations)

request the related backend or standalone to perform nb_iterations in a row, as fast as possible. Assumes the related backend or standalone is running in bursting mode

void final_burst()

Will trigger the related standalone to run one more iteration then exit. Assumes the standalone runs in bursting mode.

Observation<NB_ACTUATORS, ROBOT_STATE, EXTENDED_STATE> pulse(Iteration iteration)

write all buffered commands to the multiprocess time series commands (i.e. the related backend will read and execute them), then wait until the backend reaches the specified iteration, before returning the observation related to this iteration.

Observation<NB_ACTUATORS, ROBOT_STATE, EXTENDED_STATE> pulse()

write all buffered commands to the multiprocess time series commands (i.e. the related backend will read and execute them), then return the latest observation/

Observation<NB_ACTUATORS, ROBOT_STATE, EXTENDED_STATE> pulse_and_wait()

write all buffered commands to the multiprocess time series commands (i.e. the related backend will read and execute them), then return the latest observation once all commands have been completed

Observation<NB_ACTUATORS, ROBOT_STATE, EXTENDED_STATE> pulse_prepare_wait()

write all buffered commands to the multiprocess time series commands (i.e. the related backend will read and execute them), then return the latest observation. Optionaly, this call may be followed by a call to wait, which will be blocking until completion of all commands

Observation<NB_ACTUATORS, ROBOT_STATE, EXTENDED_STATE> wait()

If the previous call to this instance of frontend was a call to “pulse_prepare_wait”, a call to wait will be blocking until all commands added to the multiprocess time series commands have been executed. If following call to another of the “pulse” method, thows a runtime_error.

Observation<NB_ACTUATORS, ROBOT_STATE, EXTENDED_STATE> read(long int iteration = -1)

write all buffered commands to the multiprocess time series commands (i.e. the related backend will read and execute them), then wait for the backend to finish executation of all commands, then return the latest observation.

States<NB_ACTUATORS, ROBOT_STATE> initial_states() const

returns the first states ever observed by the backend

Public Static Functions

static auto get_introspection_commands(std::string segment_id)

returns the time series of commands shared between the frontend and the backend

static auto get_introspection_completed_commands(std::string segment_id)

returns the time series of (completed) command ids shared between the frontend and the backend

static auto get_introspection_waiting_for_completion(std::string segment_id)

returns the time series of command ids the frontend waits completion of

static auto get_introspection_completion_reported(std::string segment_id)

returns the time series of command ids the frontend processed reports of completion

Private Functions

void size_check()
time_series::Index last_index_read_by_backend()
void share_commands(std::set<int> &command_ids, bool store)
void wait_for_completion(std::set<int> &command_ids, time_series::Index completed_index)
void _print(CommandsTimeSeries *time_series)

Private Members

std::string segment_id_
time_series::Index history_index_
CommandsTimeSeries commands_
std::set<int> sent_command_ids_
long int pulse_id_
BufferCommandsTimeSeries buffer_commands_
time_series::Index buffer_index_
ObservationsTimeSeries observations_
time_series::Index observations_index_
CompletedCommandsTimeSeries completed_commands_
CompletedCommandsTimeSeries waiting_for_completion_
CompletedCommandsTimeSeries completion_reported_
int completed_index_
bool wait_prepared_
BursterClientPtr burster_client_
template<class ROBOT_STATE>
class o80::Introspector
#include <introspector.hpp>

Public Types

typedef time_series::MultiprocessTimeSeries<Command<ROBOT_STATE>> CommandsTimeSeries
typedef time_series::MultiprocessTimeSeries<int> CompletedCommandsTimeSeries

Public Functions

Introspector(std::string segment_id)
void start()
void stop()
template<class T>
void run(std::shared_ptr<T> time_series, std::string prefix)

Public Static Functions

static bool start_running(std::string segment_id)
static bool stop_running()

Private Functions

template<class T>
void run(std::shared_ptr<T> time_series, std::string prefix)
void run_commands()
void run_completed_commands()
void run_waiting_for_completion()
void run_completion_reported()
void run_received()
void run_starting()

Private Members

std::mutex mutex_
std::atomic<bool> running_
real_time_tools::RealTimeThread commands_thread_
real_time_tools::RealTimeThread completed_commands_thread_
real_time_tools::RealTimeThread waiting_for_completion_thread_
real_time_tools::RealTimeThread completion_reported_thread_
real_time_tools::RealTimeThread received_thread_
real_time_tools::RealTimeThread starting_thread_
std::shared_ptr<CommandsTimeSeries> commands_
std::shared_ptr<CompletedCommandsTimeSeries> completed_commands_
std::shared_ptr<CompletedCommandsTimeSeries> waiting_for_completion_
std::shared_ptr<CompletedCommandsTimeSeries> completion_reported_
std::shared_ptr<CompletedCommandsTimeSeries> received_
std::shared_ptr<CompletedCommandsTimeSeries> starting_

Private Static Attributes

static std::shared_ptr<Introspector<ROBOT_STATE>> instance_ = nullptr

Friends

friend friend THREAD_FUNCTION_RETURN_TYPE internal::run_commands_helper (void *arg)
friend friend THREAD_FUNCTION_RETURN_TYPE internal::run_completed_commands_helper (void *arg)
friend friend THREAD_FUNCTION_RETURN_TYPE internal::run_waiting_for_completion_helper (void *arg)
friend friend THREAD_FUNCTION_RETURN_TYPE internal::run_completion_reported_helper (void *arg)
friend friend THREAD_FUNCTION_RETURN_TYPE internal::run_received_helper (void *arg)
friend friend THREAD_FUNCTION_RETURN_TYPE internal::run_starting_helper (void *arg)
class o80::Iteration
#include <command_types.hpp>

! for interpolating toward the desired state such at reaching it a the specified iteration

Public Functions

inline Iteration()
inline Iteration(long int iteration)
inline Iteration(long int iteration, bool _relative)
inline Iteration(long int iteration, bool _relative, bool _do_reset)
inline void reset()
template<class Archive>
inline void serialize(Archive &archive)

Public Members

long int value
bool relative
bool do_reset
class o80::LogEntry
#include <logger.hpp>

Public Functions

inline LogEntry()
LogEntry(std::string segment_id, LogAction action)
template<class Archive>
inline void serialize(Archive &archive)

Public Members

char segment_id[segment_id_size]
LogAction action

Public Static Attributes

static constexpr int segment_id_size = {30}
class o80::Logger
#include <logger.hpp>

Public Functions

Logger(int queue_size, std::string segment_id, bool leader)
void log(std::string user_segment_id, LogAction action)
std::size_t length()
std::tuple<time_series::Timestamp, LogEntry> get(time_series::Index index, time_series::Index start_index = -1)
void save(std::string path)

Private Members

std::string segment_id_
time_series::MultiprocessTimeSeries<LogEntry> logs_
template<class Sub, typename ...Args>
class o80::MdState
#include <md_state.hpp>

A State represents the state of an actuator, as well as methods specifying how a state interpolates toward another. The interpolation (and finished) method will be used by the BackEnd to compute at each iteration the current desired state value for each actuator, based on commands sent by FrontEnd. E.g. if a command requests the desired state of an actuator to reach value “target” in 5 seconds, considering the current state of the actuator is “start”, then the interpolation methods will specified how the desired state value interpolates between “start” and “target”. By default linear interpolation methods are implemented, but they may be overriden for better control. The default linear interpolation will work only if the state value is of native type (e.g. double, float) and has to be overriden for any other type.

tparam T

value of the state

Public Functions

State(Args&&... args)
State()
template<int INDEX>
inline std::tuple_element<INDEX, std::tuple<Args...>>::type get()
template<int INDEX>
inline void set(typename std::tuple_element<INDEX, std::tuple<Args...>>::type value)
template<int INDEX>
T get() const
void set(T value)
std::string to_string() const
bool finished(const o80::TimePoint &start, const o80::TimePoint &now, const Sub &start_state, const Sub &current_state, const Sub &previous_desired_state, const Sub &target_state, const o80::Speed &speed) const
Sub intermediate_state(const o80::TimePoint &start, const o80::TimePoint &now, const Sub &start_state, const Sub &current_state, const Sub &previous_desired_state, const Sub &target_state, const o80::Speed &speed) const
Sub intermediate_state(const o80::TimePoint &start, const o80::TimePoint &now, const Sub &start_state, const Sub &current_state, const Sub &previous_desired_state, const Sub &target_state, const o80::Duration_us &duration) const
Sub intermediate_state(long int start_iteration, long int current_iteration, const Sub &start_state, const Sub &current_state, const Sub &previous_desired_state, const Sub &target_state, const o80::Iteration &iteration) const
template<class Archive>
inline void serialize(Archive &archive)

Public Members

std::tuple<Args...> values_
template<int NB_ACTUATORS, class ROBOT_STATE, class EXTENDED_STATE>
class o80::Observation
#include <observation.hpp>

! Encapsulate the current robot state, the desired robot state and an optional extended state.

It also encapsulate the current iteration, frequency and time stamp. Observations are created and written in the share memory by instances of BackEnd at each iteration, and read from the shared memory by instances of FrontEnd.

tparam NB_ACTUATORS

the number of actuators of the robot

tparam ROBOT_STATE

the class used to encapsulate robot states

tparam EXTENDED_STATE

arbitrary class used to encapsulate arbitrary information

Public Functions

Observation()
void copy(const Observation<NB_ACTUATORS, ROBOT_STATE, EXTENDED_STATE> &from, bool full)

copy the control iteration, sensor iteration and observed frequency from this to from. If full, also copies observed_states, desired_states, extended_states and time stamp.

Observation(const Observation<NB_ACTUATORS, ROBOT_STATE, EXTENDED_STATE> &other)
Observation(Observation<NB_ACTUATORS, ROBOT_STATE, EXTENDED_STATE> &&other) noexcept
Observation<NB_ACTUATORS, ROBOT_STATE, EXTENDED_STATE> &operator=(const Observation<NB_ACTUATORS, ROBOT_STATE, EXTENDED_STATE> &other)
Observation<NB_ACTUATORS, ROBOT_STATE, EXTENDED_STATE> &operator=(Observation<NB_ACTUATORS, ROBOT_STATE, EXTENDED_STATE> &&other) noexcept
Observation(States<NB_ACTUATORS, ROBOT_STATE> observed_state, States<NB_ACTUATORS, ROBOT_STATE> desired_state, long int stamp, long int iteration, double frequency)
Observation(States<NB_ACTUATORS, ROBOT_STATE> observed_state, States<NB_ACTUATORS, ROBOT_STATE> desired_state, long int stamp, long int control_iteration, long int sensor_iteration, double frequency)
Observation(States<NB_ACTUATORS, ROBOT_STATE> observed_state, States<NB_ACTUATORS, ROBOT_STATE> desired_state, EXTENDED_STATE extended_state, long int stamp, long int iteration, double frequency)
Observation(States<NB_ACTUATORS, ROBOT_STATE> observed_state, States<NB_ACTUATORS, ROBOT_STATE> desired_state, EXTENDED_STATE extended_state, long int stamp, long int control_iteration, long int sensor_iteration, double frequency)
const States<NB_ACTUATORS, ROBOT_STATE> &get_observed_states() const

returns the actual state of each actuator

const States<NB_ACTUATORS, ROBOT_STATE> &get_desired_states() const

returns the desired state of each actuator, as computed by the backend based on the command queue, and set to the robot

const EXTENDED_STATE &get_extended_state() const

returns an instance of extended state, which is an instance of an arbitrary class encaspulating arbitrary information

long int get_control_iteration() const

robot control iteration corresponding to this observation

long int get_sensor_iteration() const

robot sensor iteration corresponding to this observation

long int get_iteration() const

backend iteration corresponding to this observation

double get_frequency() const

frequency of backend at the iteration this observation was created

long int get_time_stamp() const

time stamp of the observation (in nanoseconds)

std::string to_string() const

returns a string description of the observation

template<class Archive>
inline void serialize(Archive &archive)

used internally to serialize the instances of observation when written in the shared memory

Protected Attributes

States<NB_ACTUATORS, ROBOT_STATE> observed_states_
States<NB_ACTUATORS, ROBOT_STATE> desired_states_
EXTENDED_STATE extended_state_
long int control_iteration_
long int sensor_iteration_
long int stamp_
long int iteration_
double observed_frequency_
class SensorState
#include <sensor_state.hpp>

Subclassed by o80::BoolState, o80::VoidState

class o80::Speed
#include <command_types.hpp>

! for interpolating toward the desired state using a specified velocity.

Public Functions

inline Speed()
inline Speed(double _value)
template<class Archive>
inline void serialize(Archive &archive)

Public Members

double value

Public Static Functions

static inline Speed per_second(double value)
static inline Speed per_microsecond(double value)
static inline Speed per_millisecond(double value)
static inline Speed per_nanosecond(double value)
template<int QUEUE_SIZE, int NB_ACTUATORS, class DRIVER, class o80_STATE, class o80_EXTENDED_STATE>
class o80::Standalone
#include <standalone.hpp>

! A Standalone encapsulates and organize the the communication between a (robot_interfaces) driver, a (robot_interfaces) frontend, a (robot_interfaces) backend and a (o80) backend : at each iteration, information about the current state of the robot actuators are passed by the frontend to the backend, which passes to the driver desired states values.

tparam QUEUE_SIZE

number of commands that can be hosted in the command queue at any point of time. Exceptions will be thrown if more commands are queued.

tparam NB_ACTUATORS

number of actuators of the robot

tparam RI_ACTION

template to the (robot_interfaces) frontend

tparam ROBOT_OUT

template to the (robot_interfaces) frontend

tparam o80_STATE

template to the (o80) BackEnd

tparam EXTENDED_STATE

class encapsulating supplementary arbitrary information (i.e. supplementary to the current actuators state) that will be sent from the (robot_interfaces) frontend to the backend (which will add these information to the observations)

Public Types

typedef std::shared_ptr<DRIVER> DriverPtr
typedef o80_STATE o80State
typedef o80_EXTENDED_STATE o80ExtendedState
typedef DRIVER o80DRIVER

Public Functions

Standalone(DriverPtr driver_ptr, double frequency, std::string segment_id)

Creates instances of:

  • (robot_interfaces) backend

  • (robot interfaces) frontend

  • (o80) BackEnd

Parameters
  • ri_driver – robot_interfaces robot driver

  • frequency – desired frequency

  • segment_id – shared memory segment id for o80 BackEnd.

~Standalone()
void start()

! Starts the robot interfaces backend

void stop()

! Stops the robot interfaces backend

bool spin(bool bursting = false)

! - If bursting is false, performs one iteration and then wait for the time requied to match the desired frequency.

  • If bursting is true, hang until the o80 FrontEnd calls “burst”.

bool spin(o80_EXTENDED_STATE &extended_state, bool bursting = false)

! Similar to spin, and the extended state is added to the observation that is written to the shared memory by the o80 BackEnd.

virtual o80::States<NB_ACTUATORS, o80_STATE> convert(const typename DRIVER::DRIVER_OUT &observation) = 0

! user code to convert the observation read from robot_interfaces frontend into the current o80 States.

virtual DRIVER::DRIVER_IN convert(const o80::States<NB_ACTUATORS, o80_STATE> &states) = 0

! user code to convert the desired states generated by the o80 BackEnd into action required by the robot_interfaces frontend.

inline virtual void enrich_extended_state(o80_EXTENDED_STATE &extended_state, const typename DRIVER::DRIVER_OUT &observation)

! converts the observation as returned by the robot_interfaces frontend into the o80 extended state (optional)

Public Static Attributes

static constexpr int queue_size = QUEUE_SIZE
static constexpr int nb_actuators = NB_ACTUATORS

Private Types

typedef BackEnd<QUEUE_SIZE, NB_ACTUATORS, o80_STATE, o80_EXTENDED_STATE> o80Backend

Private Functions

bool iterate(const TimePoint &time_now, o80_EXTENDED_STATE &extended_state)

Private Members

double frequency_
Microseconds period_
FrequencyManager frequency_manager_
TimePoint now_
std::shared_ptr<Burster> burster_
std::string segment_id_
DriverPtr driver_ptr_
o80Backend o8o_backend_
template<class RobotDriver, class o80Standalone>
class o80::StandaloneRunner
#include <standaloneRunner.hpp>

Public Functions

inline StandaloneRunner(std::string segment_id, double max_action_duration_s, double max_inter_action_duration_s, double frequency, bool bursting)
inline ~StandaloneRunner()
inline void start()
inline void stop()
inline void run()

Private Members

bool bursting_
std::atomic<bool> running_
real_time_tools::Thread thread_
RobotDriver driver_
o80Standalone standalone_
template<typename T, class Sub>
class o80::State
#include <state.hpp>

A State represents the state of an actuator, as well as methods specifying how a state interpolates toward another. The interpolation (and finished) method will be used by the BackEnd to compute at each iteration the current desired state value for each actuator, based on commands sent by FrontEnd. E.g. if a command requests the desired state of an actuator to reach value “target” in 5 seconds, considering the current state of the actuator is “start”, then the interpolation methods will specified how the desired state value interpolates between “start” and “target”. By default linear interpolation methods are implemented, but they may be overriden for better control. The default linear interpolation will work only if the state value is of native type (e.g. double, float) and has to be overriden for any other type.

tparam T

value of the state

Public Functions

State(T value)
State()
T get() const
void set(T value)
std::string to_string() const
bool finished(const o80::TimePoint &start, const o80::TimePoint &now, const Sub &start_state, const Sub &current_state, const Sub &previous_desired_state, const Sub &target_state, const o80::Speed &speed) const
Sub intermediate_state(const o80::TimePoint &start, const o80::TimePoint &now, const Sub &start_state, const Sub &current_state, const Sub &previous_desired_state, const Sub &target_state, const o80::Speed &speed) const
Sub intermediate_state(const o80::TimePoint &start, const o80::TimePoint &now, const Sub &start_state, const Sub &current_state, const Sub &previous_desired_state, const Sub &target_state, const o80::Duration_us &duration) const
Sub intermediate_state(long int start_iteration, long int current_iteration, const Sub &start_state, const Sub &current_state, const Sub &previous_desired_state, const Sub &target_state, const o80::Iteration &iteration) const
template<class Archive>
inline void serialize(Archive &archive)

Public Members

T value
class o80::State1d : public o80::State<double, State1d>
#include <state1d.hpp>

Public Functions

inline State1d()
inline State1d(double value)
template<int NB_ACTUATORS, class STATE>
class o80::States
#include <states.hpp>

! States is a container of instances of NB_ACTUATORS instances of STATE.

Expected usage is that STATE encapsulate the state of a specific actuator, hence States represents the full robot state.

tparam NB_ACTUATOR

the number of actuators of the robot

tparam STATE

class representing an actuator state

Public Functions

inline States()
template<class Archive>
inline void serialize(Archive &archive)
void set(int actuator, STATE state)

! set the state for the specified actuator

const STATE &get(int actuator) const

! returns the state of the specified actuator

Public Members

std::array<STATE, NB_ACTUATORS> values
template<typename ...Args>
class o80::StateXd
#include <statexd.hpp>

Similarly to an instance of State, an instance of StateXd represents the state of an actuator, and provides methods for iterpolating between states. The difference with State is that StateXd supports several attributes, e.g.

class Joint2d : public StateXd<Joint2d,int,double>
Joint2d will encapsulate an int and a double attribute.

Public Functions

StateXd(Args... args)
StateXd()
template<int INDEX>
std::tuple_element<INDEX, std::tuple<Args...>>::type get() const

returns the INDEXth attributes

template<int INDEX>
void set(typename std::tuple_element<INDEX, std::tuple<Args...>>::type value)

set the INDEXth attribute

std::string to_string() const
bool finished(const o80::TimePoint &start, const o80::TimePoint &now, const StateXd<Args...> &start_state, const StateXd<Args...> &current_state, const StateXd<Args...> &previous_desired_state, const StateXd<Args...> &target_state, const o80::Speed &speed) const

returns true if the speed command finished for the attribute at the first (0) index

StateXd<Args...> intermediate_state(const o80::TimePoint &start, const o80::TimePoint &now, const StateXd<Args...> &start_state, const StateXd<Args...> &current_state, const StateXd<Args...> &previous_desired_state, const StateXd<Args...> &target_state, const o80::Speed &speed) const
StateXd<Args...> intermediate_state(const o80::TimePoint &start, const o80::TimePoint &now, const StateXd<Args...> &start_state, const StateXd<Args...> &current_state, const StateXd<Args...> &previous_desired_state, const StateXd<Args...> &target_state, const o80::Duration_us &duration) const
StateXd<Args...> intermediate_state(long int start_iteration, long int current_iteration, const StateXd<Args...> &start_state, const StateXd<Args...> &current_state, const StateXd<Args...> &previous_desired_state, const StateXd<Args...> &target_state, const o80::Iteration &iteration) const
template<class Archive>
inline void serialize(Archive &archive)
template<int INDEX>
std::tuple_element<INDEX, std::tuple<Args...>>::type get() const
template<int INDEX>
void set(typename std::tuple_element<INDEX, std::tuple<Args...>>::type value)

Private Members

std::tuple<Args...> values_
class o80::VoidExtendedState
#include <void_extended_state.hpp>

Public Functions

inline VoidExtendedState()
inline void console() const
template<class Archive>
inline void serialize(Archive &archive)

Public Members

char foo
class o80::VoidState : public o80::SensorState
#include <void_state.hpp>

Public Functions

VoidState()
std::string to_string() const
inline void get() const
inline void set() const
template<class Archive>
inline void serialize(Archive &archive)
namespace internal

Functions

template<class ROBOT_STATE>
THREAD_FUNCTION_RETURN_TYPE run_commands_helper(void *arg)
template<class ROBOT_STATE>
THREAD_FUNCTION_RETURN_TYPE run_completed_commands_helper(void *arg)
template<class ROBOT_STATE>
THREAD_FUNCTION_RETURN_TYPE run_waiting_for_completion_helper(void *arg)
template<class ROBOT_STATE>
THREAD_FUNCTION_RETURN_TYPE run_completion_reported_helper(void *arg)
template<class ROBOT_STATE>
THREAD_FUNCTION_RETURN_TYPE run_received_helper(void *arg)
template<class ROBOT_STATE>
THREAD_FUNCTION_RETURN_TYPE run_starting_helper(void *arg)
template<typename T, typename ...Ts>
constexpr bool has_type()
template<size_t INDEX = 0, typename INCR, typename TUPLE, size_t SIZE = std::tuple_size_v<std::remove_reference_t<TUPLE>>, typename COMMAND_TYPE>
void intermediates(INCR &&start, INCR &&now, const TUPLE &start_state, const TUPLE &previous_desired_state, const TUPLE &target_state, const COMMAND_TYPE &command, TUPLE &interpolated_state, bool use_duration = false)
template<size_t INDEX = 0, typename TUPLE, size_t SIZE = std::tuple_size_v<std::remove_reference_t<TUPLE>>>
bool all_finished(const o80::TimePoint &start, const o80::TimePoint &now, const TUPLE &start_state, const TUPLE &previous_desired_state, const TUPLE &target_state, const o80::Speed &speed)
template<std::size_t INDEX = 0, typename TUPLE, size_t SIZE = std::tuple_size_v<std::remove_reference_t<TUPLE>>>
void to_string(std::stringstream &stream, const TUPLE &tuple)
namespace o80

Typedefs

typedef std::integral_constant<int, 0> NO_STATES
typedef std::integral_constant<int, 1> NO_STATE
typedef std::integral_constant<int, 2> NO_OBSERVATION
typedef std::integral_constant<int, 3> NO_SERIALIZER
typedef std::integral_constant<int, 4> NO_EXTENDED_STATE
typedef std::integral_constant<int, 5> NO_FRONTEND
typedef std::integral_constant<int, 6> NO_BACKEND
typedef std::integral_constant<int, 7> NO_INTROSPECTOR
typedef StateXd<double, double> State2d
typedef StateXd<double, double, double> State3d
typedef std::chrono::seconds Seconds
typedef std::chrono::milliseconds Milliseconds
typedef std::chrono::microseconds Microseconds
typedef std::chrono::nanoseconds Nanoseconds
typedef std::chrono::nanoseconds TimePoint
typedef std::chrono::steady_clock Clock

Enums

enum Type

! commands type, used by the add_commands methods of FrontEnd.

  • duration : will try to reach target state over the given duration

  • speed : will try to reach target state using the specified speed

  • direct : will request to set the target state direcly

  • iteration: will request for the target state to be reached at the provided backend iteration.

Values:

enumerator DURATION
enumerator SPEED
enumerator DIRECT
enumerator ITERATION
enum LogAction

Values:

enumerator FRONTEND_WAIT_START
enumerator FRONTEND_WAIT_END
enumerator FRONTEND_COMMUNICATE
enumerator FRONTEND_READ
enumerator FRONTEND_COMPLETION_WAIT_START
enumerator FRONTEND_COMPLETION_WAIT_END
enumerator BACKEND_READ
enumerator BACKEND_WRITE_REAPPLY
enumerator BACKEND_WRITE_NEW
enum Mode

Possible mode for a command.

queue : will run after all previous commands (corresponding to the same actuator) finished overwrite : will stop and cancel all previous commands and run immediately

Values:

enumerator QUEUE
enumerator OVERWRITE

Functions

template<typename T>
bool finished(const o80::TimePoint &start, const o80::TimePoint &now, const T &start_state, const T &current_state, const T &target_state, const o80::Speed &speed)
template<typename T>
T intermediate_state(const o80::TimePoint &start, const o80::TimePoint &now, const T &start_state, const T &current_state, const T &target_state, const o80::Speed &speed)

Interpolate between start and target state so that the state changes according to the provided speed.

template<typename T>
T intermediate_state(const o80::TimePoint &start, const o80::TimePoint &now, const T &start_state, const T &current_state, const T &target_state, const o80::Duration_us &duration)

Interpolate between start and target state so that the target state is reached in the specified duration.

template<typename T>
T intermediate_state(long int start_iteration, long int current_iteration, const T &start_state, const T &current_state, const T &target_state, const o80::Iteration &iteration)

Interpolate between start and target state so that the target state is reached at the specified iteration.

void clear_shared_memory(std::string segment_id)

clear all the shared memory segments corresponding to the specified segment id. The destructor of BackEnd calls this function, but user code may call it in case a program has been abruptly terminated (i.e. the BackEnd ‘s destructor has not been called). Starting a BackEnd using a segment id that has not been properly cleared will result in the program hanging.

template<int QUEUE_SIZE, int NB_ACTUATORS, class o80_STATE, class o80_EXTENDED_STATE, typename ...EXCLUDED_CLASSES>
void create_python_bindings(pybind11::module &m, std::string prefix = std::string(""))

! Creates in python module m python bindings for the classes States, State, ExtendedState, Observation, FrontEnd and Backend.

template<class RobotStandalone, typename ...EXCLUDED_CLASSES>
void create_python_bindings(pybind11::module &m, std::string prefix = std::string(""))

! Creates in python module m python bindings for the classes States, State, ExtendedState, Observation, FrontEnd and Backend.

template<class RobotDriver, class RobotStandalone, typename ...DriverArgs>
void create_standalone_python_bindings(pybind11::module &m, std::string prefix = std::string(""))

! Creates the python bindings for the classes States, State, ExtendedState, Observation, FrontEnd and Backend (except of the one inhibited by the configuration), as well as to the class Standalone and of the functions start_standalone, stop_standalone, standalone_is_running and please_stop.

Template Parameters

DriverArgs – : argument list required to instantiate RobotDriver.

void please_stop(std::string segment_id)

! if an instance of Standalone of the related segment_id is running, will send a stop request to this standalone.

template<class RobotDriver, class o80Standalone, typename ...Args>
void start_action_timed_standalone(std::string segment_id, double frequency, bool bursting, Args&&... args)
template<class RobotDriver, class o80Standalone, typename ...Args>
void start_standalone(std::string segment_id, double frequency, bool bursting, Args&&... args)

instantiates instances of (robot_interfaces) RobotDriver and of (o80) Standalone, and starts them in a thread.

A runtime exception is thrown if another standalone of the same segment_id has already been started. tparam Args template arguments of the driver param segment id o80 BackEnd segment id param frequency frequency at which the standalone will iterate (non bursting mode) param bursting if true, will run in bursting mode, i.e. the standalone will iterate only when the o80 frontend calls its burst function. param args arguments for the driver

void stop_standalone(std::string segment_id)

! Stop the standalone of the specified segment_id.

A runtime error is thrown if no such standalone is running.

bool standalone_is_running(std::string segment_id)

! Returns true if the standalone is iterating.

(returns also false if the standalone does not exist)

template<class RobotDriver, class o80Standalone>
THREAD_FUNCTION_RETURN_TYPE run_helper(void *arg)
TimePoint time_now()

! return current time with nanoseconds precision

long int time_diff(const TimePoint &before, const TimePoint &after)

! returns the duration between after and before, in nanoseconds

long int time_diff_us(const TimePoint &before, const TimePoint &after)

! returns the duration between after and before, in microseconds

namespace o80::internal

Functions

template<class ROBOT_STATE>
THREAD_FUNCTION_RETURN_TYPE run_commands_helper(void *arg)
template<class ROBOT_STATE>
THREAD_FUNCTION_RETURN_TYPE run_completed_commands_helper(void *arg)
template<class ROBOT_STATE>
THREAD_FUNCTION_RETURN_TYPE run_waiting_for_completion_helper(void *arg)
template<class ROBOT_STATE>
THREAD_FUNCTION_RETURN_TYPE run_completion_reported_helper(void *arg)
template<class ROBOT_STATE>
THREAD_FUNCTION_RETURN_TYPE run_received_helper(void *arg)
template<class ROBOT_STATE>
THREAD_FUNCTION_RETURN_TYPE run_starting_helper(void *arg)
file clear_memory.cpp
#include <iostream>#include <string>#include “o80/memory_clearing.hpp”

Functions

void clear_memory(std::string segment_id)
int main(int argc, char *argv[])
file demo_burster.cpp
#include <unistd.h>#include <iostream>#include “o80/burster.hpp”

Functions

void run()
int main()
file demo_burster_client.cpp
#include <unistd.h>#include “o80/burster.hpp”

Functions

void run()
int main()
file back_end.hpp
#include <type_traits>#include “o80/frequency_measure.hpp”#include “o80/logger.hpp”#include “o80/memory_clearing.hpp”#include “o80/observation.hpp”#include “o80/sensor_state.hpp”#include “o80/states.hpp”#include “o80_internal/controllers_manager.hpp”#include “time_series/multiprocess_time_series.hpp”#include “back_end.hxx”
file back_end.hxx

Defines

TEMPLATE_BACKEND
BACKEND
file bool_state.hpp
#include <string>#include “o80/sensor_state.hpp”
file burster.hpp
#include <memory>#include “shared_memory/shared_memory.hpp”#include “synchronizer/follower.hpp”#include “synchronizer/leader.hpp”
file bursting.hpp
#include “shared_memory/shared_memory.hpp”#include “synchronizer/follower.hpp”
file command_types.hpp
#include “o80_internal/time_stamp.hpp”
file driver.hpp
file frequency_manager.hpp
#include <time.h>#include <chrono>#include “o80/time.hpp”
file frequency_measure.hpp
#include <chrono>#include “o80/time.hpp”
file front_end.hpp
#include <memory>#include <vector>#include “burster.hpp”#include “o80_internal/command.hpp”#include “observation.hpp”#include “shared_memory/shared_memory.hpp”#include “synchronizer/leader.hpp”#include “time_series/multiprocess_time_series.hpp”#include “time_series/time_series.hpp”#include “front_end.hxx”
file front_end.hxx

Defines

TEMPLATE_FRONTEND
FRONTEND
file interpolation.hpp
#include <iostream>#include “o80/command_types.hpp”#include “o80/time.hpp”#include “interpolation.hxx”
file interpolation.hxx

Functions

template<int>
int cast(double v)
template<long int>
long int cast(double v)
template<typename T>
T cast(double v)
template<typename T>
bool finished(const o80::TimePoint &start, const o80::TimePoint &now, const T &start_state, const T &current_state, const T &target_state, const o80::Speed &speed)
template<typename T>
T intermediate_state(const o80::TimePoint &start, const o80::TimePoint &now, const T &start_state, const T &current, const T &target_state, const o80::Speed &speed)

Interpolate between start and target state so that the state changes according to the provided speed.

template<typename T>
T intermediate_state(const o80::TimePoint &start, const o80::TimePoint &now, const T &start_state, const T &current, const T &target_state, const o80::Duration_us &duration)

Interpolate between start and target state so that the target state is reached in the specified duration.

template<typename T>
T intermediate_state(long int iteration_start, long int iteration_now, const T &start_state, const T &current_state, const T &target_state, const o80::Iteration &iteration)

Interpolate between start and target state so that the target state is reached at the specified iteration.

file introspector.hpp
#include <atomic>#include <chrono>#include <memory>#include <mutex>#include <real_time_tools/thread.hpp>#include “o80/front_end.hpp”#include “introspector.hxx”
file introspector.hxx

Functions

template<class T>
std::string to_string(const T &element)
file logger.hpp
#include <fstream>#include <iostream>#include <stdexcept>#include <string>#include <tuple>#include “shared_memory/serializer.hpp”#include “time_series/multiprocess_time_series.hpp”
file md_state.hpp
#include <string>#include <tuple>#include “o80/interpolation.hpp”#include “state.hxx”
file memory_clearing.hpp
#include “o80/burster.hpp”#include “shared_memory/shared_memory.hpp”#include “time_series/multiprocess_time_series.hpp”
file mode.hpp
file observation.hpp
#include <sstream>#include “shared_memory/serializer.hpp”#include “states.hpp”#include “time.hpp”#include “void_extended_state.hpp”#include “observation.hxx”
file observation.hxx

Defines

TEMPLATE_OBSERVATION
OBSERVATION
file pybind11_helper.hpp
#include <o80/burster.hpp>#include <o80/command_types.hpp>#include <o80/front_end.hpp>#include <o80/introspector.hpp>#include <o80/mode.hpp>#include <o80/observation.hpp>#include <o80/standalone.hpp>#include <o80/states.hpp>#include <pybind11/pybind11.h>#include <pybind11/stl.h>#include <pybind11/stl_bind.h>#include <string>#include <type_traits>#include “pybind11_helper.hxx”
file pybind11_helper.hxx

Functions

template<int QUEUE_SIZE, int NB_ACTUATORS, class o80_STATE, class o80_EXTENDED_STATE, typename ...EXCLUDED_CLASSES>
void create_python_bindings(pybind11::module &m, std::string prefix)

! Creates in python module m python bindings for the classes States, State, ExtendedState, Observation, FrontEnd and Backend.

template<class RobotStandalone, typename ...EXCLUDED_CLASSES>
void create_python_bindings(pybind11::module &m, std::string prefix)

! Creates in python module m python bindings for the classes States, State, ExtendedState, Observation, FrontEnd and Backend.

template<class RobotDriver, class RobotStandalone, typename ...DriverArgs>
void create_standalone_python_bindings(pybind11::module &m, std::string prefix)

! Creates the python bindings for the classes States, State, ExtendedState, Observation, FrontEnd and Backend (except of the one inhibited by the configuration), as well as to the class Standalone and of the functions start_standalone, stop_standalone, standalone_is_running and please_stop.

Template Parameters

DriverArgs – : argument list required to instantiate RobotDriver.

file sensor_state.hpp
file standalone.hpp
#include “o80/back_end.hpp”#include “o80/burster.hpp”#include “o80/driver.hpp”#include “o80/frequency_manager.hpp”#include “o80/observation.hpp”#include “o80/time.hpp”#include “o80_internal/standalone_runner.hpp”#include “synchronizer/leader.hpp”#include “standalone.hxx”
file standalone.hxx

Defines

TEMPLATE_STANDALONE
STANDALONE

Functions

static long int get_bursting(const std::string &segment_id)
static void reset_bursting(const std::string &segment_id)
template<class Driver, class o80Standalone, typename ...Args>
void start_action_timed_standalone(std::string segment_id, double frequency, bool bursting, Args&&... args)
template<class Driver, class o80Standalone, typename ...Args>
void start_standalone(std::string segment_id, double frequency, bool bursting, Args&&... args)

instantiates instances of (robot_interfaces) RobotDriver and of (o80) Standalone, and starts them in a thread.

A runtime exception is thrown if another standalone of the same segment_id has already been started. tparam Args template arguments of the driver param segment id o80 BackEnd segment id param frequency frequency at which the standalone will iterate (non bursting mode) param bursting if true, will run in bursting mode, i.e. the standalone will iterate only when the o80 frontend calls its burst function. param args arguments for the driver

bool standalone_is_running(std::string segment_id)

! Returns true if the standalone is iterating.

(returns also false if the standalone does not exist)

void stop_standalone(std::string segment_id)

! Stop the standalone of the specified segment_id.

A runtime error is thrown if no such standalone is running.

file standaloneRunner.hpp
#include <o80/standalone.hpp>#include <real_time_tools/thread.hpp>
file state.hpp
#include <string>#include “o80/interpolation.hpp”#include “state.hxx”
file state.hxx
file state1d.hpp
#include “o80/state.hpp”
file state2d.hpp
#include “o80/statexd.hpp”
file state3d.hpp
#include “o80/statexd.hpp”
file states.hpp
#include <array>#include “states.hxx”
file states.hxx
file statexd.hpp
#include <string>#include <tuple>#include <utility>#include “o80/interpolation.hpp”#include “statexd.hxx”
file statexd.hxx
file time.hpp
#include <chrono>#include <eigen3/Eigen/Core>
file void_extended_state.hpp
file void_state.hpp
#include “o80/command_types.hpp”#include “o80/sensor_state.hpp”#include “o80/time.hpp”
file bool_state.cpp
#include “o80/bool_state.hpp”
file burster.cpp
#include “o80/burster.hpp”
file command_type.cpp
#include “o80_internal/command_type.hpp”
file frequency_manager.cpp
#include “o80/frequency_manager.hpp”#include <iostream>
file frequency_measure.cpp
#include “o80/frequency_measure.hpp”#include <iostream>
file logger.cpp
#include “o80/logger.hpp”
file time.cpp
#include “o80/time.hpp”#include <iostream>
file time_stamp.cpp
#include “o80_internal/time_stamp.hpp”
file void_state.cpp
#include “o80/void_state.hpp”
file wrappers.cpp
#include “o80/back_end.hpp”#include “o80/bool_state.hpp”#include “o80/burster.hpp”#include “o80/command_types.hpp”#include “o80/frequency_manager.hpp”#include “o80/frequency_measure.hpp”#include “o80/memory_clearing.hpp”#include “o80/pybind11_helper.hpp”#include “o80/state1d.hpp”#include “o80/state2d.hpp”#include “o80/state3d.hpp”#include “o80/time.hpp”

Functions

PYBIND11_MODULE(o80, m)
dir bin
dir demos
dir include
dir include/o80
dir src
dir srcpy