hardware_interface.h

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

#ifndef HARDWARE_INTERFACE_H
#define HARDWARE_INTERFACE_H

#include <string>
#include <vector>

#include <ros/ros.h>

#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Wrench.h>

namespace pr2_hardware_interface{


class ActuatorState{
public:
  ActuatorState() :
      timestamp_(0),
      device_id_(0),
      encoder_count_(0),
      position_(0),
      encoder_velocity_(0),
      velocity_(0),
      calibration_reading_(0),
      calibration_rising_edge_valid_(0),
      calibration_falling_edge_valid_(0),
      last_calibration_rising_edge_(0),
      last_calibration_falling_edge_(0),
      is_enabled_(0),
      halted_(0),
      last_commanded_current_(0),
      last_executed_current_(0),
      last_measured_current_(0),
      last_commanded_effort_(0),
      last_executed_effort_(0),
      last_measured_effort_(0),
      motor_voltage_(0),
      num_encoder_errors_(0),
      zero_offset_(0)
  {}

  ros::Duration sample_timestamp_;

  double timestamp_;

  int device_id_; 

  int encoder_count_; 
  double position_; 
  double encoder_velocity_; 
  double velocity_; 

  bool calibration_reading_; 
  bool calibration_rising_edge_valid_; 
  bool calibration_falling_edge_valid_; 
  double last_calibration_rising_edge_; 
  double last_calibration_falling_edge_; 

  bool is_enabled_; 
  bool halted_; 

  double last_commanded_current_; 
  double last_executed_current_; 
  double last_measured_current_; 

  double last_commanded_effort_; 
  double last_executed_effort_; 
  double last_measured_effort_; 

  double max_effort_; 

  double motor_voltage_; 

  int num_encoder_errors_; 

  double zero_offset_; 
};

class ActuatorCommand
{
public:
  ActuatorCommand() :
    enable_(0), effort_(0)
  {}
  bool enable_; 
  double effort_; 
};

class Actuator
{
public:
  Actuator() {};
  Actuator(std::string name) : name_(name) {}
  std::string name_;
  ActuatorState state_;
  ActuatorCommand command_;
};

class PressureSensorCommand
{
};

class PressureSensorState
{
public:
  std::vector<uint16_t> data_; 
};

class PressureSensor
{
public:
  std::string name_;
  PressureSensorState state_;
  PressureSensorCommand command_;
};

class AccelerometerCommand
{
public:
  enum {RANGE_2G=0, RANGE_4G=1, RANGE_8G=2}; 
  enum {BANDWIDTH_1500HZ=6, BANDWIDTH_750HZ=5, BANDWIDTH_375HZ=4, BANDWIDTH_190HZ=3, BANDWIDTH_100HZ=2, BANDWIDTH_50HZ=1, BANDWIDTH_25HZ=0}; 
  AccelerometerCommand() : range_(RANGE_2G), bandwidth_(BANDWIDTH_1500HZ) {}
  int range_; 
  int bandwidth_; 
};

class AccelerometerState
{
public:
  std::string frame_id_;  
  std::vector<geometry_msgs::Vector3> samples_; 
};

class Accelerometer
{
public:
  std::string name_;
  AccelerometerState state_;
  AccelerometerCommand command_;
};

class ForceTorqueState
{
public:
  bool good_; 

  std::vector<geometry_msgs::Wrench> samples_;
};

class ForceTorqueCommand
{
public:
  bool halt_on_error_;
};

class ForceTorque
{
public:
  std::string name_;
  ForceTorqueState state_;
  ForceTorqueCommand command_;
};


class DigitalOutCommand
{
public:
  DigitalOutCommand() : data_(false) {}
  DigitalOutCommand(bool data) : data_(data) {}
  uint8_t data_;
};

class DigitalOutState
{
public:
  uint8_t data_;
};

class DigitalOut
{
public:
  std::string name_;
  DigitalOutState state_;
  DigitalOutCommand command_;
};

class ProjectorCommand
{
public:
  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) {}
  bool enable_;
  double current_;
  uint8_t &A_;
  uint8_t &B_;
  uint8_t &I_;
  uint8_t &M_;
  uint8_t &L0_;
  uint8_t &L1_;
  bool pulse_replicator_;
};

class ProjectorState
{
public:
  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) {}
  bool enable_;
  double last_commanded_current_;
  double last_executed_current_;
  double last_measured_current_;
  double max_current_;  
  uint8_t &A_;
  uint8_t &B_;
  uint8_t &I_;
  uint8_t &M_;
  uint8_t &L0_;
  uint8_t &L1_;
  bool pulse_replicator_;

  bool output_;
  bool rising_timestamp_valid_;
  bool falling_timestamp_valid_;

  uint32_t timestamp_us_;
  uint32_t rising_timestamp_us_;
  uint32_t falling_timestamp_us_;
};

class Projector
{
public:
  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_) {}
  std::string name_;
  ProjectorState state_;
  ProjectorCommand command_;
};

class AnalogInCommand
{
};

class AnalogInState
{
public:
  std::vector<double> state_;
};

class AnalogIn
{
public:
  std::string name_;
  AnalogInState state_;
  AnalogInCommand command_;
};

class CustomHW
{
public:
  std::string name_;
};

typedef std::map<std::string, Actuator*> ActuatorMap;
typedef std::map<std::string, PressureSensor*> PressureSensorMap;
typedef std::map<std::string, Accelerometer*> AccelerometerMap;
typedef std::map<std::string, ForceTorque*> ForceTorqueMap;
typedef std::map<std::string, DigitalOut*> DigitalOutMap;
typedef std::map<std::string, Projector*> ProjectorMap;
typedef std::map<std::string, AnalogIn*> AnalogInMap;
typedef std::map<std::string, CustomHW*> CustomHWMap;

class HardwareInterface
{
public:
  ActuatorMap actuators_;
  PressureSensorMap pressure_sensors_;
  AccelerometerMap accelerometers_;
  ForceTorqueMap ft_sensors_;
  DigitalOutMap digital_outs_;
  ProjectorMap projectors_;
  AnalogInMap analog_ins_;
  CustomHWMap custom_hws_;
  Actuator* getActuator(const std::string &name) const {
    ActuatorMap::const_iterator it = actuators_.find(name);
    return it != actuators_.end() ? it->second : NULL;
  }

  PressureSensor* getPressureSensor(const std::string &name) const {
    PressureSensorMap::const_iterator it = pressure_sensors_.find(name);
    return it != pressure_sensors_.end() ? it->second : NULL;
  }

  Accelerometer* getAccelerometer(const std::string &name) const {
    AccelerometerMap::const_iterator it = accelerometers_.find(name);
    return it != accelerometers_.end() ? it->second : NULL;
  }

  ForceTorque* getForceTorque(const std::string &name) const {
    ForceTorqueMap::const_iterator it = ft_sensors_.find(name);
    return it != ft_sensors_.end() ? it->second : NULL;
  }

  DigitalOut* getDigitalOut(const std::string &name) const {
    DigitalOutMap::const_iterator it = digital_outs_.find(name);
    return it != digital_outs_.end() ? it->second : NULL;
  }

  Projector* getProjector(const std::string &name) const {
    ProjectorMap::const_iterator it = projectors_.find(name);
    return it != projectors_.end() ? it->second : NULL;
  }

  AnalogIn* getAnalogIn(const std::string &name) const {
    AnalogInMap::const_iterator it = analog_ins_.find(name);
    return it != analog_ins_.end() ? it->second : NULL;
  }

  CustomHW* getCustomHW(const std::string &name) const {
    CustomHWMap::const_iterator it = custom_hws_.find(name);
    return it != custom_hws_.end() ? it->second : NULL;
  }

  bool addActuator(Actuator *actuator) {
    std::pair<ActuatorMap::iterator, bool> p;
    p = actuators_.insert(ActuatorMap::value_type(actuator->name_, actuator));
    return p.second;
  }

  bool addPressureSensor(PressureSensor *sensor) {
    std::pair<PressureSensorMap::iterator, bool> p;
    p = pressure_sensors_.insert(PressureSensorMap::value_type(sensor->name_, sensor));
    return p.second;
  }

  bool addAccelerometer(Accelerometer *accelerometer) {
    std::pair<AccelerometerMap::iterator, bool> p;
    p = accelerometers_.insert(AccelerometerMap::value_type(accelerometer->name_, accelerometer));
    return p.second;
  }

  bool addForceTorque(ForceTorque *forcetorque) {
    std::pair<ForceTorqueMap::iterator, bool> p;
    p = ft_sensors_.insert(ForceTorqueMap::value_type(forcetorque->name_, forcetorque));
    return p.second;
  }

  bool addDigitalOut(DigitalOut *digital_out) {
    std::pair<DigitalOutMap::iterator, bool> p;
    p = digital_outs_.insert(DigitalOutMap::value_type(digital_out->name_, digital_out));
    return p.second;
  }

  bool addProjector(Projector *projector) {
    std::pair<ProjectorMap::iterator, bool> p;
    p = projectors_.insert(ProjectorMap::value_type(projector->name_, projector));
    return p.second;
  }

  bool addAnalogIn(AnalogIn *analog_in) {
    std::pair<AnalogInMap::iterator, bool> p;
    p = analog_ins_.insert(AnalogInMap::value_type(analog_in->name_, analog_in));
    return p.second;
  }

  bool addCustomHW(CustomHW *custom_hw) {
    std::pair<CustomHWMap::iterator, bool> p;
    p = custom_hws_.insert(CustomHWMap::value_type(custom_hw->name_, custom_hw));
    return p.second;
  }

  ros::Time current_time_; 
};
}

#endif