sr_robot_lib.hpp

Go to the documentation of this file.

#ifndef _SR_ROBOT_LIB_HPP_
#define _SR_ROBOT_LIB_HPP_

#include <boost/smart_ptr.hpp>
#include <boost/ptr_container/ptr_vector.hpp>
#include <boost/thread/thread.hpp>
#include <boost/thread/mutex.hpp>
#include <vector>
#include <utility>
#include <string>
#include <deque>

// used to publish debug values
#include <std_msgs/Int16.h>

#include <sr_hardware_interface/sr_actuator.hpp>

#include <diagnostic_msgs/DiagnosticStatus.h>
#include <diagnostic_updater/DiagnosticStatusWrapper.h>

#include <ros_ethercat_model/robot_state.hpp>

#include <sr_robot_msgs/NullifyDemand.h>
#include <sr_robot_msgs/SetDebugData.h>

#include <sr_utilities/sr_math_utils.hpp>
#include <sr_utilities/calibration.hpp>
#include <sr_utilities/thread_safe_map.hpp>

#include <sr_self_test/sr_self_test.hpp>

#include "sr_robot_lib/sr_joint_motor.hpp"
#include "sr_robot_lib/generic_tactiles.hpp"

#include <sr_external_dependencies/types_for_external.h>

extern "C"
{
#include <sr_external_dependencies/external/0220_palm_edc/0220_palm_edc_ethercat_protocol.h>
#include <sr_external_dependencies/external/0230_palm_edc_TS/0230_palm_edc_ethercat_protocol.h>
#include <sr_external_dependencies/external/0320_palm_edc_muscle/0320_palm_edc_ethercat_protocol.h>
#include <sr_external_dependencies/external/simplemotor-bootloader/bootloader.h>
}

namespace operation_mode
{
namespace robot_state
{
  enum RobotState
  {
    INITIALIZATION,
    OPERATION,
    SHUTDOWN
  };
}  // namespace robot_state
}  // namespace operation_mode

namespace crc_unions
{
union CRCUnion
{
  int16u word;
  int8u byte[2];
};

typedef CRCUnion union16;
}

namespace shadow_robot
{
template<class StatusType, class CommandType>
class SrRobotLib
{
public:
  SrRobotLib(hardware_interface::HardwareInterface *hw, ros::NodeHandle nh, ros::NodeHandle nhtilde,
             std::string device_id, std::string joint_prefix);

  virtual ~SrRobotLib()
  {
  }

  virtual void update(StatusType *status_data) = 0;

  virtual void build_command(CommandType *command) = 0;

  void build_tactile_command(CommandType *command);

  void update_tactile_info(StatusType *status);

  virtual void add_diagnostics(std::vector<diagnostic_msgs::DiagnosticStatus> &vec,
                               diagnostic_updater::DiagnosticStatusWrapper &d) = 0;

  void reinitialize_sensors();

  bool nullify_demand_callback(sr_robot_msgs::NullifyDemand::Request &request,
                               sr_robot_msgs::NullifyDemand::Response &response);


  boost::shared_ptr<tactiles::GenericTactiles<StatusType, CommandType> > tactiles;

  int main_pic_idle_time;

  int main_pic_idle_time_min;

  // Current update state of the sensors (initialization, operation..)
  operation_mode::device_update_state::DeviceUpdateState tactile_current_state;

  ros_ethercat_model::RobotState *hw_;

protected:
  // True if we want to set the demand to 0 (stop the controllers)
  bool nullify_demand_;

  std::vector<shadow_joints::Joint> joints_vector;

  virtual void initialize(std::vector<std::string> joint_names, std::vector<int> actuator_ids,
                          std::vector<shadow_joints::JointToSensor> joint_to_sensors) = 0;

  virtual void calibrate_joint(std::vector<shadow_joints::Joint>::iterator joint_tmp, StatusType *status_data) = 0;

  virtual ros_ethercat_model::Actuator *get_joint_actuator(std::vector<shadow_joints::Joint>::iterator joint_tmp) = 0;

  std::vector<shadow_joints::JointToSensor> read_joint_to_sensor_mapping();

  shadow_joints::CalibrationMap read_joint_calibration();

  std::vector<generic_updater::UpdateConfig> read_update_rate_configs(std::string base_param, int nb_data_defined,
                                                                      const char *human_readable_data_types[],
                                                                      const int32u data_types[]);

  void tactile_init_timer_callback(const ros::TimerEvent &event);

  boost::shared_ptr<shadow_robot::JointCalibration> calibration_tmp;


  ros::NodeHandle nh_tilde;

  // The ROS service handler for nullifying the demand
  ros::ServiceServer nullify_demand_server_;

  // boost::shared_ptr<SrSelfTest> self_tests_;

  // Thread for running the tests in parallel when doing the tests on real hand
  boost::shared_ptr<boost::thread> self_test_thread_;

  ros::NodeHandle nodehandle_;

  std::string joint_prefix_;
  std::string device_id_;


#ifdef DEBUG_PUBLISHER
  // These publishers are useful for debugging
  static const int nb_debug_publishers_const;
  std::vector<ros::Publisher> debug_publishers;
  std::vector<boost::shared_ptr<std::pair<int, int> > > debug_motor_indexes_and_data;
  // static const int debug_mutex_lock_wait_time;
  // boost::shared_mutex debug_mutex;
  ros::NodeHandle node_handle;
  std_msgs::Int16 msg_debug;
#endif

  // The update rate for each sensor information type
  std::vector<generic_updater::UpdateConfig> generic_sensor_update_rate_configs_vector;
  // The update rate for each sensor information type
  std::vector<generic_updater::UpdateConfig> pst3_sensor_update_rate_configs_vector;
  // The update rate for each sensor information type
  std::vector<generic_updater::UpdateConfig> biotac_sensor_update_rate_configs_vector;
  // The update rate for each sensor information type
  std::vector<generic_updater::UpdateConfig> ubi0_sensor_update_rate_configs_vector;

  static const int nb_sensor_data;
  static const char *human_readable_sensor_data_types[];
  static const int32u sensor_data_types[];

  // This avoids the tests blocking the main thread
  void checkSelfTests();

public:
  static const double tactile_timeout;
  ros::Duration tactile_init_max_duration;
  ros::Timer tactile_check_init_timeout_timer;

  boost::shared_ptr<boost::mutex> lock_tactile_init_timeout_;

  boost::shared_ptr<tactiles::GenericTactiles<StatusType, CommandType> > tactiles_init;

  shadow_joints::CalibrationMap calibration_map;
};  // end class
}  // namespace shadow_robot

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

#endif