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")) {
98 if (subdevice_ready) {
116 yCWarning(
CONTROLBOARD,
"Part <%s>: iJointFault interface was not found in subdevice.", partName.c_str());
121 yCWarning(
CONTROLBOARD,
"Part <%s>: IPidControl interface was not found in subdevice.", partName.c_str());
126 yCWarning(
CONTROLBOARD,
"Part <%s>: IPositionControl interface was not found in subdevice.", partName.c_str());
131 yCWarning(
CONTROLBOARD,
"Part <%s>: IPositionDirect interface was not found in subdevice.", partName.c_str());
136 yCWarning(
CONTROLBOARD,
"Part <%s>: IVelocityControl interface was not found in subdevice.", partName.c_str());
141 yCWarning(
CONTROLBOARD,
"Part <%s>: IEncodersTimed interface was not found in subdevice.", partName.c_str());
145 if (!m_allInterfaces.
iMotor) {
146 yCWarning(
CONTROLBOARD,
"Part <%s>: IMotor interface was not found in subdevice.", partName.c_str());
151 yCWarning(
CONTROLBOARD,
"Part <%s>: IMotorEncoders interface was not found in subdevice.", partName.c_str());
156 yCWarning(
CONTROLBOARD,
"Part <%s>: IAmplifierControl interface was not found in subdevice.", partName.c_str());
161 yCWarning(
CONTROLBOARD,
"Part <%s>: IControlLimits interface was not found in subdevice.", partName.c_str());
166 yCWarning(
CONTROLBOARD,
"Part <%s>: IControlCalibration interface was not found in subdevice.", partName.c_str());
171 yCWarning(
CONTROLBOARD,
"Part <%s>: ITorqueControl interface was not found in subdevice.", partName.c_str());
176 yCWarning(
CONTROLBOARD,
"Part <%s>: IImpedanceControl interface was not found in subdevice.", partName.c_str());
181 yCWarning(
CONTROLBOARD,
"Part <%s>: IControlMode interface was not found in subdevice.", partName.c_str());
186 yCWarning(
CONTROLBOARD,
"Part <%s>: IAxisInfo interface was not found in subdevice.", partName.c_str());
191 yCWarning(
CONTROLBOARD,
"Part <%s>: IPreciselyTimed interface was not found in subdevice.", partName.c_str());
196 yCWarning(
CONTROLBOARD,
"Part <%s>: IInteractionMode interface was not found in subdevice.", partName.c_str());
201 yCWarning(
CONTROLBOARD,
"Part <%s>: iRemoteCalibrator interface was not found in subdevice.", partName.c_str());
206 yCWarning(
CONTROLBOARD,
"Part <%s>: IRemoteVariables interface was not found in subdevice.", partName.c_str());
211 yCWarning(
CONTROLBOARD,
"Part <%s>: IPWMControl interface was not found in subdevice.", partName.c_str());
216 yCWarning(
CONTROLBOARD,
"Part <%s>: ICurrentControl interface was not found in subdevice.", partName.c_str());
221 yCWarning(
CONTROLBOARD,
"Part <%s>: iJointBrake interface was not found in subdevice.", partName.c_str());
226 yCWarning(
CONTROLBOARD,
"Part <%s>: iVelocityDirect interface was not found in subdevice.", partName.c_str());
234 yCError(
CONTROLBOARD) <<
"Part <%s>: iAxisInfo->getAxes() failed for subdevice " << partName.c_str();
241 yCError(
CONTROLBOARD) <<
"Part <%s>: iEncodersTimed->getAxes() failed for subdevice " << partName.c_str();
248 yCError(
CONTROLBOARD) <<
"Part <%s>: iPositionControl->getAxes() failed for subdevice " << partName.c_str();
255 yCError(
CONTROLBOARD) <<
"Part <%s>: iVelocityControl->getAxes() failed for subdevice " << partName.c_str();
264 subdevice_joints =
static_cast<size_t>(
tmp_axes);
265 times.
resize(subdevice_joints);
274void ControlBoard_nws_yarp::closeDevice()
277 streaming_parser.
reset();
279 subdevice_joints = 0;
280 subdevice_ready =
false;
290 m_allInterfaces.
iMotor =
nullptr;
311 if (subdevice_ready) {
315 if (!setDevice(poly,
false)) {
325 m_RPC = std::make_unique<ControlBoardRPCd>(this->subdevice_joints, m_allInterfaces);
358 data.
torque.resize(subdevice_joints);
361 data.
current.resize(subdevice_joints);
366 tmpVariableForFloatSignals.resize(subdevice_joints);
367 tmpVariableForControlModes.resize(subdevice_joints);
368 tmpVariableForInteractionModes.resize(subdevice_joints);
391 if (m_allInterfaces.
iMotor) {
398 std::copy(tmpVariableForFloatSignals.begin(), tmpVariableForFloatSignals.end(), data.
temperature.begin());
416 std::copy(tmpVariableForFloatSignals.begin(), tmpVariableForFloatSignals.end(), data.
pwmDutycycle.begin());
431 for (
size_t i =0; i< tmpVariableForControlModes.size(); i++)
433 data.
controlMode[i] =
static_cast<int>(tmpVariableForControlModes[i]);
441 for (
size_t i =0; i< tmpVariableForInteractionModes.size(); i++)
443 data.
interactionMode[i] =
static_cast<int>(tmpVariableForInteractionModes[i]);
450 for (
double tt : times)
452 if (std::abs(times[0] -
tt) > 1.0)
460 time.
update(std::accumulate(times.
begin(), times.
end(), 0.0) / subdevice_joints);
464 extendedOutputStatePort.
write();
468 v.
resize(subdevice_joints);
472 outputPositionStatePort.
write();
484 std::lock_guard<std::mutex> lock(m_mutex);
485 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...
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...
Scalar * data()
Return a pointer to the first element of the vector.
iterator begin() noexcept
Returns an iterator to the beginning of the Vector.
void resize(size_t size)
Resize the vector.
void clear()
Clear (removes all elements) the vector.
#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