hardware_interface.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #ifndef HARDWARE_INTERFACE_H
00036 #define HARDWARE_INTERFACE_H
00037 
00038 #include <string>
00039 #include <vector>
00040 
00041 #include <ros/ros.h>
00042 
00043 #include <geometry_msgs/Vector3.h>
00044 #include <geometry_msgs/Wrench.h>
00045 
00046 namespace pr2_hardware_interface{
00047 
00048 
00049 class ActuatorState{
00050 public:
00051   ActuatorState() :
00052       timestamp_(0),
00053       device_id_(0),
00054       encoder_count_(0),
00055       position_(0),
00056       encoder_velocity_(0),
00057       velocity_(0),
00058       calibration_reading_(0),
00059       calibration_rising_edge_valid_(0),
00060       calibration_falling_edge_valid_(0),
00061       last_calibration_rising_edge_(0),
00062       last_calibration_falling_edge_(0),
00063       is_enabled_(0),
00064       halted_(0),
00065       last_commanded_current_(0),
00066       last_executed_current_(0),
00067       last_measured_current_(0),
00068       last_commanded_effort_(0),
00069       last_executed_effort_(0),
00070       last_measured_effort_(0),
00071       motor_voltage_(0),
00072       num_encoder_errors_(0),
00073       zero_offset_(0)
00074   {}
00075 
00083   ros::Duration sample_timestamp_;
00084 
00090   double timestamp_;
00091 
00092   int device_id_; 
00093 
00094   int encoder_count_; 
00095   double position_; 
00096   double encoder_velocity_; 
00097   double velocity_; 
00098 
00099   bool calibration_reading_; 
00100   bool calibration_rising_edge_valid_; 
00101   bool calibration_falling_edge_valid_; 
00102   double last_calibration_rising_edge_; 
00103   double last_calibration_falling_edge_; 
00104 
00105   bool is_enabled_; 
00106   bool halted_; 
00107 
00108   double last_commanded_current_; 
00109   double last_executed_current_; 
00110   double last_measured_current_; 
00111 
00112   double last_commanded_effort_; 
00113   double last_executed_effort_; 
00114   double last_measured_effort_; 
00115 
00116   double max_effort_; 
00117 
00118   double motor_voltage_; 
00119 
00120   int num_encoder_errors_; 
00121 
00122   double zero_offset_; 
00123 };
00124 
00125 class ActuatorCommand
00126 {
00127 public:
00128   ActuatorCommand() :
00129     enable_(0), effort_(0)
00130   {}
00131   bool enable_; 
00132   double effort_; 
00133 };
00134 
00147 class Actuator
00148 {
00149 public:
00150   Actuator() {};
00151   Actuator(std::string name) : name_(name) {}
00152   std::string name_;
00153   ActuatorState state_;
00154   ActuatorCommand command_;
00155 };
00156 
00157 class PressureSensorCommand
00158 {
00159 };
00160 
00161 class PressureSensorState
00162 {
00163 public:
00164   std::vector<uint16_t> data_; 
00165 };
00166 
00177 class PressureSensor
00178 {
00179 public:
00180   std::string name_;
00181   PressureSensorState state_;
00182   PressureSensorCommand command_;
00183 };
00184 
00185 class AccelerometerCommand
00186 {
00187 public:
00188   enum {RANGE_2G=0, RANGE_4G=1, RANGE_8G=2}; 
00189   enum {BANDWIDTH_1500HZ=6, BANDWIDTH_750HZ=5, BANDWIDTH_375HZ=4, BANDWIDTH_190HZ=3, BANDWIDTH_100HZ=2, BANDWIDTH_50HZ=1, BANDWIDTH_25HZ=0}; 
00190   AccelerometerCommand() : range_(RANGE_2G), bandwidth_(BANDWIDTH_1500HZ) {}
00191   int range_; 
00192   int bandwidth_; 
00193 };
00194 
00195 class AccelerometerState
00196 {
00197 public:
00198   std::string frame_id_;  
00199   std::vector<geometry_msgs::Vector3> samples_; 
00200 };
00201 
00212 class Accelerometer
00213 {
00214 public:
00215   std::string name_;
00216   AccelerometerState state_;
00217   AccelerometerCommand command_;
00218 };
00219 
00220 class ForceTorqueState
00221 {
00222 public:
00223   bool good_; 
00224 
00225   std::vector<geometry_msgs::Wrench> samples_;
00226 };
00227 
00228 class ForceTorqueCommand
00229 {
00230 public:
00232   bool halt_on_error_;
00233 };
00234 
00235 class ForceTorque
00236 {
00237 public:
00238   std::string name_;
00239   ForceTorqueState state_;
00240   ForceTorqueCommand command_;
00241 };
00242 
00243 
00244 class DigitalOutCommand
00245 {
00246 public:
00247   DigitalOutCommand() : data_(false) {}
00248   DigitalOutCommand(bool data) : data_(data) {}
00249   uint8_t data_;
00250 };
00251 
00252 class DigitalOutState
00253 {
00254 public:
00255   uint8_t data_;
00256 };
00257 
00268 class DigitalOut
00269 {
00270 public:
00271   std::string name_;
00272   DigitalOutState state_;
00273   DigitalOutCommand command_;
00274 };
00275 
00276 class ProjectorCommand
00277 {
00278 public:
00279   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) {}
00280   bool enable_;
00281   double current_;
00282   uint8_t &A_;
00283   uint8_t &B_;
00284   uint8_t &I_;
00285   uint8_t &M_;
00286   uint8_t &L0_;
00287   uint8_t &L1_;
00288   bool pulse_replicator_;
00289 };
00290 
00291 class ProjectorState
00292 {
00293 public:
00294   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) {}
00295   bool enable_;
00296   double last_commanded_current_;
00297   double last_executed_current_;
00298   double last_measured_current_;
00299   double max_current_;  
00300   uint8_t &A_;
00301   uint8_t &B_;
00302   uint8_t &I_;
00303   uint8_t &M_;
00304   uint8_t &L0_;
00305   uint8_t &L1_;
00306   bool pulse_replicator_;
00307 
00308   bool output_;
00309   bool rising_timestamp_valid_;
00310   bool falling_timestamp_valid_;
00311 
00312   uint32_t timestamp_us_;
00313   uint32_t rising_timestamp_us_;
00314   uint32_t falling_timestamp_us_;
00315 };
00316 
00335 class Projector
00336 {
00337 public:
00338   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_) {}
00339   std::string name_;
00340   ProjectorState state_;
00341   ProjectorCommand command_;
00342 };
00343 
00344 class AnalogInCommand
00345 {
00346 };
00347 
00348 class AnalogInState
00349 {
00350 public:
00351   std::vector<double> state_;
00352 };
00353 
00354 class AnalogIn
00355 {
00356 public:
00357   std::string name_;
00358   AnalogInState state_;
00359   AnalogInCommand command_;
00360 };
00361 
00368 class CustomHW
00369 {
00370 public:
00371   std::string name_;
00372 };
00373 
00374 typedef std::map<std::string, Actuator*> ActuatorMap;
00375 typedef std::map<std::string, PressureSensor*> PressureSensorMap;
00376 typedef std::map<std::string, Accelerometer*> AccelerometerMap;
00377 typedef std::map<std::string, ForceTorque*> ForceTorqueMap;
00378 typedef std::map<std::string, DigitalOut*> DigitalOutMap;
00379 typedef std::map<std::string, Projector*> ProjectorMap;
00380 typedef std::map<std::string, AnalogIn*> AnalogInMap;
00381 typedef std::map<std::string, CustomHW*> CustomHWMap;
00382 
00408 class HardwareInterface
00409 {
00410 public:
00411   ActuatorMap actuators_;
00412   PressureSensorMap pressure_sensors_;
00413   AccelerometerMap accelerometers_;
00414   ForceTorqueMap ft_sensors_;
00415   DigitalOutMap digital_outs_;
00416   ProjectorMap projectors_;
00417   AnalogInMap analog_ins_;
00418   CustomHWMap custom_hws_;
00424   Actuator* getActuator(const std::string &name) const {
00425     ActuatorMap::const_iterator it = actuators_.find(name);
00426     return it != actuators_.end() ? it->second : NULL;
00427   }
00428 
00434   PressureSensor* getPressureSensor(const std::string &name) const {
00435     PressureSensorMap::const_iterator it = pressure_sensors_.find(name);
00436     return it != pressure_sensors_.end() ? it->second : NULL;
00437   }
00438 
00444   Accelerometer* getAccelerometer(const std::string &name) const {
00445     AccelerometerMap::const_iterator it = accelerometers_.find(name);
00446     return it != accelerometers_.end() ? it->second : NULL;
00447   }
00448 
00454   ForceTorque* getForceTorque(const std::string &name) const {
00455     ForceTorqueMap::const_iterator it = ft_sensors_.find(name);
00456     return it != ft_sensors_.end() ? it->second : NULL;
00457   }
00458 
00464   DigitalOut* getDigitalOut(const std::string &name) const {
00465     DigitalOutMap::const_iterator it = digital_outs_.find(name);
00466     return it != digital_outs_.end() ? it->second : NULL;
00467   }
00468 
00474   Projector* getProjector(const std::string &name) const {
00475     ProjectorMap::const_iterator it = projectors_.find(name);
00476     return it != projectors_.end() ? it->second : NULL;
00477   }
00478 
00484   AnalogIn* getAnalogIn(const std::string &name) const {
00485     AnalogInMap::const_iterator it = analog_ins_.find(name);
00486     return it != analog_ins_.end() ? it->second : NULL;
00487   }
00488 
00494   CustomHW* getCustomHW(const std::string &name) const {
00495     CustomHWMap::const_iterator it = custom_hws_.find(name);
00496     return it != custom_hws_.end() ? it->second : NULL;
00497   }
00498 
00504   bool addActuator(Actuator *actuator) {
00505     std::pair<ActuatorMap::iterator, bool> p;
00506     p = actuators_.insert(ActuatorMap::value_type(actuator->name_, actuator));
00507     return p.second;
00508   }
00509 
00515   bool addPressureSensor(PressureSensor *sensor) {
00516     std::pair<PressureSensorMap::iterator, bool> p;
00517     p = pressure_sensors_.insert(PressureSensorMap::value_type(sensor->name_, sensor));
00518     return p.second;
00519   }
00520 
00526   bool addAccelerometer(Accelerometer *accelerometer) {
00527     std::pair<AccelerometerMap::iterator, bool> p;
00528     p = accelerometers_.insert(AccelerometerMap::value_type(accelerometer->name_, accelerometer));
00529     return p.second;
00530   }
00531 
00537   bool addForceTorque(ForceTorque *forcetorque) {
00538     std::pair<ForceTorqueMap::iterator, bool> p;
00539     p = ft_sensors_.insert(ForceTorqueMap::value_type(forcetorque->name_, forcetorque));
00540     return p.second;
00541   }
00542 
00548   bool addDigitalOut(DigitalOut *digital_out) {
00549     std::pair<DigitalOutMap::iterator, bool> p;
00550     p = digital_outs_.insert(DigitalOutMap::value_type(digital_out->name_, digital_out));
00551     return p.second;
00552   }
00553 
00559   bool addProjector(Projector *projector) {
00560     std::pair<ProjectorMap::iterator, bool> p;
00561     p = projectors_.insert(ProjectorMap::value_type(projector->name_, projector));
00562     return p.second;
00563   }
00564 
00570   bool addAnalogIn(AnalogIn *analog_in) {
00571     std::pair<AnalogInMap::iterator, bool> p;
00572     p = analog_ins_.insert(AnalogInMap::value_type(analog_in->name_, analog_in));
00573     return p.second;
00574   }
00575 
00581   bool addCustomHW(CustomHW *custom_hw) {
00582     std::pair<CustomHWMap::iterator, bool> p;
00583     p = custom_hws_.insert(CustomHWMap::value_type(custom_hw->name_, custom_hw));
00584     return p.second;
00585   }
00586 
00587   ros::Time current_time_; 
00588 };
00589 }
00590 
00591 #endif


pr2_hardware_interface
Author(s): Eric Berger berger@willowgarage.com
autogenerated on Thu Jun 6 2019 21:11:28