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   ForceTorqueState state_;
00239   ForceTorqueCommand command_;
00240 };
00241 
00242 
00243 class DigitalOutCommand
00244 {
00245 public:
00246   DigitalOutCommand() : data_(false) {}
00247   DigitalOutCommand(bool data) : data_(data) {}
00248   uint8_t data_;
00249 };
00250 
00251 class DigitalOutState
00252 {
00253 public:
00254   uint8_t data_;
00255 };
00256 
00267 class DigitalOut
00268 {
00269 public:
00270   std::string name_;
00271   DigitalOutState state_;
00272   DigitalOutCommand command_;
00273 };
00274 
00275 class ProjectorCommand
00276 {
00277 public:
00278   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) {}
00279   bool enable_;
00280   double current_;
00281   uint8_t &A_;
00282   uint8_t &B_;
00283   uint8_t &I_;
00284   uint8_t &M_;
00285   uint8_t &L0_;
00286   uint8_t &L1_;
00287   bool pulse_replicator_;
00288 };
00289 
00290 class ProjectorState
00291 {
00292 public:
00293   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) {}
00294   bool enable_;
00295   double last_commanded_current_;
00296   double last_executed_current_;
00297   double last_measured_current_;
00298   double max_current_;  
00299   uint8_t &A_;
00300   uint8_t &B_;
00301   uint8_t &I_;
00302   uint8_t &M_;
00303   uint8_t &L0_;
00304   uint8_t &L1_;
00305   bool pulse_replicator_;
00306 
00307   bool output_;
00308   bool rising_timestamp_valid_;
00309   bool falling_timestamp_valid_;
00310 
00311   uint32_t timestamp_us_;
00312   uint32_t rising_timestamp_us_;
00313   uint32_t falling_timestamp_us_;
00314 };
00315 
00334 class Projector
00335 {
00336 public:
00337   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_) {}
00338   std::string name_;
00339   ProjectorState state_;
00340   ProjectorCommand command_;
00341 };
00342 
00343 class AnalogInCommand
00344 {
00345 };
00346 
00347 class AnalogInState
00348 {
00349 public:
00350   std::vector<double> state_;
00351 };
00352 
00353 class AnalogIn
00354 {
00355 public:
00356   std::string name_;
00357   AnalogInState state_;
00358   AnalogInCommand command_;
00359 };
00360 
00361 typedef std::map<std::string, Actuator*> ActuatorMap;
00362 typedef std::map<std::string, PressureSensor*> PressureSensorMap;
00363 typedef std::map<std::string, Accelerometer*> AccelerometerMap;
00364 typedef std::map<std::string, DigitalOut*> DigitalOutMap;
00365 typedef std::map<std::string, Projector*> ProjectorMap;
00366 typedef std::map<std::string, AnalogIn*> AnalogInMap;
00367 
00392 class HardwareInterface
00393 {
00394 public:
00395   ActuatorMap actuators_;
00396   PressureSensorMap pressure_sensors_;
00397   AccelerometerMap accelerometers_;
00398   DigitalOutMap digital_outs_;
00399   ProjectorMap projectors_;
00400   AnalogInMap analog_ins_;
00406   Actuator* getActuator(const std::string &name) const {
00407     ActuatorMap::const_iterator it = actuators_.find(name);
00408     return it != actuators_.end() ? it->second : NULL;
00409   }
00410 
00416   PressureSensor* getPressureSensor(const std::string &name) const {
00417     PressureSensorMap::const_iterator it = pressure_sensors_.find(name);
00418     return it != pressure_sensors_.end() ? it->second : NULL;
00419   }
00420 
00426   Accelerometer* getAccelerometer(const std::string &name) const {
00427     AccelerometerMap::const_iterator it = accelerometers_.find(name);
00428     return it != accelerometers_.end() ? it->second : NULL;
00429   }
00430 
00436   DigitalOut* getDigitalOut(const std::string &name) const {
00437     DigitalOutMap::const_iterator it = digital_outs_.find(name);
00438     return it != digital_outs_.end() ? it->second : NULL;
00439   }
00440 
00446   Projector* getProjector(const std::string &name) const {
00447     ProjectorMap::const_iterator it = projectors_.find(name);
00448     return it != projectors_.end() ? it->second : NULL;
00449   }
00450 
00456   AnalogIn* getAnalogIn(const std::string &name) const {
00457     AnalogInMap::const_iterator it = analog_ins_.find(name);
00458     return it != analog_ins_.end() ? it->second : NULL;
00459   }
00460 
00466   bool addActuator(Actuator *actuator) {
00467     std::pair<ActuatorMap::iterator, bool> p;
00468     p = actuators_.insert(ActuatorMap::value_type(actuator->name_, actuator));
00469     return p.second;
00470   }
00471 
00477   bool addPressureSensor(PressureSensor *sensor) {
00478     std::pair<PressureSensorMap::iterator, bool> p;
00479     p = pressure_sensors_.insert(PressureSensorMap::value_type(sensor->name_, sensor));
00480     return p.second;
00481   }
00482 
00488   bool addAccelerometer(Accelerometer *accelerometer) {
00489     std::pair<AccelerometerMap::iterator, bool> p;
00490     p = accelerometers_.insert(AccelerometerMap::value_type(accelerometer->name_, accelerometer));
00491     return p.second;
00492   }
00493 
00499   bool addDigitalOut(DigitalOut *digital_out) {
00500     std::pair<DigitalOutMap::iterator, bool> p;
00501     p = digital_outs_.insert(DigitalOutMap::value_type(digital_out->name_, digital_out));
00502     return p.second;
00503   }
00504 
00510   bool addProjector(Projector *projector) {
00511     std::pair<ProjectorMap::iterator, bool> p;
00512     p = projectors_.insert(ProjectorMap::value_type(projector->name_, projector));
00513     return p.second;
00514   }
00515 
00521   bool addAnalogIn(AnalogIn *analog_in) {
00522     std::pair<AnalogInMap::iterator, bool> p;
00523     p = analog_ins_.insert(AnalogInMap::value_type(analog_in->name_, analog_in));
00524     return p.second;
00525   }
00526 
00527   ros::Time current_time_; 
00528 };
00529 }
00530 
00531 #endif


pr2_hardware_interface
Author(s): Eric Berger berger@willowgarage.com
autogenerated on Thu Dec 12 2013 12:03:52