cod_decod_std_io.hpp

Go to the documentation of this file.

/*
 * Copyright (c) 2013, Shadow Robot Company, All rights reserved.
 * 
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 3.0 of the License, or (at your option) any later version.
 * 
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
 * Lesser General Public License for more details.
 * 
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library.
 */

#ifndef _COD_DECOD_STD_IO_HPP_
#define _COD_DECOD_STD_IO_HPP_

#include "sr_ronex_drivers/cod_decod/cod_decod.hpp"
#include <sr_ronex_msgs/BoolArray.h>
#include <std_msgs/UInt16MultiArray.h>
#include "realtime_tools/realtime_box.h"
#include "realtime_tools/realtime_publisher.h"
#include <vector>

namespace sr_cod_decod_std_io
{
  enum DigitalIo
  {
    DigitalIo0,
    DigitalIo1,
    DigitalIo2,
    DigitalIo3
  };

  enum AnalogOutput
  {
    AnalogOutput0,
    AnalogOutput1
  };

  enum AnalogInput
  {
    AnlogInput0,
    AnlogInput1,
    AnlogInput2,
    AnlogInput3
  };
}  // namespace sr_cod_decod_std_io

namespace sr_cod_decod
{
class CodDecodStdIo: public CodDecod
{
public:
  CodDecodStdIo();

  virtual void construct(hardware_interface::HardwareInterface *hw, EtherCAT_SlaveHandler *sh, int n_digital_outputs,
                         int n_analog_outputs, int n_digital_inputs, int n_analog_inputs, int n_PWM_outputs);
  virtual void update(unsigned char *status_buffer);
  virtual void build_command(unsigned char *command_buffer);
  virtual void add_diagnostics(std::vector<diagnostic_msgs::DiagnosticStatus> &vec,
                                   diagnostic_updater::DiagnosticStatusWrapper &d);

  virtual void setPinAsDigitalInput(sr_cod_decod_std_io::DigitalIo pin);

  virtual bool digitalInputToBool(sr_cod_decod_std_io::DigitalIo input_pin);

  virtual void digitalInputToBool(sr_cod_decod_std_io::DigitalIo input_pin, bool *where_to_store_it);

  virtual double analogInputToDouble(sr_cod_decod_std_io::AnalogInput input_pin);

  virtual void analogInputToDouble(sr_cod_decod_std_io::AnalogInput input_pin, double *where_to_store_it);

  uint16_t analogInputToUint16(sr_cod_decod_std_io::AnalogInput input_pin);

  virtual void analogInputToUint16(sr_cod_decod_std_io::AnalogInput input_pin, uint16_t *where_to_store_it);

  int analogInputToInt(sr_cod_decod_std_io::AnalogInput input_pin);

  virtual void analogInputToInt(sr_cod_decod_std_io::AnalogInput input_pin, int *where_to_store_it);

  virtual void signToDigitalOutput(sr_cod_decod_std_io::DigitalIo output_pin, double value);
  virtual void boolToDigitalOutput(sr_cod_decod_std_io::DigitalIo output_pin, bool value);

  virtual void doubleToAnalogOutput(sr_cod_decod_std_io::AnalogOutput output_pin, double value);

  virtual void uint16ToAnalogOutput(sr_cod_decod_std_io::AnalogOutput output_pin, uint16_t value);

  virtual void doubleToPWMOutput(sr_cod_decod_std_io::DigitalIo output_pin, uint16_t PWM_period, double PWM_ON_time);

  virtual void uint16ToPWMOutput(sr_cod_decod_std_io::DigitalIo output_pin, uint16_t PWM_period, uint16_t PWM_ON_time);

protected:
  // This will be used for the moment. If we have inputs and outputs that don't fit this digital and analog i/o array
  // model then we'll have to change it and probably move this extract and publish stage to the different CodDecod
  // children classes.
  unsigned n_digital_outputs_;
  unsigned n_analog_outputs_;
  unsigned n_digital_inputs_;
  unsigned n_analog_inputs_;
  unsigned n_PWM_outputs_;

  unsigned int command_size_;
  unsigned int status_size_;

  void digitalOutputCommandCB(const sr_ronex_msgs::BoolArrayConstPtr& msg);
  void analogOutputCommandCB(const std_msgs::UInt16MultiArrayConstPtr& msg);
  void PWMOutputCommandCB(const std_msgs::UInt16MultiArrayConstPtr& msg);


  boost::scoped_ptr<realtime_tools::RealtimePublisher<sr_ronex_msgs::BoolArray> > digital_input_state_publisher_;
  boost::scoped_ptr<realtime_tools::RealtimePublisher<std_msgs::UInt16MultiArray> >analog_input_state_publisher_;

  realtime_tools::RealtimeBox<boost::shared_ptr<sr_ronex_msgs::BoolArray> > digital_output_;
  realtime_tools::RealtimeBox<boost::shared_ptr<std_msgs::UInt16MultiArray> > analog_output_;
  realtime_tools::RealtimeBox<boost::shared_ptr<std_msgs::UInt16MultiArray> > PWM_output_;

  sr_ronex_msgs::BoolArray d_in_;
  std_msgs::UInt16MultiArray a_in_;

  ros::Subscriber sub_digital_output_command_;
  ros::Subscriber sub_analog_output_command_;
  ros::Subscriber sub_PWM_output_command_;

  ros::NodeHandle node_;
};
}  // namespace sr_cod_decod


#endif