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.hpp>
#include <vector>
#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 <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 crc_unions
{
  typedef union
  {
    int16u word;
    int8u byte[2];
  } union16;
}

namespace shadow_robot
{
  template <class StatusType, class CommandType>
  class SrRobotLib
  {
  public:
    SrRobotLib(pr2_hardware_interface::HardwareInterface *hw);
    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 );

    shadow_joints::CalibrationMap calibration_map;

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

    int main_pic_idle_time;

    int main_pic_idle_time_min;

    operation_mode::device_update_state::DeviceUpdateState tactile_current_state;

  protected:
    boost::ptr_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,
                            std::vector<sr_actuator::SrGenericActuator*> actuators) = 0;

    void calibrate_joint(boost::ptr_vector<shadow_joints::Joint>::iterator joint_tmp, StatusType* status_data);


    void process_position_sensor_data(boost::ptr_vector<shadow_joints::Joint>::iterator joint_tmp, StatusType* status_data, double timestamp);

    sr_actuator::SrActuatorState* get_joint_actuator_state(boost::ptr_vector<shadow_joints::Joint>::iterator joint_tmp);

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


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


    ros::NodeHandle nh_tilde;

    ros::NodeHandle nodehandle_;


#ifdef DEBUG_PUBLISHER

    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

    std::vector<generic_updater::UpdateConfig> generic_sensor_update_rate_configs_vector;
    std::vector<generic_updater::UpdateConfig> pst3_sensor_update_rate_configs_vector;
    std::vector<generic_updater::UpdateConfig> biotac_sensor_update_rate_configs_vector;
    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[];


    bool nullify_demand_;
    ros::ServiceServer nullify_demand_server_;

    void checkSelfTests();

    boost::shared_ptr<SrSelfTest> self_tests_;

    boost::shared_ptr<boost::thread> self_test_thread_;
  };//end class
}//end namespace

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

#endif