sr09.h
Go to the documentation of this file.
1 
26 #ifndef SR_EDC_ETHERCAT_DRIVERS_SR09_H
27 #define SR_EDC_ETHERCAT_DRIVERS_SR09_H
28 
29 #include <ros_ethercat_hardware/ethercat_hardware.h>
30 #include <ros_ethercat_model/robot_state.hpp>
33 #include <std_msgs/Int16.h>
34 #include <std_msgs/Float64MultiArray.h>
35 #include <sr_robot_msgs/SetImuScale.h>
36 #include <sr_robot_msgs/SimpleMotorFlasher.h>
37 #include <pthread.h>
38 #include <bfd.h>
39 #include <boost/smart_ptr.hpp>
40 #include <map>
41 #include <vector>
42 #include <boost/assign.hpp>
43 #include <boost/algorithm/string.hpp>
44 #include <boost/algorithm/string/find_iterator.hpp>
45 
47 
48 #include <sr_robot_msgs/EthercatDebug.h>
49 
51 
52 extern "C"
53 {
55 }
56 
57 class SR09 :
58  public SrEdc
59 {
60 public:
61  SR09();
62 
63  virtual void construct(EtherCAT_SlaveHandler *sh, int &start_address);
64 
65  virtual int initialize(hardware_interface::HardwareInterface *hw, bool allow_unprogrammed = true);
66 
67  virtual void multiDiagnostics(vector<diagnostic_msgs::DiagnosticStatus> &vec, unsigned char *buffer);
68 
69  virtual void packCommand(unsigned char *buffer, bool halt, bool reset);
70 
71  virtual bool unpackState(unsigned char *this_buffer, unsigned char *prev_buffer);
72 
73 protected:
75  std::vector<boost::shared_ptr<rt_pub_int16_t> > realtime_pub_;
76 
79 
81  virtual void reinitialize_boards();
82 
91  virtual void get_board_id_and_can_bus(int board_id, int *can_bus, unsigned int *board_can_id);
92 
93  bool imu_scale_callback_(sr_robot_msgs::SetImuScale::Request &request,
94  sr_robot_msgs::SetImuScale::Response &response, const char *which);
95 
96 
97 private:
98  // std::string firmware_file_name;
99 
102 
103  // Counter for the number of empty buffer we're reading.
104  unsigned int zero_buffer_read;
105 
106  // Robot state interface
107  ros_ethercat_model::RobotState * hw_;
108 
109  ros_ethercat_model::ImuState * imu_state_;
110  boost::shared_ptr<shadow_robot::SrMotorHandLib<ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS,
111  ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_COMMAND> > sr_hand_lib;
112 
120  int16_t cycle_count;
121  // Function to read imu data from edc status into interface
122  void readImu(ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS * status);
123 
126 };
127 
128 
129 /* For the emacs weenies in the crowd.
130 Local Variables:
131  c-basic-offset: 2
132 End:
133 */
134 
135 
136 #endif // SR_EDC_ETHERCAT_DRIVERS_SR09_H
Definition: sr_edc.h:60
int16_t cycle_count
Definition: sr09.h:120
bool imu_scale_change_
Definition: sr09.h:115
realtime_tools::RealtimePublisher< std_msgs::Int16 > rt_pub_int16_t
Definition: sr09.h:74
ros::ServiceServer imu_gyr_scale_server_
Definition: sr09.h:100
boost::shared_ptr< shadow_robot::SrMotorHandLib< ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_COMMAND > > sr_hand_lib
Definition: sr09.h:111
virtual void construct(EtherCAT_SlaveHandler *sh, int &start_address)
Construct function, run at startup to set SyncManagers and FMMUs.
Definition: sr09.cpp:126
unsigned int zero_buffer_read
Definition: sr09.h:104
Definition: sr09.h:57
ros_ethercat_model::RobotState * hw_
Definition: sr09.h:107
std::vector< boost::shared_ptr< rt_pub_int16_t > > realtime_pub_
Definition: sr09.h:75
ros::ServiceServer imu_acc_scale_server_
Definition: sr09.h:101
virtual void packCommand(unsigned char *buffer, bool halt, bool reset)
packs the commands before sending them to the EtherCAT bus
Definition: sr09.cpp:271
virtual void multiDiagnostics(vector< diagnostic_msgs::DiagnosticStatus > &vec, unsigned char *buffer)
This function gives some diagnostics data.
Definition: sr09.cpp:219
virtual void reinitialize_boards()
This function will call the reinitialization function for the boards attached to the CAN bus...
Definition: sr09.cpp:520
virtual void get_board_id_and_can_bus(int board_id, int *can_bus, unsigned int *board_can_id)
Definition: sr09.cpp:526
int imu_scale_gyr_
Definition: sr09.h:113
boost::shared_ptr< realtime_tools::RealtimePublisher< sr_robot_msgs::EthercatDebug > > debug_publisher
Debug real time publisher: publishes the raw ethercat data.
Definition: sr09.h:125
virtual bool unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
This functions receives data from the EtherCAT bus.
Definition: sr09.cpp:393
virtual int initialize(hardware_interface::HardwareInterface *hw, bool allow_unprogrammed=true)
Definition: sr09.cpp:139
SR09()
Constructor of the SR09 driver.
Definition: sr09.cpp:78
int imu_scale_acc_
Definition: sr09.h:114
void readImu(ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS *status)
This funcion reads the ethercat status and fills the imu_state with the relevant values.
Definition: sr09.cpp:334
ros_ethercat_model::ImuState * imu_state_
Definition: sr09.h:109
boost::shared_ptr< realtime_tools::RealtimePublisher< std_msgs::Float64MultiArray > > extra_analog_inputs_publisher
Extra analog inputs real time publisher (+ accelerometer and gyroscope)
Definition: sr09.h:78
This is a parent class for the ROS drivers for any Shadow Robot EtherCAT Dual CAN Slave...
bool imu_scale_callback_(sr_robot_msgs::SetImuScale::Request &request, sr_robot_msgs::SetImuScale::Response &response, const char *which)
Definition: sr09.cpp:187


sr_edc_ethercat_drivers
Author(s): Ugo Cupcic, Yann Sionneau, Toni Oliver
autogenerated on Mon Feb 28 2022 23:50:53