sr09.cpp

Go to the documentation of this file.

#include <sr_edc_ethercat_drivers/sr09.h>

#include <realtime_tools/realtime_publisher.h>

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

#include <sr_utilities/sr_math_utils.hpp>

using std::string;
using std::stringstream;
using std::vector;

#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_0240_PALM_EDC_STATUS)
#define ETHERCAT_COMMAND_DATA_SIZE sizeof(ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_COMMAND)

#define ETHERCAT_CAN_BRIDGE_DATA_SIZE sizeof(ETHERCAT_CAN_BRIDGE_DATA)

#define ETHERCAT_COMMAND_DATA_ADDRESS                   PALM_0240_ETHERCAT_COMMAND_DATA_ADDRESS
#define ETHERCAT_STATUS_DATA_ADDRESS                    PALM_0240_ETHERCAT_STATUS_DATA_ADDRESS
#define ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS        PALM_0240_ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS
#define ETHERCAT_CAN_BRIDGE_DATA_STATUS_ADDRESS         PALM_0240_ETHERCAT_CAN_BRIDGE_DATA_STATUS_ADDRESS


PLUGINLIB_EXPORT_CLASS(SR09, EthercatDevice);

SR09::SR09()
        : zero_buffer_read(0),
          cycle_count(0),
          imu_scale_change_(false)
{
  /*
    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 );
   */
}

void SR09::construct(EtherCAT_SlaveHandler *sh, int &start_address)
{
  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 SR09 driver");
}

int SR09::initialize(hardware_interface::HardwareInterface *hw, bool allow_unprogrammed)
{
  int retval = SR0X::initialize(hw, allow_unprogrammed);

  if (retval != 0)
  {
    return retval;
  }
  hw_ = static_cast<ros_ethercat_model::RobotState *> (hw);

  sr_hand_lib = boost::shared_ptr<shadow_robot::SrMotorHandLib<ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS,
          ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_COMMAND> >(
          new shadow_robot::SrMotorHandLib<ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS,
                  ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_COMMAND>(hw, nodehandle_, nh_tilde_,
                                                                 device_id_, device_joint_prefix_));

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


  std::string imu_name = device_joint_prefix_ + "imu";
  imu_state_ = hw_->getImuState(imu_name);

  imu_gyr_scale_server_ =  nodehandle_.advertiseService
    <sr_robot_msgs::SetImuScale::Request, sr_robot_msgs::SetImuScale::Response>
    ("/" + imu_name + "/set_gyr_scale", boost::bind(&SR09::imu_scale_callback_, this, _1, _2, "gyr"));
  imu_acc_scale_server_ =  nodehandle_.advertiseService
    <sr_robot_msgs::SetImuScale::Request, sr_robot_msgs::SetImuScale::Response>
    ("/" + imu_name + "/set_acc_scale", boost::bind(&SR09::imu_scale_callback_, this, _1, _2, "acc"));

  ros::param::param<int>("/" + imu_name + "/acc_scale", imu_scale_acc_, 0);
  ros::param::param<int>("/" + imu_name + "/gyr_scale", imu_scale_gyr_, 0);

  imu_scale_change_ = true;

  return retval;
}

bool SR09::imu_scale_callback_(sr_robot_msgs::SetImuScale::Request & request,
                         sr_robot_msgs::SetImuScale::Response & response,
                         const char *which)
{
  if (request.scale == 0 || request.scale == 1 || request.scale == 2)
  {
    if (which == "acc")
    {
      imu_scale_acc_ = request.scale;
    }
    else if (which == "gyr")
    {
      imu_scale_gyr_ = request.scale;
    }

    imu_scale_change_ = true;
    return true;
  }
  else
  {
    ROS_WARN_STREAM("Tried to set illegal value: " << (int) request.scale);
    return false;
  }
}


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

  stringstream name;
  string prefix = device_id_.empty() ? device_id_ : (device_id_ + " ");
  d.name = prefix + "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 SR09::packCommand(unsigned char *buffer, bool halt, bool reset)
{
  SrEdc::packCommand(buffer, halt, reset);

  ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_COMMAND *command =
          reinterpret_cast<ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_COMMAND *>(buffer);
  ETHERCAT_CAN_BRIDGE_DATA *message = reinterpret_cast<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;
  }

  if (imu_scale_change_)
  {
    command->imu_command.command = IMU_COMMAND_SET_SCALE;
    command->imu_command.argument[0] = imu_scale_acc_;
    command->imu_command.argument[1] = imu_scale_gyr_;
    imu_scale_change_ = false;
  }
  else
  {
    command->imu_command.command = IMU_COMMAND_NONE;
  }

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

  build_CAN_message(message);
}

void SR09::readImu(ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS * status_data)
{
  imu_state_->data_.orientation[0] = 0.0; imu_state_->data_.orientation[1] = 0.0;
  imu_state_->data_.orientation[2] = 0.0; imu_state_->data_.orientation[3] = 1.0;

  double acc_multiplier = 1 << imu_scale_acc_;
  double gyr_multiplier = 1 << imu_scale_gyr_;

  int zero_catch = 0;

  for (size_t x = 0; x < 2; ++x)
  {
    if (status_data->sensors[ACCX + 0] == 0)
    {
      ++zero_catch;
    }
    if (status_data->sensors[GYRX + 0] == 0)
    {
      ++zero_catch;
    }
  }
  if (zero_catch <= 1)
  {
    imu_state_->data_.linear_acceleration[0] = acc_multiplier * static_cast<int16s>(status_data->sensors[ACCX]);
    imu_state_->data_.linear_acceleration[1] = acc_multiplier * static_cast<int16s>(status_data->sensors[ACCY]);
    imu_state_->data_.linear_acceleration[2] = acc_multiplier * static_cast<int16s>(status_data->sensors[ACCZ]);

    imu_state_->data_.angular_velocity[0] = gyr_multiplier * static_cast<int16s>(status_data->sensors[GYRX]);
    imu_state_->data_.angular_velocity[1] = gyr_multiplier * static_cast<int16s>(status_data->sensors[GYRY]);
    imu_state_->data_.angular_velocity[2] = gyr_multiplier * static_cast<int16s>(status_data->sensors[GYRZ]);
  }
  for (size_t x = 0; x < 9; ++x)
  {
    imu_state_->data_.linear_acceleration_covariance[x] = 0.0;
    imu_state_->data_.angular_velocity_covariance[x] = 0.0;
    imu_state_->data_.orientation_covariance[x] = 0.0;
  }
}



bool SR09::unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
{
  ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS *status_data =
          reinterpret_cast<ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS *>(this_buffer + command_size_);
  ETHERCAT_CAN_BRIDGE_DATA *can_data = reinterpret_cast<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;

  readImu(status_data);

  // 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 < 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 * (static_cast<float>(zero_buffer_read) /
                                            static_cast<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 >= 10)
  {
    // 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 SR09::reinitialize_boards()
{
  // Reinitialize motors information
  sr_hand_lib->reinitialize_motors();
}

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

  *board_can_id = motor_id_tmp;
}

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