riq_hand.h

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

#ifndef RIQ_HAND_ETHERCAT_HARDWARE__RIQ_HAND_H
#define RIQ_HAND_ETHERCAT_HARDWARE__RIQ_HAND_H

#include "ethercat_hardware/ethercat_device.h"
#include "riq_msgs/RIQHandCommand.h"
#include "riq_msgs/RIQHandState.h"
#include "riq_hand_ethercat_hardware/ethercat_al.h"
#include <boost/thread/mutex.hpp>
#include "realtime_tools/realtime_publisher.h"


namespace riq_hand_ethercat_hardware
{

struct RIQHandDiagnostics
{
  RIQHandDiagnostics();
  ECatALStatusAll al_status_;
  bool al_status_good_;
};

class RIQHand : public EthercatDevice
{
public:
  RIQHand();
  ~RIQHand();
  void construct(EtherCAT_SlaveHandler *sh, int &start_address);
  int initialize(pr2_hardware_interface::HardwareInterface *, bool);
  void packCommand(unsigned char *buffer, bool halt, bool reset);
  bool unpackState(unsigned char *this_buffer, unsigned char *prev_buffer);
  void diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *);
  void collectDiagnostics(EthercatCom *com);

  static const unsigned COMMAND_PHY_ADDR = 0x1100;
  static const unsigned COMMAND_SIZE     = 12;
  static const unsigned STATUS_PHY_ADDR  = 0x1D00;
  static const unsigned STATUS_SIZE      = 12;

  static const unsigned MBX_COMMAND_PHY_ADDR = 0x1000;
  static const unsigned MBX_COMMAND_SIZE     = 128;
  static const unsigned MBX_STATUS_PHY_ADDR  = 0x1080;
  static const unsigned MBX_STATUS_SIZE      = 128;

  enum {PRODUCT_CODE = 11};

protected:

  static void sizeAssert(); 

  static void disableWatchdog(EtherCAT_SlaveHandler *sh);

  void commandCallback(const riq_msgs::RIQHandCommand::ConstPtr& msg);
  ros::Subscriber commandSubscriber_;

  realtime_tools::RealtimePublisher<riq_msgs::RIQHandState> publisher_;
  unsigned state_cycle_count_;
  static const unsigned STATE_PUBLISH_PERIOD = 10; 

  bool should_publish_;
  

  boost::mutex command_mutex_;

  riq_msgs::RIQHandCommand new_command_;
  riq_msgs::RIQHandCommand command_;

  unsigned scaleAndSaturate255(double value);

  RIQHandDiagnostics collect_diagnostics_;
  boost::mutex diagnostics_mutex_;

  RIQHandDiagnostics publish_diagnostics_;

  // Watchdog reset will performed by non-realtime thread and flag will be 
  // cleared when reset is completed successfully
  volatile bool needs_watchdog_reset_;

  // Outwardly, reset is just like calibration (homing) of PR2.
  // When device is released from reset, it moves finger back and forth to find end-points
  bool needs_reset_;
  int needs_reset_countdown_;


  // Used by diagnostics publish thread to determine what error message to display.
  volatile unsigned latched_fault_code_;

  // When a FAULT_MAJOR_SCISSORS_DISPLACEMENT, FAULT_MAJOR_FINGER_DISPLACEMENT, or watchdog
  // timeout has occurred,  the software will clear the error code once reset_motors is called.
  // However, the fault code from the device will still be present for some number of cycles
  // when initializing.   These downcounters prevents driver from imediately triggering  
  // halt state after after reset_motors is called because fault code has not yet cleared
  int after_reset_countdown_;
  int after_watchdog_reset_countdown_;
  
  bool halted_;
};

}; //namespace riq_hand_ethercat_hardware

#endif /* RIQ_HAND_ETHERCAT_HARDWARE__RIQ_HAND_H */