sr06.h

Go to the documentation of this file.

#ifndef SR06_H
#define SR06_H

#include <ethercat_hardware/ethercat_device.h>
#include <sr_edc_ethercat_drivers/sr0x.h>
#include <realtime_tools/realtime_publisher.h>
#include <std_msgs/Int16.h>
#include <std_msgs/Float64MultiArray.h>
#include <sr_robot_msgs/SimpleMotorFlasher.h>
#include <pthread.h>
#include <bfd.h>
#include <boost/smart_ptr.hpp>
#include <map>
#include <boost/assign.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/algorithm/string/find_iterator.hpp>

#include <sr_robot_lib/sr_hand_lib.hpp>

#include <sr_robot_msgs/EthercatDebug.h>

#include <sr_external_dependencies/types_for_external.h>
extern "C"
{
  #include <sr_external_dependencies/external/0220_palm_edc/0220_palm_edc_ethercat_protocol.h>
}


class SR06 : public SR0X
{
public:
  SR06();
  ~SR06();

  void construct(EtherCAT_SlaveHandler *sh, int &start_address);
  int  initialize(pr2_hardware_interface::HardwareInterface *hw, bool allow_unprogrammed=true);
  void multiDiagnostics(vector<diagnostic_msgs::DiagnosticStatus> &vec, unsigned char *buffer);

  bool simple_motor_flasher(sr_robot_msgs::SimpleMotorFlasher::Request &req, sr_robot_msgs::SimpleMotorFlasher::Response &res);
  void packCommand(unsigned char *buffer, bool halt, bool reset);
  bool unpackState(unsigned char *this_buffer, unsigned char *prev_buffer);
  bool can_data_is_ack(ETHERCAT_CAN_BRIDGE_DATA * packet);
  void erase_flash();
  //bool read_flash(unsigned int offset, unsigned char baddrl, unsigned char baddrh, unsigned char baddru);
  bool read_flash(unsigned int offset, unsigned int baddr);

protected:
  int                                                                  counter_;
  ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_STATUS                         data_;
  ros::NodeHandle                                                      nodehandle_;

  typedef realtime_tools::RealtimePublisher<std_msgs::Int16> rt_pub_int16_t;
  std::vector< boost::shared_ptr<rt_pub_int16_t> >   realtime_pub_;

  boost::shared_ptr<realtime_tools::RealtimePublisher<std_msgs::Float64MultiArray> > extra_analog_inputs_publisher;

private:

  static const unsigned int        nb_sensors_const;
  static const unsigned int        max_retry;
  static const unsigned short int  max_iter_const;
  static const unsigned short int  ros_pub_freq_const;
  static const unsigned short int  device_pub_freq_const;
  static const unsigned char       nb_publish_by_unpack_const;
  std::string                      firmware_file_name;
  pthread_mutex_t                  producing;
  ros::ServiceServer               serviceServer;

  bool                             flashing;
  ETHERCAT_CAN_BRIDGE_DATA         can_message_;
  bool                             can_message_sent;
  bool                             can_packet_acked;
  bfd_byte                        *binary_content; // buffer containing the binary content to be flashed
  unsigned int                     pos; // position in binary_content buffer
  unsigned int                     motor_being_flashed;

  unsigned int                     zero_buffer_read;

  boost::shared_ptr<shadow_robot::SrHandLib> sr_hand_lib;

  short cycle_count;

  boost::shared_ptr<realtime_tools::RealtimePublisher<sr_robot_msgs::EthercatDebug> > debug_publisher;

  int can_bus_;

  void send_CAN_msg(int8u can_bus, int16u msg_id, int8u msg_length, int8u msg_data[], int timeout, bool *timedout);

  bool read_back_and_check_flash(unsigned int baddr, unsigned int total_size);

  void find_address_range(bfd *fd, unsigned int *smallest_start_address, unsigned int *biggest_end_address);

  bool read_content_from_object_file(bfd *fd, bfd_byte *content, unsigned int base_addr);

  bool write_flash_data(unsigned int base_addr, unsigned int total_size);

  static inline std::string get_filename( std::string full_path )
  {
    std::vector<std::string> splitted_string;
    boost::split(splitted_string, full_path, boost::is_any_of("/"));
    return splitted_string.back();
  }

};


/* For the emacs weenies in the crowd.
Local Variables:
   c-basic-offset: 2
End:
*/


#endif /* SR06_H */