ROSEndEffector
ROS End-Effector package: provides a ROS-based set of standard interfaces to command robotics end-effectors in an agnostic fashion.
Functions
EEHalExecutor.cpp File Reference
#include <vector>
#include <unordered_map>
#include <ros/ros.h>
#include <end_effector/HAL/EEHal.h>
+ Include dependency graph for EEHalExecutor.cpp:

Go to the source code of this file.

Functions

int main (int argc, char **argv)
 

Function Documentation

int main ( int  argc,
char **  argv 
)

Definition at line 13 of file EEHalExecutor.cpp.

13  {
14 
15  ros::init ( argc, argv, "EEHalExecutor" );
16  ros::NodeHandle nh("EEHalExecutor");
17 
18  std::string hal_lib;
19  if ( ! nh.getParam ( "/rosee/hal_library_name", hal_lib ) ) {
20  ROS_ERROR_STREAM( "Ros parameter 'hal_library_name' not found" );
21  return -1;
22  }
23 
24 #ifdef _MATLOGGER2
25  std::string matlogger_path;
26  XBot::MatLogger2::Ptr logger; /* mt logger */
27  XBot::MatAppender::Ptr appender; /* mt logger */
28 
29  bool logging = false;
30 
31  if ( nh.getParam ( "/rosee/matlogger_path", matlogger_path ) && matlogger_path.size() != 0) {
32  ROS_INFO_STREAM( "Logging data with matlogger into " << matlogger_path );
33 
34  logger = XBot::MatLogger2::MakeLogger(matlogger_path); // date-time automatically appended
35  appender = XBot::MatAppender::MakeInstance();
36  appender->add_logger(logger);
37  appender->start_flush_thread();
38  logging = true;
39  }
40 #endif
41 
42  std::unique_ptr<ROSEE::EEHal> eeHalPtr = ROSEE::Utils::loadObject<ROSEE::EEHal>
43  (hal_lib, "create_object_"+hal_lib, &nh);
44 
45  if (eeHalPtr == nullptr) {
46  ROS_ERROR_STREAM( "[EEHalExecutor ERROR] in loading the EEHal Object" );
47  return -1;
48  }
49 
50  ROS_INFO_STREAM ( "[EEHalExecutor] Loaded "<< hal_lib << " HAL" );
51 
52  if (eeHalPtr->isHandInfoPresent()) {
53  eeHalPtr->init_hand_info_response();
54  eeHalPtr->setHandInfoCallback();
55  }
56 
57 
58 
59  //TODO take rate from param
60  ros::Rate r(100);
61  while(ros::ok()) {
62 
63  //TODO check order of these
64 
65  //receive info from robot, and fill _js_msg
66  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  if (eeHalPtr->checkCommandPub()) {
71  eeHalPtr->move();
72  ROS_WARN_STREAM_ONCE ("[EEHalExecutor] someone is publishing on '/ros_end_effector/motor_reference_pos', I will call the move()" );
73 
74  } else {
75  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  eeHalPtr->publish_joint_state();
80 
81 
82 
83  if (eeHalPtr->_pressure_active) {
84  eeHalPtr->publish_pressure();
85  }
86 
87 #ifdef _MATLOGGER2
88  if (logging) {
89  logger->add("motor_pos_ref", eeHalPtr->getMotorReference());
90  logger->add("motor_pos", eeHalPtr->getJointPosition());
91  logger->add("motor_eff", eeHalPtr->getJointEffort());
92  auto pressures = eeHalPtr->getPressure();
93  logger->add("pressure1", pressures.row(0));
94  logger->add("pressure2", pressures.row(1));
95  logger->add("pressure3", pressures.row(2));
96  logger->add("pressure4", pressures.row(3));
97  }
98 #endif
99 
100  ros::spinOnce();
101  r.sleep();
102 
103  }
104 
105  return 0;
106 
107 }
bool publish_pressure()
Definition: EEHal.cpp:265
Eigen::VectorXd getJointEffort() const
Definition: EEHal.cpp:298
virtual bool move()=0
virtual bool checkCommandPub()
Definition: EEHal.cpp:47
bool _pressure_active
Definition: EEHal.h:112
virtual bool sense()=0
virtual bool publish_joint_state()
Definition: EEHal.cpp:60
Eigen::VectorXd getMotorReference() const
Definition: EEHal.cpp:274
virtual bool init_hand_info_response()
init the service message with info parsed from yaml file, ie info that will not change according to h...
Definition: EEHal.cpp:217
virtual bool isHandInfoPresent()
Definition: EEHal.cpp:52
Eigen::MatrixXd getPressure() const
Definition: EEHal.cpp:310
virtual bool setHandInfoCallback()
Here service is advertise and callback set: if derived class wants to use different callback...
Definition: EEHal.cpp:235
Eigen::VectorXd getJointPosition() const
Definition: EEHal.cpp:286