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
00045 class TestMoveItControllerHandle : public moveit_controller_manager::MoveItControllerHandle
00046 {
00047 public:
00048
00049 TestMoveItControllerHandle(const std::string &name) : MoveItControllerHandle(name)
00050 {
00051 }
00052
00053 virtual bool sendTrajectory(const moveit_msgs::RobotTrajectory &trajectory)
00054 {
00055 return true;
00056 }
00057
00058 virtual bool cancelExecution()
00059 {
00060 return true;
00061 }
00062
00063 virtual bool waitForExecution(const ros::Duration &timeout = ros::Duration(0))
00064 {
00065 return false;
00066 }
00067
00068 virtual moveit_controller_manager::ExecutionStatus getLastExecutionStatus()
00069 {
00070 return moveit_controller_manager::ExecutionStatus::SUCCEEDED;
00071 }
00072
00073 };
00074
00075 class TestMoveItControllerManager : public moveit_controller_manager::MoveItControllerManager
00076 {
00077 public:
00078 static const int ACTIVE = 1;
00079 static const int DEFAULT = 2;
00080
00081 TestMoveItControllerManager()
00082 {
00083 controllers_["right_arm"] = DEFAULT;
00084 controllers_["left_arm"] = ACTIVE + DEFAULT;
00085 controllers_["arms"] = 0;
00086 controllers_["base"] = DEFAULT;
00087 controllers_["head"] = 0;
00088 controllers_["left_arm_head"] = 0;
00089
00090 controller_joints_["right_arm"].push_back("rj1");
00091 controller_joints_["right_arm"].push_back("rj2");
00092
00093 controller_joints_["left_arm"].push_back("lj1");
00094 controller_joints_["left_arm"].push_back("lj2");
00095 controller_joints_["left_arm"].push_back("lj3");
00096
00097 controller_joints_["arms"].insert(controller_joints_["arms"].end(), controller_joints_["left_arm"].begin(), controller_joints_["left_arm"].end());
00098 controller_joints_["arms"].insert(controller_joints_["arms"].end(), controller_joints_["right_arm"].begin(), controller_joints_["right_arm"].end());
00099
00100 controller_joints_["base"].push_back("basej");
00101 controller_joints_["head"].push_back("headj");
00102
00103 controller_joints_["left_arm_head"].insert(controller_joints_["left_arm_head"].end(), controller_joints_["left_arm"].begin(), controller_joints_["left_arm"].end());
00104 controller_joints_["left_arm_head"].insert(controller_joints_["left_arm_head"].end(), controller_joints_["head"].begin(), controller_joints_["head"].end());
00105 }
00106
00107 virtual moveit_controller_manager::MoveItControllerHandlePtr getControllerHandle(const std::string &name)
00108 {
00109 return moveit_controller_manager::MoveItControllerHandlePtr(new TestMoveItControllerHandle(name));
00110 }
00111
00112 virtual void getControllersList(std::vector<std::string> &names)
00113 {
00114 names.clear();
00115 for (std::map<std::string, int>::const_iterator it = controllers_.begin() ; it != controllers_.end() ; ++it)
00116 names.push_back(it->first);
00117 }
00118
00119 virtual void getActiveControllers(std::vector<std::string> &names)
00120 {
00121 names.clear();
00122 for (std::map<std::string, int>::const_iterator it = controllers_.begin() ; it != controllers_.end() ; ++it)
00123 if (it->second & ACTIVE)
00124 names.push_back(it->first);
00125 }
00126
00127 virtual void getControllerJoints(const std::string &name, std::vector<std::string> &joints)
00128 {
00129 joints = controller_joints_[name];
00130 }
00131
00132 virtual moveit_controller_manager::MoveItControllerManager::ControllerState getControllerState(const std::string &name)
00133 {
00134 moveit_controller_manager::MoveItControllerManager::ControllerState state;
00135 state.active_ = controllers_[name] & ACTIVE;
00136 state.default_ = false;
00137 return state;
00138 }
00139
00140
00141 virtual bool switchControllers(const std::vector<std::string> &activate, const std::vector<std::string> &deactivate)
00142 {
00143 for (std::size_t i = 0 ; i < deactivate.size() ; ++i)
00144 {
00145 controllers_[deactivate[i]] &= ~ACTIVE;
00146 std::cout << "Deactivated controller " << deactivate[i] << std::endl;
00147 }
00148 for (std::size_t i = 0 ; i < activate.size() ; ++i)
00149 {
00150 controllers_[activate[i]] |= ACTIVE;
00151 std::cout << "Activated controller " << activate[i] << std::endl;
00152 }
00153 return true;
00154 }
00155
00156 protected:
00157
00158 std::map<std::string, int> controllers_;
00159 std::map<std::string, std::vector<std::string> > controller_joints_;
00160
00161 };
00162
00163
00164 }
00165 #endif