Go to the documentation of this file.00001 #ifndef MODE_ARBITER_H
00002 #define MODE_ARBITER_H
00003
00004 #include <list>
00005
00006 #include <kdl/tree.hpp>
00007 #include <kdl/jntarray.hpp>
00008 #include <kdl/frames.hpp>
00009
00010 #include "r2_msgs/Modes.h"
00011 #include "r2_msgs/LabeledGains.h"
00012 #include "r2_msgs/LabeledJointState.h"
00013 #include "r2_msgs/LabeledJointTrajectory.h"
00014 #include "r2_msgs/LabeledPoseTrajectory.h"
00015 #include "r2_msgs/LabeledControllerPoseSettings.h"
00016 #include "r2_msgs/LabeledControllerJointSettings.h"
00017 #include "r2_msgs/LabeledGripperPositionCommand.h"
00018 #include "r2_msgs/LabeledJointControlDataArray.h"
00019 #include "r2_msgs/LabeledTrajectoryMonitorFactors.h"
00020 #include "r2_msgs/Gains.h"
00021 #include "sensor_msgs/JointState.h"
00022 #include "trajectory_msgs/JointTrajectory.h"
00023 #include "r2_msgs/PoseTrajectory.h"
00024 #include "r2_msgs/ControllerPoseSettings.h"
00025 #include "r2_msgs/ControllerJointSettings.h"
00026 #include "r2_msgs/GripperPositionCommand.h"
00027 #include "r2_msgs/JointControlDataArray.h"
00028 #include "r2_msgs/TrajectoryMonitorFactors.h"
00029 #include "robodyn_controllers/KdlTreeParser.h"
00030
00031
00032 class ModeArbiter : public KdlTreeParser
00033 {
00034 public:
00035 ModeArbiter();
00036 ~ModeArbiter();
00037
00038 void InitializeMaps(std::vector<std::string> mechanisms, std::vector<std::string> exJoints);
00039 void ClearMaps();
00040
00041 void AssignMaster(const std::string& name);
00042
00043 int AssignArbitrationProperties(const r2_msgs::Modes& modeMsg);
00044
00045 void CreateTriggerMessages(std::vector<r2_msgs::Modes>& triggerMsgs);
00046
00047 bool GetMapElement(const std::string& map, const std::string& element, std::string& value);
00048
00049 void GetMaster(std::string& name);
00050
00051 bool isValidHijack(const std::string& name);
00052
00053 bool CreateCommandMessage(const r2_msgs::LabeledGains& gainsIn, r2_msgs::Gains& gainsOut);
00054 bool CreateCommandMessage(const r2_msgs::LabeledJointState& jsIn, sensor_msgs::JointState& jsOut);
00055 bool CreateCommandMessage(const r2_msgs::LabeledPoseTrajectory& ptIn, r2_msgs::PoseTrajectory& ptOut);
00056 bool CreateCommandMessage(const r2_msgs::LabeledControllerPoseSettings& psIn, r2_msgs::ControllerPoseSettings& psOut);
00057 bool CreateCommandMessage(const r2_msgs::LabeledJointTrajectory& jtIn, trajectory_msgs::JointTrajectory& jtOut);
00058 bool CreateCommandMessage(const r2_msgs::LabeledControllerJointSettings& jsIn, r2_msgs::ControllerJointSettings& jsOut);
00059 bool CreateCommandMessage(const r2_msgs::LabeledGripperPositionCommand& gpIn, r2_msgs::GripperPositionCommand& gpOut);
00060 bool CreateCommandMessage(const r2_msgs::LabeledJointControlDataArray& jcIn, r2_msgs::JointControlDataArray& jcOut);
00061 bool CreateCommandMessage(const r2_msgs::LabeledTrajectoryMonitorFactors& tmfIn, r2_msgs::TrajectoryMonitorFactors& tmfOut);
00062
00063 private:
00064 int AssignSupervisors(const r2_msgs::Modes& modeMsg);
00065 int AssignIncumbents(const r2_msgs::Modes& modeMsg);
00066 int AssignModes(const r2_msgs::Modes& modeMsg);
00067
00068 bool isValidCommand(const std::string& originator, const std::vector<std::string>& jointNames);
00069 bool isValidCommand(const r2_msgs::Modes& modeMsg);
00070 bool isValidCommand(const r2_msgs::LabeledGains& gains);
00071 bool isValidCommand(const r2_msgs::LabeledJointState& js);
00072 bool isValidCommand(const r2_msgs::LabeledPoseTrajectory& pt);
00073 bool isValidCommand(const r2_msgs::LabeledControllerPoseSettings& ps);
00074 bool isValidCommand(const r2_msgs::LabeledJointTrajectory& jt);
00075 bool isValidCommand(const r2_msgs::LabeledControllerJointSettings& js);
00076 bool isValidCommand(const r2_msgs::LabeledGripperPositionCommand& gp);
00077 bool isValidCommand(const r2_msgs::LabeledJointControlDataArray& jc);
00078 bool isValidCommand(const r2_msgs::LabeledTrajectoryMonitorFactors& tmf);
00079
00080 std::map<std::string, std::string> supervisorMap, incumbentMap, modeMap;
00081 std::string master;
00082 std::vector<std::string> grippers;
00083
00084 };
00085
00086 #endif