30void ControlBoard_nws_yarp::closePorts()
37 inputStreamingPort.
close();
40 outputPositionStatePort.
close();
43 extendedOutputStatePort.
close();
83 if (!outputPositionStatePort.
open(
rootName +
"/state:o")) {
90 if (!extendedOutputStatePort.
open(
rootName +
"/stateExt:o")) {
95 extendedOutputState_buffer.
attach(extendedOutputStatePort);
99 if (subdevice_ready) {
117 yCWarning(
CONTROLBOARD,
"Part <%s>: iJointFault interface was not found in subdevice.", partName.c_str());
122 yCWarning(
CONTROLBOARD,
"Part <%s>: IPidControl interface was not found in subdevice.", partName.c_str());
127 yCWarning(
CONTROLBOARD,
"Part <%s>: IPositionControl interface was not found in subdevice.", partName.c_str());
132 yCWarning(
CONTROLBOARD,
"Part <%s>: IPositionDirect interface was not found in subdevice.", partName.c_str());
137 yCWarning(
CONTROLBOARD,
"Part <%s>: IVelocityControl interface was not found in subdevice.", partName.c_str());
142 yCWarning(
CONTROLBOARD,
"Part <%s>: IEncodersTimed interface was not found in subdevice.", partName.c_str());
146 if (!m_allInterfaces.
iMotor) {
147 yCWarning(
CONTROLBOARD,
"Part <%s>: IMotor interface was not found in subdevice.", partName.c_str());
152 yCWarning(
CONTROLBOARD,
"Part <%s>: IMotorEncoders interface was not found in subdevice.", partName.c_str());
157 yCWarning(
CONTROLBOARD,
"Part <%s>: IAmplifierControl interface was not found in subdevice.", partName.c_str());
162 yCWarning(
CONTROLBOARD,
"Part <%s>: IControlLimits interface was not found in subdevice.", partName.c_str());
167 yCWarning(
CONTROLBOARD,
"Part <%s>: IControlCalibration interface was not found in subdevice.", partName.c_str());
172 yCWarning(
CONTROLBOARD,
"Part <%s>: ITorqueControl interface was not found in subdevice.", partName.c_str());
177 yCWarning(
CONTROLBOARD,
"Part <%s>: IImpedanceControl interface was not found in subdevice.", partName.c_str());
182 yCWarning(
CONTROLBOARD,
"Part <%s>: IControlMode interface was not found in subdevice.", partName.c_str());
187 yCWarning(
CONTROLBOARD,
"Part <%s>: IAxisInfo interface was not found in subdevice.", partName.c_str());
192 yCWarning(
CONTROLBOARD,
"Part <%s>: IPreciselyTimed interface was not found in subdevice.", partName.c_str());
197 yCWarning(
CONTROLBOARD,
"Part <%s>: IInteractionMode interface was not found in subdevice.", partName.c_str());
202 yCWarning(
CONTROLBOARD,
"Part <%s>: iRemoteCalibrator interface was not found in subdevice.", partName.c_str());
207 yCWarning(
CONTROLBOARD,
"Part <%s>: IRemoteVariables interface was not found in subdevice.", partName.c_str());
212 yCWarning(
CONTROLBOARD,
"Part <%s>: IPWMControl interface was not found in subdevice.", partName.c_str());
217 yCWarning(
CONTROLBOARD,
"Part <%s>: ICurrentControl interface was not found in subdevice.", partName.c_str());
222 yCWarning(
CONTROLBOARD,
"Part <%s>: iJointBrake interface was not found in subdevice.", partName.c_str());
227 yCWarning(
CONTROLBOARD,
"Part <%s>: iVelocityDirect interface was not found in subdevice.", partName.c_str());
235 yCError(
CONTROLBOARD) <<
"Part <%s>: iAxisInfo->getAxes() failed for subdevice " << partName.c_str();
242 yCError(
CONTROLBOARD) <<
"Part <%s>: iEncodersTimed->getAxes() failed for subdevice " << partName.c_str();
249 yCError(
CONTROLBOARD) <<
"Part <%s>: iPositionControl->getAxes() failed for subdevice " << partName.c_str();
256 yCError(
CONTROLBOARD) <<
"Part <%s>: iVelocityControl->getAxes() failed for subdevice " << partName.c_str();
265 subdevice_joints =
static_cast<size_t>(
tmp_axes);
266 times.
resize(subdevice_joints);
275void ControlBoard_nws_yarp::closeDevice()
278 streaming_parser.
reset();
280 subdevice_joints = 0;
281 subdevice_ready =
false;
291 m_allInterfaces.
iMotor =
nullptr;
312 if (subdevice_ready) {
316 if (!setDevice(poly,
false)) {
326 m_RPC = std::make_unique<ControlBoardRPCd>(this->subdevice_joints, m_allInterfaces);
367 tmpVariableForFloatSignals.
resize(subdevice_joints);
368 tmpVariableForControlModes.resize(subdevice_joints);
369 tmpVariableForInteractionModes.resize(subdevice_joints);
392 if (m_allInterfaces.
iMotor) {
432 for (
size_t i =0; i< tmpVariableForControlModes.size(); i++)
434 data.
controlMode[i] =
static_cast<int>(tmpVariableForControlModes[i]);
442 for (
size_t i =0; i< tmpVariableForInteractionModes.size(); i++)
444 data.
interactionMode[i] =
static_cast<int>(tmpVariableForInteractionModes[i]);
451 for (
double tt : times)
453 if (std::abs(times[0] -
tt) > 1.0)
461 time.
update(std::accumulate(times.
begin(), times.
end(), 0.0) / subdevice_joints);
466 extendedOutputState_buffer.
write();
470 v.
resize(subdevice_joints);
474 outputPositionStatePort.
write();
486 std::lock_guard<std::mutex> lock(m_mutex);
487 bool b = m_RPC->read(connection);
const yarp::os::LogComponent & CONTROLBOARD()
const double DEFAULT_PERIOD
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
void run() override
Loop function.
bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
bool open(yarp::os::Searchable &prop) override
Open the DeviceDriver.
bool close() override
Close the DeviceDriver.
bool attach(yarp::dev::PolyDriver *poly) override
Attach to another object.
bool detach() override
Detach the object (you must have first called attach).
void init(yarp::dev::DeviceDriver *x)
Initialization.
Interface implemented by all device drivers.
virtual yarp::dev::ReturnValue getCurrents(double *vals)=0
virtual yarp::dev::ReturnValue getAxes(size_t &ax)=0
Get the number of controlled axes.
virtual yarp::dev::ReturnValue getControlModes(std::vector< yarp::dev::ControlModeEnum > &mode)=0
Get the current control mode (multiple joints).
virtual yarp::dev::ReturnValue getCurrents(double *currs)=0
Get the instantaneous current measurement for all motors.
virtual yarp::dev::ReturnValue getEncodersTimed(double *encs, double *time)=0
Read the instantaneous acceleration of all axes.
virtual yarp::dev::ReturnValue getEncoderAccelerations(double *accs)=0
Read the instantaneous acceleration of all axes.
virtual yarp::dev::ReturnValue getEncoderSpeeds(double *spds)=0
Read the instantaneous speed of all axes.
virtual yarp::dev::ReturnValue getAxes(size_t &ax)=0
Get the number of controlled axes.
virtual yarp::dev::ReturnValue getInteractionModes(const std::vector< int > &joints, std::vector< yarp::dev::InteractionModeEnum > &modes)=0
Get the current interaction mode of the robot for a set of joints, values can be stiff or compliant.
virtual yarp::dev::ReturnValue getMotorEncoders(double *encs)=0
Read the position of all motor encoders.
virtual yarp::dev::ReturnValue getMotorEncoderSpeeds(double *spds)=0
Read the instantaneous speed of all motor encoders.
virtual yarp::dev::ReturnValue getMotorEncoderAccelerations(double *accs)=0
Read the instantaneous acceleration of all motor encoders.
virtual yarp::dev::ReturnValue getTemperatures(double *vals)=0
Get temperature of all the motors.
virtual yarp::dev::ReturnValue getDutyCycles(double *vals)=0
Gets the current dutycycle of the output of the amplifier (i.e.
virtual yarp::dev::ReturnValue getAxes(size_t &ax)=0
Get the number of controlled axes.
virtual yarp::dev::ReturnValue getTorques(double *t)=0
Get the value of the torque for all joints (this is the feedback if you have torque sensors).
virtual yarp::dev::ReturnValue getAxes(size_t &axes)=0
Get the number of controlled axes.
yarp::sig::VectorOf< float > pwmDutycycle
bool motorVelocity_isValid
yarp::sig::VectorOf< int > controlMode
bool pwmDutycycle_isValid
bool jointVelocity_isValid
yarp::sig::VectorOf< int > interactionMode
yarp::sig::VectorOf< double > motorAcceleration
yarp::sig::VectorOf< double > jointPosition
bool jointAcceleration_isValid
yarp::sig::VectorOf< double > torque
yarp::sig::VectorOf< double > motorVelocity
bool jointPosition_isValid
yarp::sig::VectorOf< double > jointVelocity
yarp::sig::VectorOf< double > current
bool interactionMode_isValid
bool motorPosition_isValid
yarp::sig::VectorOf< float > temperature
yarp::sig::VectorOf< double > jointAcceleration
bool motorAcceleration_isValid
yarp::sig::VectorOf< double > motorPosition
A container for a device driver.
A mini-server for performing network communication in the background.
void close() override
Stop port activity.
bool setEnvelope(PortWriter &envelope) override
Set an envelope (e.g., a timestamp) to the next message which will be sent.
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
int getPendingReads() override
Get the number of objects ready to be read.
void interrupt() override
Interrupt any current reads or writes attached to the port.
void useCallback(TypedReaderCallback< T > &callback) override
Set an object whose onRead method will be called when data is available.
void setStrict(bool strict=true) override
Call this to strictly keep all messages, or allow old ones to be quietly dropped.
void write(bool forceStrict=false)
Write the current object being returned by BufferedPort::prepare.
T & prepare()
Access the object which will be transmitted by the next call to yarp::os::BufferedPort::write.
An interface for reading from a network connection.
virtual bool isValid() const =0
An abstraction for a periodic thread.
bool setPeriod(double period)
Set the (new) period of the thread.
bool isRunning() const
Returns true when the thread is started, false otherwise.
bool start()
Call this to start the thread.
void stop()
Call this to stop the thread, this call blocks until the thread is terminated (and releaseThread() ca...
T & get()
A synonym of PortWriterBuffer::prepare.
void write(bool forceStrict=false)
Try to write the last buffer returned by PortWriterBuffer::get.
void attach(Port &port)
Set the Port to which objects will be written.
void enableBackgroundWrite(bool backgroundFlag)
control whether writing from this port is done in the background.
void interrupt() override
Interrupt any current reads or writes attached to the port.
bool setEnvelope(PortWriter &envelope) override
Set an envelope (e.g., a timestamp) to the next message which will be sent.
void close() override
Stop port activity.
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
A base class for nested structures that can be searched.
An abstraction for a time stamp and/or sequence number.
void update()
Set the timestamp to the current time, and increment the sequence number (wrapping to 0 if the sequen...
void resize(size_t size) override
Resize the vector.
iterator begin() noexcept
Returns an iterator to the beginning of the VectorOf.
T * data()
Return a pointer to the first element of the vector.
iterator end() noexcept
Returns an iterator to the end of the VectorOf.
#define yCError(component,...)
#define yCWarning(component,...)
#define yCIErrorThrottle(component, id, period,...)
#define yCDebug(component,...)
#define yCIWarning(component, id,...)
For streams capable of holding different kinds of content, check what they actually have.
An interface to the operating system, including Port based communication.
The main, catch-all namespace for YARP.
yarp::dev::IJointBrake * iJointBrake
yarp::dev::IRemoteCalibrator * iRemoteCalibrator
yarp::dev::IControlMode * iControlMode
yarp::dev::IJointFault * iJointFault
yarp::dev::IPidControl * iPidControl
yarp::dev::IAmplifierControl * iAmplifierControl
yarp::dev::ITorqueControl * iTorqueControl
yarp::dev::IPWMControl * iPWMControl
yarp::dev::IImpedanceControl * iImpedanceControl
yarp::dev::IVelocityDirect * iVelocityDirect
yarp::dev::IVelocityControl * iVelocityControl
yarp::dev::IInteractionMode * iInteractionMode
yarp::dev::IEncodersTimed * iEncodersTimed
yarp::dev::IRemoteVariables * iRemoteVariables
yarp::dev::IControlLimits * iControlLimits
yarp::dev::IAxisInfo * iAxisInfo
yarp::dev::IPositionDirect * iPositionDirect
yarp::dev::ICurrentControl * iCurrentControl
yarp::dev::IPreciselyTimed * iPreciselyTimed
yarp::dev::IMotorEncoders * iMotorEncoders
yarp::dev::IControlCalibration * iControlCalibration
yarp::dev::IMotor * iMotor
yarp::dev::IPositionControl * iPositionControl