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


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