YARP
Yet Another Robot Platform
PriorityCarrier.h
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: BSD-3-Clause
4 */
5
6#ifndef PRIORITYCARRIER_INC
7#define PRIORITYCARRIER_INC
8
9#include <cmath>
11#include <yarp/os/Election.h>
13#include <yarp/os/Semaphore.h>
14#include <yarp/os/Time.h>
17
18#include <yarp/sig/Vector.h>
19#include <yarp/sig/Matrix.h>
20
21#define STIMUL_THRESHOLD 1.0
22#define WITH_PRIORITY_DEBUG
23
24class PriorityCarrier;
25
30 public yarp::os::PeerRecord<PriorityCarrier>
31{
32public:
33 virtual ~PriorityGroup() {}
35 PriorityCarrier *source);
36 bool recalculate(double t);
37
38public:
39 yarp::sig::Matrix InvA; // the inverse of matrix (I-A) in the equation y(t) = [(I-A)^(-1) * B] .*x(t)
40 yarp::sig::Matrix B; // matrix of biases B in the equation y(t) = [(I-A)^(-1) * B] .*x(t)
41 yarp::sig::Matrix Y; // matrix y(t)
42 yarp::sig::Matrix X; // matrix x(t)
43 //yarp::os::Semaphore semDebug; // this semaphor is used only when debug mode is active
44 // to control the access to matrices from debug thread
45};
46
47
48
49#ifdef WITH_PRIORITY_DEBUG
52{
53public:
55 ~PriorityDebugThread() override;
56 void run() override;
57 bool threadInit() override;
58 void threadRelease() override;
59
60public:
61 int count;
63 std::string debugPortName;
65};
66#endif //WITH_PRIORITY_DEBUG
67
68
77{
78
79#ifdef WITH_PRIORITY_DEBUG
80 friend class PriorityDebugThread;
81#endif //WITH_PRIORITY_DEBUG
82
83public:
85#ifdef WITH_PRIORITY_DEBUG
86 : debugger(this)
87#endif //WITH_PRIORITY_DEBUG
88 {
89 group = 0/*NULL*/;
91 timeResting = 0;
92 stimulation = 0;
94 isVirtual = false;
95 isActive = false;
96 bias = 0;
97 yi = 0; // used in debug
98 }
99
101 if (portName!="") {
102 // let peer carriers know I'm gone.
103 getPeers().remove(portName,this);
104 }
105 }
106
107 Carrier *create() const override {
108 return new PriorityCarrier();
109 }
110
111 std::string getName() const override {
112 return "priority";
113 }
114
115 std::string toString() const override {
116 return "priority_carrier";
117 }
118
119 bool isResting(double priority) {
120 return ((timeResting > 0) &&
121 ((priority < 0) || (priority >= STIMUL_THRESHOLD)));
122 }
123
124
125 void stimulate(double t) {
126
128 // if it is in active state, stimulates.
130 {
132 // respecting priority ceiling
134 {
136 isActive = true;
137 }
138 // updating arrival time
139 timeArrival = t;
140 }
141 }
142
143 double getActualStimulation(double t);
144
145 double getActualInput(double t);
146
147 bool configure(yarp::os::ConnectionState& proto) override;
148
149 bool acceptIncomingData(yarp::os::ConnectionReader& reader) override;
150
151 void setCarrierParams(const yarp::os::Property& params) override {
152 yarp::os::Property property = params;
153 timeConstant = property.check("tc", yarp::os::Value(timeConstant)).asFloat64();
154 timeResting = property.check("tr", yarp::os::Value(timeResting)).asFloat64();
155 stimulation = property.check("st", yarp::os::Value(stimulation)).asFloat64();
156 bias = property.check("bs", yarp::os::Value(bias)).asFloat64();
157 isVirtual = property.check("virtual", yarp::os::Value(isVirtual)).asBool();
158 if(property.check("ex")) {
159 excitation = property.findGroup("ex");
160 }
161 }
162
163 void getCarrierParams(yarp::os::Property& params) const override {
164 params.put("tc", timeConstant);
165 params.put("tr", timeResting);
166 params.put("st", stimulation);
167 params.put("bs", bias);
168 params.put("virtual", (int)isVirtual);
169 params.put("ex", excitation.toString());
170 }
171
172
173
174public:
175 double timeConstant; // priority reset time (tc > 0)
176 double timeResting; // priority resting time (tr > 0)
177 double stimulation; // stimulation value of the message (s > 0)
178 double temporalStimulation; // current priority of the message S(t)
179 double timeArrival; // arrival time of the message
180 bool isVirtual; // a virtual link does not carry any data
181 bool isActive; // true if port is in active state X(t)
182 double bias; // bias value for excitation
183 yarp::os::Bottle excitation; // a list of exitatory signals as (name, value)
184 std::string sourceName;
185
186 double yi; // this is set in the recalculate() for the debug purpose
187
188private:
189 std::string portName;
190 PriorityGroup *group;
191
193
194 static yarp::os::ElectionOf<PriorityGroup>& getPeers();
195
196#ifdef WITH_PRIORITY_DEBUG
197private:
198 PriorityDebugThread debugger;
199#endif //WITH_PRIORITY_DEBUG
200};
201
202#endif // PRIORITYCARRIER_INC
float t
contains the definition of a Matrix type
#define STIMUL_THRESHOLD
contains the definition of a Vector type
Allow priority-based message selection.
std::string sourceName
bool configure(yarp::os::ConnectionState &proto) override
Give carrier a shot at looking at how the connection is set up.
void getCarrierParams(yarp::os::Property &params) const override
Get carrier configuration and deliver it by port administrative commands.
std::string toString() const override
Get name of carrier.
double getActualInput(double t)
Carrier * create() const override
Factory method.
void setCarrierParams(const yarp::os::Property &params) override
Configure carrier from port administrative commands.
double temporalStimulation
void stimulate(double t)
std::string getName() const override
Get the name of this connection type ("tcp", "mcast", "shmem", ...)
bool isResting(double priority)
bool acceptIncomingData(yarp::os::ConnectionReader &reader) override
Determine whether incoming data should be accepted.
yarp::os::Bottle excitation
virtual ~PriorityCarrier()
double getActualStimulation(double t)
yarp::os::BufferedPort< yarp::sig::Vector > debugPort
~PriorityDebugThread() override
void threadRelease() override
Release method.
void run() override
Loop function.
PriorityCarrier * pcarrier
bool threadInit() override
Initialization method.
PriorityDebugThread(PriorityCarrier *carrier)
Class PriorityDebugThread.
std::string debugPortName
Manager for priority-aware inputs to a given port.
yarp::sig::Matrix X
virtual ~PriorityGroup()
virtual bool acceptIncomingData(yarp::os::ConnectionReader &reader, PriorityCarrier *source)
bool recalculate(double t)
Class PriorityGroup.
yarp::sig::Matrix Y
yarp::sig::Matrix InvA
yarp::sig::Matrix B
A simple collection of objects that can be described and transmitted in a portable way.
Definition: Bottle.h:64
Bottle & findGroup(const std::string &key) const override
Gets a list corresponding to a given keyword.
Definition: Bottle.cpp:302
std::string toString() const override
Gives a human-readable textual representation of the bottle.
Definition: Bottle.cpp:211
An interface for reading from a network connection.
The basic state of a connection - route, streams in use, etc.
void remove(const std::string &key, typename PR::peer_type *entity)
Definition: Election.h:98
A starter class for implementing simple modifying carriers.
An abstraction for a periodic thread.
A class for storing options and configuration information.
Definition: Property.h:33
void put(const std::string &key, const std::string &value)
Associate the given key with the given string.
Definition: Property.cpp:1015
A single value (typically within a Bottle).
Definition: Value.h:43
A class for a Matrix.
Definition: Matrix.h:39