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/ForceController.h>
#include <sr_robot_msgs/NullifyDemand.h>
#include <sr_robot_msgs/SetDebugData.h>
#include <sr_robot_msgs/ControlType.h>
#include <sr_robot_msgs/ChangeControlType.h>
#include <sr_robot_msgs/MotorSystemControls.h>
#include <sr_robot_msgs/ChangeMotorSystemControls.h>

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

#include "sr_robot_lib/sr_joint_motor.hpp"
#include "sr_robot_lib/motor_updater.hpp"
#include "sr_robot_lib/generic_tactiles.hpp"
#include "sr_robot_lib/motor_data_checker.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/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
{
  class SrRobotLib
  {
  public:
    SrRobotLib(pr2_hardware_interface::HardwareInterface *hw);
    ~SrRobotLib() {};

    void update(ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_STATUS* status_data);

    void build_motor_command(ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_COMMAND* command);

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

    void reinitialize_motors();

    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> tactiles;
    boost::shared_ptr<tactiles::GenericTactiles> tactiles_init;

    int main_pic_idle_time;

    int main_pic_idle_time_min;

    operation_mode::device_update_state::DeviceUpdateState motor_current_state;

    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> motor_ids,
                            std::vector<shadow_joints::JointToSensor> joint_to_sensors,
                            std::vector<sr_actuator::SrActuator*> actuators) = 0;

    void calibrate_joint(boost::ptr_vector<shadow_joints::Joint>::iterator joint_tmp);

    void read_additional_data(boost::ptr_vector<shadow_joints::Joint>::iterator joint_tmp);

    std::vector<std::pair<std::string, bool> > humanize_flags(int flag);

    void generate_force_control_config(int motor_index, int max_pwm, int sg_left, int sg_right,
                                       int f, int p, int i, int d, int imax,
                                       int deadband, int sign);

    boost::shared_ptr<generic_updater::MotorUpdater> motor_updater_;


    typedef std::pair<int, std::vector<crc_unions::union16> > ForceConfig;
    std::queue<ForceConfig, std::list<ForceConfig> > reconfig_queue;
    int config_index;

    std::queue<short, std::list<short> > reset_motors_queue;

    sr_actuator::SrActuator* actuator;
    ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_STATUS* status_data;
    boost::shared_ptr<shadow_robot::JointCalibration> calibration_tmp;

    int motor_index_full;
    int index_motor_in_msg;

    int8u crc_byte;
    int16u crc_result;
    int8u crc_i;

    ros::NodeHandle nh_tilde;

#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> motor_update_rate_configs_vector;
    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;

    boost::shared_ptr<generic_updater::MotorDataChecker> motor_data_checker;

    bool nullify_demand_;
    ros::ServiceServer nullify_demand_server_;

    sr_robot_msgs::ControlType control_type_;
    bool control_type_changed_flag_;
    ros::ServiceServer change_control_type_;
    boost::shared_ptr<boost::mutex> lock_command_sending_;

    bool change_control_type_callback_( sr_robot_msgs::ChangeControlType::Request& request,
                                        sr_robot_msgs::ChangeControlType::Response& response );

    bool change_control_parameters(int16_t control_type);

    std::queue<std::vector<sr_robot_msgs::MotorSystemControls>, std::list<std::vector<sr_robot_msgs::MotorSystemControls> > > motor_system_control_flags_;
    ros::ServiceServer motor_system_control_server_;

    bool motor_system_controls_callback_( sr_robot_msgs::ChangeMotorSystemControls::Request& request,
                                          sr_robot_msgs::ChangeMotorSystemControls::Response& response );
  };//end class
}//end namespace

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

#endif