sr_edc_muscle.cpp

Go to the documentation of this file.

#include <sr_edc_ethercat_drivers/sr_edc_muscle.h>

#include <dll/ethercat_dll.h>
#include <al/ethercat_AL.h>
#include <dll/ethercat_device_addressed_telegram.h>
#include <dll/ethercat_frame.h>
#include <realtime_tools/realtime_publisher.h>

#include <math.h>
#include <sstream>
#include <iomanip>
#include <boost/foreach.hpp>
#include <std_msgs/Int16.h>
#include <math.h>
#include <fcntl.h>
#include <stdio.h>
#include <pthread.h>

#include <sr_utilities/sr_math_utils.hpp>

using namespace std;

#include <sr_external_dependencies/types_for_external.h>

#include <boost/static_assert.hpp>
namespace is_edc_command_32_bits
{
  //check is the EDC_COMMAND is 32bits on the computer
  //if not, fails
  BOOST_STATIC_ASSERT(sizeof(EDC_COMMAND) == 4);
} // namespace is_edc_command_32_bits

#define ETHERCAT_STATUS_DATA_SIZE sizeof(ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_STATUS)
#define ETHERCAT_COMMAND_DATA_SIZE sizeof(ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_COMMAND)

#define ETHERCAT_CAN_BRIDGE_DATA_SIZE sizeof(ETHERCAT_CAN_BRIDGE_DATA)

#define ETHERCAT_COMMAND_DATA_ADDRESS                   PALM_0300_ETHERCAT_COMMAND_DATA_ADDRESS
#define ETHERCAT_STATUS_DATA_ADDRESS                    PALM_0300_ETHERCAT_STATUS_DATA_ADDRESS
#define ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS        PALM_0300_ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS
#define ETHERCAT_CAN_BRIDGE_DATA_STATUS_ADDRESS         PALM_0300_ETHERCAT_CAN_BRIDGE_DATA_STATUS_ADDRESS



PLUGINLIB_EXPORT_CLASS(SrEdcMuscle, EthercatDevice);


SrEdcMuscle::SrEdcMuscle()
  : SrEdc(),
    zero_buffer_read(0),
    cycle_count(0)
{
/*
  ROS_INFO("There are %d sensors", nb_sensors_const);
  ROS_INFO(     "device_pub_freq_const = %d", device_pub_freq_const      );
  ROS_INFO(        "ros_pub_freq_const = %d", ros_pub_freq_const         );
  ROS_INFO(            "max_iter_const = %d", max_iter_const             );
  ROS_INFO(          "nb_sensors_const = %d", nb_sensors_const           );
  ROS_INFO("nb_publish_by_unpack_const = %d", nb_publish_by_unpack_const );
*/
}

SrEdcMuscle::~SrEdcMuscle()
{
  delete sh_->get_fmmu_config();
  delete sh_->get_pd_config();
}

void SrEdcMuscle::construct(EtherCAT_SlaveHandler *sh, int &start_address)
{
  ROS_ASSERT(ETHERCAT_STATUS_0300_AGREED_SIZE == ETHERCAT_STATUS_DATA_SIZE);
  ROS_ASSERT(ETHERCAT_COMMAND_0300_AGREED_SIZE == ETHERCAT_COMMAND_DATA_SIZE);

  SrEdc::construct(sh, start_address, ETHERCAT_COMMAND_DATA_SIZE, ETHERCAT_STATUS_DATA_SIZE, ETHERCAT_CAN_BRIDGE_DATA_SIZE,
        ETHERCAT_COMMAND_DATA_ADDRESS, ETHERCAT_STATUS_DATA_ADDRESS, ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS, ETHERCAT_CAN_BRIDGE_DATA_STATUS_ADDRESS);

  ROS_INFO("Finished constructing the SrEdcMuscle driver");
}

int SrEdcMuscle::initialize(pr2_hardware_interface::HardwareInterface *hw, bool allow_unprogrammed)
{

  int retval = SrEdc::initialize(hw, allow_unprogrammed);

  if(retval != 0)
    return retval;

  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) );

  ROS_INFO("ETHERCAT_STATUS_DATA_SIZE      = %4d bytes", static_cast<int>(ETHERCAT_STATUS_DATA_SIZE) );
  ROS_INFO("ETHERCAT_COMMAND_DATA_SIZE     = %4d bytes", static_cast<int>(ETHERCAT_COMMAND_DATA_SIZE) );
  ROS_INFO("ETHERCAT_CAN_BRIDGE_DATA_SIZE  = %4d bytes", static_cast<int>(ETHERCAT_CAN_BRIDGE_DATA_SIZE) );

  //initialise the publisher for the extra analog inputs, gyroscope and accelerometer on the palm
  extra_analog_inputs_publisher.reset(new realtime_tools::RealtimePublisher<std_msgs::Float64MultiArray>(nodehandle_ , "palm_extras", 10));


  // Debug real time publisher: publishes the raw ethercat data
  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));
  return retval;
}


void SrEdcMuscle::multiDiagnostics(vector<diagnostic_msgs::DiagnosticStatus> &vec, unsigned char *buffer)
{
  diagnostic_updater::DiagnosticStatusWrapper &d(diagnostic_status_);

  stringstream name;
  d.name = "EtherCAT Dual CAN Palm";
  d.summary(d.OK, "OK");
  stringstream hwid;
  hwid << sh_->get_product_code() << "-" << sh_->get_serial();
  d.hardware_id = hwid.str();

  d.clear();
  d.addf("Position", "%02d", sh_->get_ring_position());
  d.addf("Product Code", "%d", sh_->get_product_code());
  d.addf("Serial Number", "%d", sh_->get_serial());
  d.addf("Revision", "%d", sh_->get_revision());
  d.addf("Counter", "%d", ++counter_);

  d.addf("PIC idle time (in microsecs)", "%d", sr_hand_lib->main_pic_idle_time);
  d.addf("Min PIC idle time (since last diagnostics)", "%d", sr_hand_lib->main_pic_idle_time_min);
  //reset the idle time min to a big number, to get a fresh number on next diagnostic
  sr_hand_lib->main_pic_idle_time_min = 1000;

  this->ethercatDiagnostics(d,2);
  vec.push_back(d);

  //Add the diagnostics from the hand
  sr_hand_lib->add_diagnostics(vec, d);

  //Add the diagnostics from the tactiles
  if( sr_hand_lib->tactiles != NULL )
    sr_hand_lib->tactiles->add_diagnostics(vec, d);
}



void SrEdcMuscle::packCommand(unsigned char *buffer, bool halt, bool reset)
{
  SrEdc::packCommand(buffer, halt, reset);

  ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_COMMAND   *command = (ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_COMMAND *)(buffer                             );
  ETHERCAT_CAN_BRIDGE_DATA                        *message = (ETHERCAT_CAN_BRIDGE_DATA                      *)(buffer + ETHERCAT_COMMAND_DATA_SIZE);

  if ( !flashing )
  {
    command->EDC_command = EDC_COMMAND_SENSOR_DATA;
  }
  else
  {
    command->EDC_command = EDC_COMMAND_CAN_DIRECT_MODE;
  }

  //alternate between even and uneven motors
  // and ask for the different informations.
  sr_hand_lib->build_command(command);

  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",
                command->to_muscle_data_type,
                command->muscle_data[0],
                command->muscle_data[1],
                command->muscle_data[2],
                command->muscle_data[3],
                command->muscle_data[4],
                command->muscle_data[5],
                command->muscle_data[6],
                command->muscle_data[7],
                command->muscle_data[8],
                command->muscle_data[9],
                command->muscle_data[10],
                command->muscle_data[11],
                command->muscle_data[12],
                command->muscle_data[13],
                command->muscle_data[14],
                command->muscle_data[15],
                command->muscle_data[16],
                command->muscle_data[17],
                command->muscle_data[18],
                command->muscle_data[19]);

  build_CAN_message(message);
}


bool SrEdcMuscle::unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
{
  ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_STATUS  *status_data = (ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_STATUS *)(this_buffer + command_size_                             );
  ETHERCAT_CAN_BRIDGE_DATA                      *can_data    = (ETHERCAT_CAN_BRIDGE_DATA                     *)(this_buffer + command_size_ + ETHERCAT_STATUS_DATA_SIZE );
  //  int16u                                        *status_buffer = (int16u*)status_data;
  static unsigned int num_rxed_packets = 0;

  ++num_rxed_packets;


  // publishes the debug information (a slightly formatted version of the incoming ethercat packet):
  if(debug_publisher->trylock())
  {
    debug_publisher->msg_.header.stamp = ros::Time::now();

    debug_publisher->msg_.sensors.clear();
    for(unsigned int i=0; i<SENSORS_NUM_0220 + 1; ++i)
      debug_publisher->msg_.sensors.push_back( status_data->sensors[i] );
    /*
    debug_publisher->msg_.motor_data_type.data = static_cast<int>(status_data->motor_data_type);
    debug_publisher->msg_.which_motors = status_data->which_motors;
    debug_publisher->msg_.which_motor_data_arrived = status_data->which_motor_data_arrived;
    debug_publisher->msg_.which_motor_data_had_errors = status_data->which_motor_data_had_errors;
    */
    debug_publisher->msg_.motor_data_packet_torque.clear();
    debug_publisher->msg_.motor_data_packet_misc.clear();

    for(unsigned int i=0; i < 8; ++i)
    {
      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);
      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);
      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);
      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);
      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);
    }
    /*
    for(unsigned int i=0; i < 10; ++i)
    {
      debug_publisher->msg_.motor_data_packet_torque.push_back( status_data->motor_data_packet[i].torque );
      debug_publisher->msg_.motor_data_packet_misc.push_back( status_data->motor_data_packet[i].misc );
    }
    */
    debug_publisher->msg_.tactile_data_type = static_cast<unsigned int>(static_cast<int32u>(status_data->tactile_data_type));
    debug_publisher->msg_.tactile_data_valid = static_cast<unsigned int>(static_cast<int16u>(status_data->tactile_data_valid));
    debug_publisher->msg_.tactile.clear();
    for(unsigned int i=0; i < 5; ++i)
      debug_publisher->msg_.tactile.push_back( static_cast<unsigned int>(static_cast<int16u>(status_data->tactile[i].word[0])) );

    debug_publisher->msg_.idle_time_us = status_data->idle_time_us;

    debug_publisher->unlockAndPublish();
  }

  if (status_data->EDC_command == EDC_COMMAND_INVALID)
  {
    //received empty message: the pic is not writing to its mailbox.
    ++zero_buffer_read;
    float percentage_packet_loss = 100.f * ((float)zero_buffer_read / (float)num_rxed_packets);

    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);
    return true;
  }

  //We received a coherent message.
  //Update the library (positions, diagnostics values, actuators, etc...)
  //with the received information
  sr_hand_lib->update(status_data);

  //Now publish the additional data at 100Hz (every 10 cycles)
  if( cycle_count >= 9)
  {
    //publish tactiles if we have them
    if( sr_hand_lib->tactiles != NULL )
      sr_hand_lib->tactiles->publish();

    //And we also publish the additional data (accelerometer / gyroscope / analog inputs)
    std_msgs::Float64MultiArray extra_analog_msg;
    extra_analog_msg.layout.dim.resize(3);
    extra_analog_msg.data.resize(3+3+4);
    std::vector<double> data;

    extra_analog_msg.layout.dim[0].label = "accelerometer";
    extra_analog_msg.layout.dim[0].size = 3;
    extra_analog_msg.data[0] = status_data->sensors[ACCX];
    extra_analog_msg.data[1] = status_data->sensors[ACCY];
    extra_analog_msg.data[2] = status_data->sensors[ACCZ];

    extra_analog_msg.layout.dim[1].label = "gyrometer";
    extra_analog_msg.layout.dim[1].size = 3;
    extra_analog_msg.data[3] = status_data->sensors[GYRX];
    extra_analog_msg.data[4] = status_data->sensors[GYRY];
    extra_analog_msg.data[5] = status_data->sensors[GYRZ];

    extra_analog_msg.layout.dim[2].label = "analog_inputs";
    extra_analog_msg.layout.dim[2].size = 4;
    extra_analog_msg.data[6] = status_data->sensors[ANA0];
    extra_analog_msg.data[7] = status_data->sensors[ANA1];
    extra_analog_msg.data[8] = status_data->sensors[ANA2];
    extra_analog_msg.data[9] = status_data->sensors[ANA3];

    if( extra_analog_inputs_publisher->trylock() )
    {
      extra_analog_inputs_publisher->msg_ = extra_analog_msg;
      extra_analog_inputs_publisher->unlockAndPublish();
    }

    cycle_count = 0;
  }
  ++cycle_count;


  //If we're flashing, check is the packet has been acked
  if (flashing & !can_packet_acked)
  {
    if (can_data_is_ack(can_data))
    {
      can_packet_acked = true;
    }
  }

  return true;
}

void SrEdcMuscle::reinitialize_boards()
{
  //Reinitialize motors information
  sr_hand_lib->reinitialize_motors();
}

void SrEdcMuscle::get_board_id_and_can_bus(int board_id, int *can_bus, unsigned int *board_can_id)
{
  // We're using 2 can busses,
  // if muscle_driver_board id is between 0 and 1, then we're using the can_bus 1
  // else, we're using the can bus 2.
  int muscle_driver_board_id_tmp = board_id;
  if( muscle_driver_board_id_tmp > 1 )
  {
    muscle_driver_board_id_tmp -= 2;
    *can_bus = 2;
  }
  else
  {
    *can_bus = 1;
  }

  *board_can_id = muscle_driver_board_id_tmp;
}

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