sr08.cpp
Go to the documentation of this file.
00001 
00028 #include <sr_edc_ethercat_drivers/sr08.h>
00029 
00030 #include <dll/ethercat_dll.h>
00031 #include <al/ethercat_AL.h>
00032 #include <dll/ethercat_device_addressed_telegram.h>
00033 #include <dll/ethercat_frame.h>
00034 #include <realtime_tools/realtime_publisher.h>
00035 
00036 #include <math.h>
00037 #include <sstream>
00038 #include <iomanip>
00039 #include <boost/foreach.hpp>
00040 #include <std_msgs/Int16.h>
00041 #include <math.h>
00042 #include <fcntl.h>
00043 #include <stdio.h>
00044 #include <pthread.h>
00045 
00046 #include <sr_utilities/sr_math_utils.hpp>
00047 
00048 using namespace std;
00049 
00050 #include <sr_external_dependencies/types_for_external.h>
00051 
00052 #include <boost/static_assert.hpp>
00053 namespace is_edc_command_32_bits
00054 {
00055   //check is the EDC_COMMAND is 32bits on the computer
00056   //if not, fails
00057   BOOST_STATIC_ASSERT(sizeof(EDC_COMMAND) == 4);
00058 } // namespace is_edc_command_32_bits
00059 
00060 #define ETHERCAT_STATUS_DATA_SIZE sizeof(ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_STATUS)
00061 #define ETHERCAT_COMMAND_DATA_SIZE sizeof(ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_COMMAND)
00062 
00063 #define ETHERCAT_CAN_BRIDGE_DATA_SIZE sizeof(ETHERCAT_CAN_BRIDGE_DATA)
00064 
00065 #define ETHERCAT_COMMAND_DATA_ADDRESS                   PALM_0230_ETHERCAT_COMMAND_DATA_ADDRESS
00066 #define ETHERCAT_STATUS_DATA_ADDRESS                    PALM_0230_ETHERCAT_STATUS_DATA_ADDRESS
00067 #define ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS        PALM_0230_ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS
00068 #define ETHERCAT_CAN_BRIDGE_DATA_STATUS_ADDRESS         PALM_0230_ETHERCAT_CAN_BRIDGE_DATA_STATUS_ADDRESS
00069 
00070 
00071 
00072 PLUGINLIB_EXPORT_CLASS(SR08, EthercatDevice);
00073 
00074 
00081 SR08::SR08()
00082   : SrEdc(),
00083     zero_buffer_read(0),
00084     cycle_count(0)
00085 {
00086 /*
00087   ROS_INFO("There are %d sensors", nb_sensors_const);
00088   ROS_INFO(     "device_pub_freq_const = %d", device_pub_freq_const      );
00089   ROS_INFO(        "ros_pub_freq_const = %d", ros_pub_freq_const         );
00090   ROS_INFO(            "max_iter_const = %d", max_iter_const             );
00091   ROS_INFO(          "nb_sensors_const = %d", nb_sensors_const           );
00092   ROS_INFO("nb_publish_by_unpack_const = %d", nb_publish_by_unpack_const );
00093 */
00094 }
00095 
00100 SR08::~SR08()
00101 {
00102   delete sh_->get_fmmu_config();
00103   delete sh_->get_pd_config();
00104 }
00105 
00139 void SR08::construct(EtherCAT_SlaveHandler *sh, int &start_address)
00140 {
00141   ROS_ASSERT(ETHERCAT_STATUS_0230_AGREED_SIZE == ETHERCAT_STATUS_DATA_SIZE);
00142   ROS_ASSERT(ETHERCAT_COMMAND_0230_AGREED_SIZE == ETHERCAT_COMMAND_DATA_SIZE);
00143 
00144   SrEdc::construct(sh, start_address, ETHERCAT_COMMAND_DATA_SIZE, ETHERCAT_STATUS_DATA_SIZE, ETHERCAT_CAN_BRIDGE_DATA_SIZE,
00145         ETHERCAT_COMMAND_DATA_ADDRESS, ETHERCAT_STATUS_DATA_ADDRESS, ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS, ETHERCAT_CAN_BRIDGE_DATA_STATUS_ADDRESS);
00146 
00147   ROS_INFO("Finished constructing the SR08 driver");
00148 }
00149 
00153 int SR08::initialize(pr2_hardware_interface::HardwareInterface *hw, bool allow_unprogrammed)
00154 {
00155 
00156   int retval = SrEdc::initialize(hw, allow_unprogrammed);
00157 
00158   if(retval != 0)
00159     return retval;
00160 
00161   sr_hand_lib = boost::shared_ptr<shadow_robot::SrMotorHandLib<ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_COMMAND> >( new shadow_robot::SrMotorHandLib<ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_COMMAND>(hw) );
00162 
00163   ROS_INFO("ETHERCAT_STATUS_DATA_SIZE      = %4d bytes", static_cast<int>(ETHERCAT_STATUS_DATA_SIZE) );
00164   ROS_INFO("ETHERCAT_COMMAND_DATA_SIZE     = %4d bytes", static_cast<int>(ETHERCAT_COMMAND_DATA_SIZE) );
00165   ROS_INFO("ETHERCAT_CAN_BRIDGE_DATA_SIZE  = %4d bytes", static_cast<int>(ETHERCAT_CAN_BRIDGE_DATA_SIZE) );
00166 
00167   //initialise the publisher for the extra analog inputs, gyroscope and accelerometer on the palm
00168   extra_analog_inputs_publisher.reset(new realtime_tools::RealtimePublisher<std_msgs::Float64MultiArray>(nodehandle_ , "palm_extras", 10));
00169 
00170 
00171   // Debug real time publisher: publishes the raw ethercat data
00172   debug_publisher = boost::shared_ptr<realtime_tools::RealtimePublisher<sr_robot_msgs::EthercatDebug> >( new realtime_tools::RealtimePublisher<sr_robot_msgs::EthercatDebug>(nodehandle_ , "debug_etherCAT_data", 4));
00173   return retval;
00174 }
00175 
00176 
00183 void SR08::multiDiagnostics(vector<diagnostic_msgs::DiagnosticStatus> &vec, unsigned char *buffer)
00184 {
00185   diagnostic_updater::DiagnosticStatusWrapper &d(diagnostic_status_);
00186 
00187   stringstream name;
00188   d.name = "EtherCAT Dual CAN Palm";
00189   d.summary(d.OK, "OK");
00190   stringstream hwid;
00191   hwid << sh_->get_product_code() << "-" << sh_->get_serial();
00192   d.hardware_id = hwid.str();
00193 
00194   d.clear();
00195   d.addf("Position", "%02d", sh_->get_ring_position());
00196   d.addf("Product Code", "%d", sh_->get_product_code());
00197   d.addf("Serial Number", "%d", sh_->get_serial());
00198   d.addf("Revision", "%d", sh_->get_revision());
00199   d.addf("Counter", "%d", ++counter_);
00200 
00201   d.addf("PIC idle time (in microsecs)", "%d", sr_hand_lib->main_pic_idle_time);
00202   d.addf("Min PIC idle time (since last diagnostics)", "%d", sr_hand_lib->main_pic_idle_time_min);
00203   //reset the idle time min to a big number, to get a fresh number on next diagnostic
00204   sr_hand_lib->main_pic_idle_time_min = 1000;
00205 
00206   this->ethercatDiagnostics(d,2);
00207   vec.push_back(d);
00208 
00209   //Add the diagnostics from the hand
00210   sr_hand_lib->add_diagnostics(vec, d);
00211 
00212   //Add the diagnostics from the tactiles
00213   if( sr_hand_lib->tactiles != NULL )
00214     sr_hand_lib->tactiles->add_diagnostics(vec, d);
00215 }
00216 
00217 
00218 
00234 void SR08::packCommand(unsigned char *buffer, bool halt, bool reset)
00235 {
00236   SrEdc::packCommand(buffer, halt, reset);
00237 
00238   ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_COMMAND   *command = (ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_COMMAND *)(buffer                             );
00239   ETHERCAT_CAN_BRIDGE_DATA                        *message = (ETHERCAT_CAN_BRIDGE_DATA                      *)(buffer + ETHERCAT_COMMAND_DATA_SIZE);
00240 
00241   if ( !flashing )
00242   {
00243     command->EDC_command = EDC_COMMAND_SENSOR_DATA;
00244   }
00245   else
00246   {
00247     command->EDC_command = EDC_COMMAND_CAN_DIRECT_MODE;
00248   }
00249 
00250   //alternate between even and uneven motors
00251   // and ask for the different informations.
00252   sr_hand_lib->build_command(command);
00253 
00254   // TODO For the moment the aux_data_type in the commend will be fixed here. This is for convenience,
00255   // before we separate the aux data (and the prox_mid data) from the UBIO sensor type in the driver.
00256   // After that, this should be done in the aux_data_updater (or something similar ) in the driver
00257   command->aux_data_type = TACTILE_SENSOR_TYPE_MCP320x_TACTILE;
00258 
00259   ROS_DEBUG("Sending command : Type : 0x%02X ; data : 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X",
00260                 command->to_motor_data_type,
00261                 command->motor_data[0],
00262                 command->motor_data[1],
00263                 command->motor_data[2],
00264                 command->motor_data[3],
00265                 command->motor_data[4],
00266                 command->motor_data[5],
00267                 command->motor_data[6],
00268                 command->motor_data[7],
00269                 command->motor_data[8],
00270                 command->motor_data[9],
00271                 command->motor_data[10],
00272                 command->motor_data[11],
00273                 command->motor_data[12],
00274                 command->motor_data[13],
00275                 command->motor_data[14],
00276                 command->motor_data[15],
00277                 command->motor_data[16],
00278                 command->motor_data[17],
00279                 command->motor_data[18],
00280                 command->motor_data[19]);
00281 
00282   build_CAN_message(message);
00283 }
00284 
00285 
00304 bool SR08::unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
00305 {
00306   ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_STATUS  *status_data = (ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_STATUS *)(this_buffer + command_size_                             );
00307   ETHERCAT_CAN_BRIDGE_DATA                      *can_data    = (ETHERCAT_CAN_BRIDGE_DATA                     *)(this_buffer + command_size_ + ETHERCAT_STATUS_DATA_SIZE );
00308   //  int16u                                        *status_buffer = (int16u*)status_data;
00309   static unsigned int num_rxed_packets = 0;
00310 
00311   ++num_rxed_packets;
00312 
00313 
00314   // publishes the debug information (a slightly formatted version of the incoming ethercat packet):
00315   if(debug_publisher->trylock())
00316   {
00317     debug_publisher->msg_.header.stamp = ros::Time::now();
00318 
00319     debug_publisher->msg_.sensors.clear();
00320     for(unsigned int i=0; i<SENSORS_NUM_0220 + 1; ++i)
00321       debug_publisher->msg_.sensors.push_back( status_data->sensors[i] );
00322 
00323     debug_publisher->msg_.motor_data_type.data = static_cast<int>(status_data->motor_data_type);
00324     debug_publisher->msg_.which_motors = status_data->which_motors;
00325     debug_publisher->msg_.which_motor_data_arrived = status_data->which_motor_data_arrived;
00326     debug_publisher->msg_.which_motor_data_had_errors = status_data->which_motor_data_had_errors;
00327 
00328     debug_publisher->msg_.motor_data_packet_torque.clear();
00329     debug_publisher->msg_.motor_data_packet_misc.clear();
00330     for(unsigned int i=0; i < 10; ++i)
00331     {
00332       debug_publisher->msg_.motor_data_packet_torque.push_back( status_data->motor_data_packet[i].torque );
00333       debug_publisher->msg_.motor_data_packet_misc.push_back( status_data->motor_data_packet[i].misc );
00334     }
00335 
00336     debug_publisher->msg_.tactile_data_type = static_cast<unsigned int>(static_cast<int32u>(status_data->tactile_data_type));
00337     debug_publisher->msg_.tactile_data_valid = static_cast<unsigned int>(static_cast<int16u>(status_data->tactile_data_valid));
00338     debug_publisher->msg_.tactile.clear();
00339     for(unsigned int i=0; i < 5; ++i)
00340       debug_publisher->msg_.tactile.push_back( static_cast<unsigned int>(static_cast<int16u>(status_data->tactile[i].word[0])) );
00341 
00342     debug_publisher->msg_.idle_time_us = status_data->idle_time_us;
00343 
00344     debug_publisher->unlockAndPublish();
00345   }
00346 
00347   if (status_data->EDC_command == EDC_COMMAND_INVALID)
00348   {
00349     //received empty message: the pic is not writing to its mailbox.
00350     ++zero_buffer_read;
00351     float percentage_packet_loss = 100.f * ((float)zero_buffer_read / (float)num_rxed_packets);
00352 
00353     ROS_DEBUG("Reception error detected : %d errors out of %d rxed packets (%2.3f%%) ; idle time %dus", zero_buffer_read, num_rxed_packets, percentage_packet_loss, status_data->idle_time_us);
00354     return true;
00355   }
00356 
00357   //We received a coherent message.
00358   //Update the library (positions, diagnostics values, actuators, etc...)
00359   //with the received information
00360   sr_hand_lib->update(status_data);
00361 
00362   //Now publish the additional data at 100Hz (every 10 cycles)
00363   if( cycle_count >= 9)
00364   {
00365     //publish tactiles if we have them
00366     if( sr_hand_lib->tactiles != NULL )
00367       sr_hand_lib->tactiles->publish();
00368 
00369     //And we also publish the additional data (accelerometer / gyroscope / analog inputs)
00370     std_msgs::Float64MultiArray extra_analog_msg;
00371     extra_analog_msg.layout.dim.resize(3);
00372     extra_analog_msg.data.resize(3+3+4);
00373     std::vector<double> data;
00374 
00375     extra_analog_msg.layout.dim[0].label = "accelerometer";
00376     extra_analog_msg.layout.dim[0].size = 3;
00377     extra_analog_msg.data[0] = status_data->sensors[ACCX];
00378     extra_analog_msg.data[1] = status_data->sensors[ACCY];
00379     extra_analog_msg.data[2] = status_data->sensors[ACCZ];
00380 
00381     extra_analog_msg.layout.dim[1].label = "gyrometer";
00382     extra_analog_msg.layout.dim[1].size = 3;
00383     extra_analog_msg.data[3] = status_data->sensors[GYRX];
00384     extra_analog_msg.data[4] = status_data->sensors[GYRY];
00385     extra_analog_msg.data[5] = status_data->sensors[GYRZ];
00386 
00387     extra_analog_msg.layout.dim[2].label = "analog_inputs";
00388     extra_analog_msg.layout.dim[2].size = 4;
00389     extra_analog_msg.data[6] = status_data->sensors[ANA0];
00390     extra_analog_msg.data[7] = status_data->sensors[ANA1];
00391     extra_analog_msg.data[8] = status_data->sensors[ANA2];
00392     extra_analog_msg.data[9] = status_data->sensors[ANA3];
00393 
00394     if( extra_analog_inputs_publisher->trylock() )
00395     {
00396       extra_analog_inputs_publisher->msg_ = extra_analog_msg;
00397       extra_analog_inputs_publisher->unlockAndPublish();
00398     }
00399 
00400     cycle_count = 0;
00401   }
00402   ++cycle_count;
00403 
00404 
00405   //If we're flashing, check is the packet has been acked
00406   if (flashing & !can_packet_acked)
00407   {
00408     if (can_data_is_ack(can_data))
00409     {
00410       can_packet_acked = true;
00411     }
00412   }
00413 
00414   return true;
00415 }
00416 
00417 void SR08::reinitialize_boards()
00418 {
00419   //Reinitialize motors information
00420   sr_hand_lib->reinitialize_motors();
00421 }
00422 
00423 void SR08::get_board_id_and_can_bus(int board_id, int *can_bus, unsigned int *board_can_id)
00424 {
00425   // We're using 2 can busses,
00426   // if motor id is between 0 and 9, then we're using the can_bus 1
00427   // else, we're using the can bus 2.
00428   int motor_id_tmp = board_id;
00429   if( motor_id_tmp > 9 )
00430   {
00431     motor_id_tmp -= 10;
00432     *can_bus = 2;
00433   }
00434   else
00435   {
00436     *can_bus = 1;
00437   }
00438 
00439   *board_can_id = motor_id_tmp;
00440 }
00441 
00442 /* For the emacs weenies in the crowd.
00443    Local Variables:
00444    c-basic-offset: 2
00445    End:
00446 */


sr_edc_ethercat_drivers
Author(s): Yann Sionneau, Ugo Cupcic / ugo@shadowrobot.com, software@shadowrobot.com
autogenerated on Mon Oct 6 2014 07:38:25