Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef TEST_MOVEIT_CONTROLLER_MANAGER_
00038 #define TEST_MOVEIT_CONTROLLER_MANAGER_
00039
00040 #include <moveit/controller_manager/controller_manager.h>
00041
00042 namespace test_moveit_controller_manager
00043 {
00044 class TestMoveItControllerHandle : public moveit_controller_manager::MoveItControllerHandle
00045 {
00046 public:
00047 TestMoveItControllerHandle(const std::string& name) : MoveItControllerHandle(name)
00048 {
00049 }
00050
00051 virtual bool sendTrajectory(const moveit_msgs::RobotTrajectory& trajectory)
00052 {
00053 return true;
00054 }
00055
00056 virtual bool cancelExecution()
00057 {
00058 return true;
00059 }
00060
00061 virtual bool waitForExecution(const ros::Duration& timeout = ros::Duration(0))
00062 {
00063 return false;
00064 }
00065
00066 virtual moveit_controller_manager::ExecutionStatus getLastExecutionStatus()
00067 {
00068 return moveit_controller_manager::ExecutionStatus::SUCCEEDED;
00069 }
00070 };
00071
00072 class TestMoveItControllerManager : public moveit_controller_manager::MoveItControllerManager
00073 {
00074 public:
00075 static const int ACTIVE = 1;
00076 static const int DEFAULT = 2;
00077
00078 TestMoveItControllerManager()
00079 {
00080 controllers_["right_arm"] = DEFAULT;
00081 controllers_["left_arm"] = ACTIVE + DEFAULT;
00082 controllers_["arms"] = 0;
00083 controllers_["base"] = DEFAULT;
00084 controllers_["head"] = 0;
00085 controllers_["left_arm_head"] = 0;
00086
00087 controller_joints_["right_arm"].push_back("rj1");
00088 controller_joints_["right_arm"].push_back("rj2");
00089
00090 controller_joints_["left_arm"].push_back("lj1");
00091 controller_joints_["left_arm"].push_back("lj2");
00092 controller_joints_["left_arm"].push_back("lj3");
00093
00094 controller_joints_["arms"].insert(controller_joints_["arms"].end(), controller_joints_["left_arm"].begin(),
00095 controller_joints_["left_arm"].end());
00096 controller_joints_["arms"].insert(controller_joints_["arms"].end(), controller_joints_["right_arm"].begin(),
00097 controller_joints_["right_arm"].end());
00098
00099 controller_joints_["base"].push_back("basej");
00100 controller_joints_["head"].push_back("headj");
00101
00102 controller_joints_["left_arm_head"].insert(controller_joints_["left_arm_head"].end(),
00103 controller_joints_["left_arm"].begin(),
00104 controller_joints_["left_arm"].end());
00105 controller_joints_["left_arm_head"].insert(controller_joints_["left_arm_head"].end(),
00106 controller_joints_["head"].begin(), controller_joints_["head"].end());
00107 }
00108
00109 virtual moveit_controller_manager::MoveItControllerHandlePtr getControllerHandle(const std::string& name)
00110 {
00111 return moveit_controller_manager::MoveItControllerHandlePtr(new TestMoveItControllerHandle(name));
00112 }
00113
00114 virtual void getControllersList(std::vector<std::string>& names)
00115 {
00116 names.clear();
00117 for (std::map<std::string, int>::const_iterator it = controllers_.begin(); it != controllers_.end(); ++it)
00118 names.push_back(it->first);
00119 }
00120
00121 virtual void getActiveControllers(std::vector<std::string>& names)
00122 {
00123 names.clear();
00124 for (std::map<std::string, int>::const_iterator it = controllers_.begin(); it != controllers_.end(); ++it)
00125 if (it->second & ACTIVE)
00126 names.push_back(it->first);
00127 }
00128
00129 virtual void getControllerJoints(const std::string& name, std::vector<std::string>& joints)
00130 {
00131 joints = controller_joints_[name];
00132 }
00133
00134 virtual moveit_controller_manager::MoveItControllerManager::ControllerState
00135 getControllerState(const std::string& name)
00136 {
00137 moveit_controller_manager::MoveItControllerManager::ControllerState state;
00138 state.active_ = controllers_[name] & ACTIVE;
00139 state.default_ = false;
00140 return state;
00141 }
00142
00143 virtual bool switchControllers(const std::vector<std::string>& activate, const std::vector<std::string>& deactivate)
00144 {
00145 for (std::size_t i = 0; i < deactivate.size(); ++i)
00146 {
00147 controllers_[deactivate[i]] &= ~ACTIVE;
00148 std::cout << "Deactivated controller " << deactivate[i] << std::endl;
00149 }
00150 for (std::size_t i = 0; i < activate.size(); ++i)
00151 {
00152 controllers_[activate[i]] |= ACTIVE;
00153 std::cout << "Activated controller " << activate[i] << std::endl;
00154 }
00155 return true;
00156 }
00157
00158 protected:
00159 std::map<std::string, int> controllers_;
00160 std::map<std::string, std::vector<std::string> > controller_joints_;
00161 };
00162 }
00163 #endif