35 #ifndef HARDWARE_INTERFACE_H 36 #define HARDWARE_INTERFACE_H 43 #include <geometry_msgs/Vector3.h> 44 #include <geometry_msgs/Wrench.h> 129 enable_(0), effort_(0)
188 enum {RANGE_2G=0, RANGE_4G=1, RANGE_8G=2};
189 enum {BANDWIDTH_1500HZ=6, BANDWIDTH_750HZ=5, BANDWIDTH_375HZ=4, BANDWIDTH_190HZ=3, BANDWIDTH_100HZ=2, BANDWIDTH_50HZ=1, BANDWIDTH_25HZ=0};
224 std::vector<geometry_msgs::Wrench> samples_;
279 ProjectorCommand(uint8_t &A, uint8_t &B, uint8_t &I, uint8_t &M, uint8_t &L0, uint8_t &L1) : enable_(0), current_(0), A_(A), B_(B), I_(I), M_(M), L0_(L0), L1_(L1) {}
294 ProjectorState(uint8_t &A, uint8_t &B, uint8_t &I, uint8_t &M, uint8_t &L0, uint8_t &L1) : A_(A), B_(B), I_(I), M_(M), L0_(L0), L1_(L1) {}
338 Projector(
DigitalOut &A,
DigitalOut &B,
DigitalOut &I,
DigitalOut &M,
DigitalOut &L0,
DigitalOut &L1) : state_(A.state_.data_, B.state_.data_, I.state_.data_, M.state_.data_, L0.state_.data_, L1.state_.data_), command_(A.command_.data_, B.command_.data_, I.command_.data_, M.command_.data_, L0.command_.data_, L1.command_.data_) {}
425 ActuatorMap::const_iterator it = actuators_.find(name);
426 return it != actuators_.end() ? it->second : NULL;
435 PressureSensorMap::const_iterator it = pressure_sensors_.find(name);
436 return it != pressure_sensors_.end() ? it->second : NULL;
445 AccelerometerMap::const_iterator it = accelerometers_.find(name);
446 return it != accelerometers_.end() ? it->second : NULL;
455 ForceTorqueMap::const_iterator it = ft_sensors_.find(name);
456 return it != ft_sensors_.end() ? it->second : NULL;
465 DigitalOutMap::const_iterator it = digital_outs_.find(name);
466 return it != digital_outs_.end() ? it->second : NULL;
475 ProjectorMap::const_iterator it = projectors_.find(name);
476 return it != projectors_.end() ? it->second : NULL;
485 AnalogInMap::const_iterator it = analog_ins_.find(name);
486 return it != analog_ins_.end() ? it->second : NULL;
495 CustomHWMap::const_iterator it = custom_hws_.find(name);
496 return it != custom_hws_.end() ? it->second : NULL;
505 std::pair<ActuatorMap::iterator, bool> p;
506 p = actuators_.insert(ActuatorMap::value_type(actuator->
name_, actuator));
516 std::pair<PressureSensorMap::iterator, bool> p;
517 p = pressure_sensors_.insert(PressureSensorMap::value_type(sensor->
name_, sensor));
527 std::pair<AccelerometerMap::iterator, bool> p;
528 p = accelerometers_.insert(AccelerometerMap::value_type(accelerometer->
name_, accelerometer));
538 std::pair<ForceTorqueMap::iterator, bool> p;
539 p = ft_sensors_.insert(ForceTorqueMap::value_type(forcetorque->
name_, forcetorque));
549 std::pair<DigitalOutMap::iterator, bool> p;
550 p = digital_outs_.insert(DigitalOutMap::value_type(digital_out->
name_, digital_out));
560 std::pair<ProjectorMap::iterator, bool> p;
561 p = projectors_.insert(ProjectorMap::value_type(projector->
name_, projector));
571 std::pair<AnalogInMap::iterator, bool> p;
572 p = analog_ins_.insert(AnalogInMap::value_type(analog_in->
name_, analog_in));
582 std::pair<CustomHWMap::iterator, bool> p;
583 p = custom_hws_.insert(CustomHWMap::value_type(custom_hw->
name_, custom_hw));
bool addActuator(Actuator *actuator)
Add an actuator to the hardware interface.
PressureSensorCommand command_
ForceTorque * getForceTorque(const std::string &name) const
Get a pointer to the FT sensor by name.
CustomHW * getCustomHW(const std::string &name) const
Get a pointer to the Custom Hardware device by name.
double last_measured_effort_
The measured torque (in Nm)
AccelerometerMap accelerometers_
ProjectorCommand command_
bool addCustomHW(CustomHW *custom_hw)
Add a Custom Hardware device to the hardware interface.
Accelerometer * getAccelerometer(const std::string &name) const
Get a pointer to the accelerometer by name.
double last_calibration_rising_edge_
The position of the motor the last time the calibration switch went from low to high.
double last_measured_current_
bool is_enabled_
Enable status.
double effort_
Force to apply (in Nm)
int encoder_count_
The number of ticks as reported by the encoder.
std::map< std::string, Actuator * > ActuatorMap
int num_encoder_errors_
The number of invalid encoder signal transitions.
double max_effort_
Absolute torque limit for actuator (derived from motor current limit). (in Nm)
PressureSensorMap pressure_sensors_
std::map< std::string, PressureSensor * > PressureSensorMap
double encoder_velocity_
The velocity measured in encoder ticks per second.
uint32_t rising_timestamp_us_
bool falling_timestamp_valid_
ProjectorState(uint8_t &A, uint8_t &B, uint8_t &I, uint8_t &M, uint8_t &L0, uint8_t &L1)
DigitalOutCommand(bool data)
double last_executed_effort_
The torque applied after safety limits were enforced (in Nm)
bool addPressureSensor(PressureSensor *sensor)
Add an pressure sensor to the hardware interface.
int bandwidth_
Accelerometer bandwidth setting. Value is passed directly to Bosch accelerometer (BMA 150)...
bool addAccelerometer(Accelerometer *accelerometer)
Add an accelerometer to the hardware interface.
AnalogIn * getAnalogIn(const std::string &name) const
Get a pointer to the analog-in device by name.
double velocity_
The velocity in radians per second.
double last_commanded_effort_
The torque requested in the previous ActuatorCommand (in Nm)
uint32_t falling_timestamp_us_
std::map< std::string, Accelerometer * > AccelerometerMap
std::map< std::string, ForceTorque * > ForceTorqueMap
Actuator * getActuator(const std::string &name) const
Get a pointer to the actuator by name.
std::map< std::string, DigitalOut * > DigitalOutMap
DigitalOut * getDigitalOut(const std::string &name) const
Get a pointer to the digital I/O by name.
double last_executed_current_
The actual current requested after safety limits were enforced (in amps)
std::map< std::string, Projector * > ProjectorMap
double max_current_
Current limit (Amps). Minimum of board and LED limits.
std::vector< double > state_
ros::Duration sample_timestamp_
bool halt_on_error_
If halt_on_error_ is true, the driver with halt motors when an there is an error is detected...
Actuator(std::string name)
double last_executed_current_
bool good_
A vector of samples taken from force/torque sensor since the last iteration of the control loop (olde...
int device_id_
Position in EtherCAT chain.
Projector(DigitalOut &A, DigitalOut &B, DigitalOut &I, DigitalOut &M, DigitalOut &L0, DigitalOut &L1)
double last_commanded_current_
AccelerometerCommand command_
bool calibration_rising_edge_valid_
Is the last_callibration_rising_edge_ field valid?
PressureSensor * getPressureSensor(const std::string &name) const
Get a pointer to the pressure sensor by name.
std::string frame_id_
Frame id of accelerometer.
AccelerometerState state_
AccelerometerCommand()
Enums for possible accelerometer bandwidth settings.
double last_commanded_current_
The current computed based on the effort specified in the ActuatorCommand (in amps) ...
bool calibration_falling_edge_valid_
Is the last_callibration_falling_edge_ field valid?
DigitalOutMap digital_outs_
bool addProjector(Projector *projector)
Add an projector to the hardware interface.
bool addAnalogIn(AnalogIn *analog_in)
Add an analog-in device to the hardware interface.
ProjectorCommand(uint8_t &A, uint8_t &B, uint8_t &I, uint8_t &M, uint8_t &L0, uint8_t &L1)
double last_measured_current_
The measured current (in amps)
bool enable_
Enable this actuator.
PressureSensorState state_
ForceTorqueMap ft_sensors_
std::vector< uint16_t > data_
A vector of 22 pressure readings from each sensor.
int range_
The range of the values to be returned (range of 0 means within +/- 2g, 1 means within +/-4g...
double last_calibration_falling_edge_
The position of the motor the last time the calibration switch went from high to low.
std::map< std::string, CustomHW * > CustomHWMap
double position_
The position of the motor (in radians)
double zero_offset_
A bias applied to the position value when reported. This value is written once after calibration...
std::map< std::string, AnalogIn * > AnalogInMap
bool rising_timestamp_valid_
DigitalOutCommand command_
double motor_voltage_
Motor voltage (in volts)
ros::Time current_time_
The time at which the commands were sent to the hardware.
bool calibration_reading_
the value of the last calibration reading: low (false) or high (true)
bool halted_
indicates if the motor is halted. A motor can be halted because of voltage or communication problems ...
bool addForceTorque(ForceTorque *forcetorque)
Add a FT sensor to the hardware interface.
ForceTorqueCommand command_
bool addDigitalOut(DigitalOut *digital_out)
Add an digital I/O to the hardware interface.
std::vector< geometry_msgs::Vector3 > samples_
A vector of samples taken from the accelerometer (in m/s/s) since the last iteration of the control l...
Projector * getProjector(const std::string &name) const
Get a pointer to the projector by name.