Line data    Source code

       1             : #include <vector>
       2             : #include <unordered_map>
       3             : 
       4             : #include <ros/ros.h>
       5             : 
       6             : #include <ros_end_effector/HAL/EEHal.h>
       7             : 
       8             : #ifdef _MATLOGGER2
       9             :     #include <matlogger2/matlogger2.h>
      10             :     #include <matlogger2/utils/mat_appender.h>
      11             : #endif
      12             : 
      13          41 : int main ( int argc, char **argv ) {
      14             :     
      15          41 :     ros::init ( argc, argv, "EEHalExecutor" );
      16          82 :     ros::NodeHandle nh("EEHalExecutor");
      17             :     
      18          82 :     std::string hal_lib;
      19          41 :     if ( ! nh.getParam ( "/rosee/hal_library_name", hal_lib ) ) {
      20           0 :         ROS_ERROR_STREAM( "Ros parameter 'hal_library_name' not found" );
      21           0 :         return -1;
      22             :     }
      23             : 
      24             : #ifdef _MATLOGGER2
      25          82 :     std::string matlogger_path;
      26          82 :     XBot::MatLogger2::Ptr logger; /* mt logger */
      27          82 :     XBot::MatAppender::Ptr appender; /* mt logger */
      28             : 
      29          41 :     bool logging = false;
      30             : 
      31          41 :     if ( nh.getParam ( "/rosee/matlogger_path", matlogger_path ) && matlogger_path.size() != 0) {
      32           0 :         ROS_INFO_STREAM( "Logging data with matlogger into " << matlogger_path  );
      33             : 
      34           0 :         logger = XBot::MatLogger2::MakeLogger(matlogger_path); // date-time automatically appended
      35           0 :         appender = XBot::MatAppender::MakeInstance();
      36           0 :         appender->add_logger(logger);
      37           0 :         appender->start_flush_thread();        
      38           0 :         logging = true;
      39             :     }
      40             : #endif
      41             : 
      42             :     std::unique_ptr<ROSEE::EEHal> eeHalPtr = ROSEE::Utils::loadObject<ROSEE::EEHal>
      43          82 :                                          (hal_lib, "create_object_"+hal_lib, &nh);
      44             : 
      45          41 :     if (eeHalPtr == nullptr) {
      46           0 :         ROS_ERROR_STREAM( "[EEHalExecutor ERROR] in loading the EEHal Object" );
      47           0 :         return -1;    
      48             :     }
      49             :     
      50          41 :     ROS_INFO_STREAM ( "[EEHalExecutor] Loaded "<<  hal_lib << " HAL"  );   
      51             :     
      52          41 :     if (eeHalPtr->isHandInfoPresent()) { 
      53           0 :         eeHalPtr->init_hand_info_response();
      54           0 :         eeHalPtr->setHandInfoCallback();
      55             :     }
      56             :     
      57             : 
      58             :       
      59             :     //TODO take rate from param
      60          82 :     ros::Rate r(100);
      61       44827 :     while(ros::ok()) {
      62             :         
      63             :         //TODO check order of these
      64             :         
      65             :         //receive info from robot, and fill _js_msg
      66       22393 :         eeHalPtr->sense();
      67             :         
      68             :         //make the robot move following the refs in _mr_msg
      69             :         //But be sure that someone has sent a motor command to the hal
      70       22393 :         if (eeHalPtr->checkCommandPub()) {
      71       20981 :             eeHalPtr->move();
      72       20981 :             ROS_WARN_STREAM_ONCE ("[EEHalExecutor] someone is publishing on '/ros_end_effector/motor_reference_pos', I will call the move()" );   
      73             :             
      74             :         } else {
      75        1412 :             ROS_WARN_STREAM_THROTTLE (60, "[EEHalExecutor] no-one is publishing on '/ros_end_effector/motor_reference_pos', I will not call the move()" );   
      76             :         }
      77             :         
      78             :         //send _js_msg to external (ie to ROSEE main node)
      79       22393 :         eeHalPtr->publish_joint_state();
      80             :         
      81             :         
      82             :         
      83       22393 :         if (eeHalPtr->_pressure_active) {
      84           0 :             eeHalPtr->publish_pressure();
      85             :         } 
      86             :         
      87             : #ifdef _MATLOGGER2        
      88       22393 :         if (logging) {
      89           0 :             logger->add("motor_pos_ref", eeHalPtr->getMotorReference());
      90           0 :             logger->add("motor_pos", eeHalPtr->getJointPosition());
      91           0 :             logger->add("motor_eff", eeHalPtr->getJointEffort());
      92           0 :             auto pressures =  eeHalPtr->getPressure();
      93           0 :             logger->add("pressure1", pressures.row(0));
      94           0 :             logger->add("pressure2", pressures.row(1));
      95           0 :             logger->add("pressure3", pressures.row(2));
      96           0 :             logger->add("pressure4", pressures.row(3));
      97             :         }
      98             : #endif
      99             : 
     100       22393 :         ros::spinOnce();
     101       22393 :         r.sleep();
     102             :         
     103             :     }
     104             :     
     105          41 :     return 0;
     106             :     
     107         123 : }