Go to the documentation of this file.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/GripperPositionState.h"
00012 #include "r2_msgs/StringArray.h"
00013 #include "r2_msgs/GripperEnvironment.h"
00014 #include "actionlib_msgs/GoalStatusArray.h"
00015 #include "robodyn_utilities/BaseGripperConverter.h"
00016 #include "robodyn_utilities/RosMsgConverter.h"
00017
00018
00019 class GripperEnvironment : public BaseGripperConverter
00020 {
00021 public:
00022 GripperEnvironment();
00023 ~GripperEnvironment();
00024
00026 void initializeMap(const std::vector<std::string>& grippers);
00027 void setComponentNames(const std::string& handrail, const std::string& seatTrack);
00028
00030 void populateGripperStateInfo(const r2_msgs::GripperPositionState& eefState);
00031 void populateBaseFrameInfo(const r2_msgs::StringArray& baseFrames);
00032 void populateGoalStatus(const actionlib_msgs::GoalStatusArray& goalStats);
00033 void populatePoseStateInfo(const r2_msgs::PoseState& poseCmd);
00034 void setUserCommand(const std::string gripperName, const int& cmd);
00035
00037 void createGripperEnvironmentMsg(r2_msgs::GripperEnvironment& gripEnv);
00038
00040 void update();
00041
00042 struct GripperEnvInfo
00043 {
00044 int environment;
00045 int userCommand;
00046 int gripperLocked;
00047 int highForceSubState;
00048 KDL::Frame frame;
00049 KDL::Frame prevFrame;
00050 };
00051
00053 bool getMapEntry(const std::string& name, GripperEnvInfo& info);
00054 std::string getBaseFrame();
00055 int getGoalStatusArraySize();
00056
00057 private:
00058 std::string handrailComponent, seatTrackComponent, baseFrame;
00059 std::map<std::string, GripperEnvInfo>::iterator envIt;
00060 std::vector<std::string>::iterator nameIt;
00061 std::vector<std::string> baseFrameList;
00062 std::map<std::string, GripperEnvInfo> gripperEnvMap;
00063 actionlib_msgs::GoalStatusArray goalStatuses;
00064 KDL::Frame tempFrame;
00065 std::string name;
00066 KDL::Twist frameDiff;
00067 int type;
00068
00069 void transToHighForce(const int& attachType, std::map<std::string, GripperEnvInfo>::iterator& gripIt);
00070 void transToFreespace(std::map<std::string, GripperEnvInfo>::iterator& gripIt);
00071 bool checkForHighForce(std::map<std::string, GripperEnvInfo>::iterator& gripIt, int& attachType);
00072 bool checkForFreespaceUserCmd(std::map<std::string, GripperEnvInfo>::iterator& gripIt);
00073 bool checkForHighForceUserCmd(std::map<std::string, GripperEnvInfo>::iterator& gripIt, int& attachType);
00074 bool checkGoalStatus(const std::string& gripper, int& attachType);
00075 bool checkForMovement(const std::map<std::string, GripperEnvInfo>::iterator& gripIt);
00076 bool isGripperLocked(const std::map<std::string, GripperEnvInfo>::iterator& gripIt);
00077 bool isBaseFrame(const std::map<std::string, GripperEnvInfo>::iterator& gripIt);
00078
00079 int findAttachType(const std::string& input);
00080 };
00081
00082 #endif