00001 #ifndef GRIPPER_ENVIRONMENT_H 00002 #define GRIPPER_ENVIRONMENT_H 00003 00004 #include <kdl/tree.hpp> 00005 #include <kdl/jntarray.hpp> 00006 #include <kdl/frames.hpp> 00007 #include <diagnostic_msgs/DiagnosticArray.h> 00008 #include <cmath> 00009 00010 #include "nasa_common_logging/Logger.h" 00011 #include "r2_msgs/JointControlDataArray.h" 00012 #include "r2_msgs/StringArray.h" 00013 #include "r2_msgs/GripperEnvironment.h" 00014 #include "robodyn_utilities/RosMsgConverter.h" 00015 00016 enum GripperStatusCheckerStates 00017 { 00018 IDLE = 0, 00019 PILOT_REQUEST, 00020 CAPTAIN_REQUEST, 00021 GRANT, 00022 DRIVING 00023 }; 00024 00025 class GripperStatusChecker 00026 { 00027 public: 00028 GripperStatusChecker(); 00029 ~GripperStatusChecker(); 00030 00032 void initializeMap(const std::vector<std::string>& grippers); 00033 void setTimingParameters(const double& rTimeOut, const double& gTimeOut); 00034 00036 void populateJointControlPilotInfo(const r2_msgs::JointControlDataArray& jointControlMsg); 00037 void populateJointControlCaptainInfo(const r2_msgs::JointControlDataArray& jointControlMsg); 00038 void populatePilotRequestInfo(const r2_msgs::StringArray& pilotRequest); 00039 void populateCaptainRequestInfo(const r2_msgs::StringArray& captainRequest); 00040 00042 void createEngineerCheckMsg(r2_msgs::StringArray& engineerCheck); 00043 void createSweStopMessage(diagnostic_msgs::DiagnosticArray& sweStopMsg); 00044 00046 void update(); 00047 00048 struct GripperStatusCheckInfo 00049 { 00050 int pilotMode; 00051 int captainMode; 00052 bool pilotRequest; 00053 bool captainRequest; 00054 bool engineerCheck; 00055 GripperStatusCheckerStates state; 00056 }; 00057 00059 bool getMapEntry(const std::string& name, GripperStatusCheckInfo& info); 00060 int getState(const std::string& name); 00061 00062 private: 00063 void clearRequestsAndChecks(const std::string& gripper); 00064 00065 std::map<std::string, GripperStatusCheckInfo> gripperInfoMap; 00066 ros::Time startTime; 00067 ros::Duration requestTimeOut, grantTimeOut, timer; 00068 diagnostic_msgs::DiagnosticArray sweStopCollection; 00069 00070 }; 00071 00072 #endif