37 #ifndef TEST_MOVEIT_CONTROLLER_MANAGER_ 38 #define TEST_MOVEIT_CONTROLLER_MANAGER_ 75 static const int ACTIVE = 1;
76 static const int DEFAULT = 2;
80 controllers_[
"right_arm"] = DEFAULT;
81 controllers_[
"left_arm"] = ACTIVE + DEFAULT;
82 controllers_[
"arms"] = 0;
83 controllers_[
"base"] = DEFAULT;
84 controllers_[
"head"] = 0;
85 controllers_[
"left_arm_head"] = 0;
87 controller_joints_[
"right_arm"].push_back(
"rj1");
88 controller_joints_[
"right_arm"].push_back(
"rj2");
90 controller_joints_[
"left_arm"].push_back(
"lj1");
91 controller_joints_[
"left_arm"].push_back(
"lj2");
92 controller_joints_[
"left_arm"].push_back(
"lj3");
94 controller_joints_[
"arms"].insert(controller_joints_[
"arms"].end(), controller_joints_[
"left_arm"].begin(),
95 controller_joints_[
"left_arm"].end());
96 controller_joints_[
"arms"].insert(controller_joints_[
"arms"].end(), controller_joints_[
"right_arm"].begin(),
97 controller_joints_[
"right_arm"].end());
99 controller_joints_[
"base"].push_back(
"basej");
100 controller_joints_[
"head"].push_back(
"headj");
102 controller_joints_[
"left_arm_head"].insert(controller_joints_[
"left_arm_head"].end(),
103 controller_joints_[
"left_arm"].begin(),
104 controller_joints_[
"left_arm"].end());
105 controller_joints_[
"left_arm_head"].insert(controller_joints_[
"left_arm_head"].end(),
106 controller_joints_[
"head"].begin(), controller_joints_[
"head"].end());
117 for (std::map<std::string, int>::const_iterator it = controllers_.begin(); it != controllers_.end(); ++it)
118 names.push_back(it->first);
124 for (std::map<std::string, int>::const_iterator it = controllers_.begin(); it != controllers_.end(); ++it)
125 if (it->second & ACTIVE)
126 names.push_back(it->first);
131 joints = controller_joints_[name];
138 state.
active_ = controllers_[name] & ACTIVE;
143 virtual bool switchControllers(
const std::vector<std::string>& activate,
const std::vector<std::string>& deactivate)
145 for (std::size_t i = 0; i < deactivate.size(); ++i)
147 controllers_[deactivate[i]] &= ~ACTIVE;
148 std::cout <<
"Deactivated controller " << deactivate[i] << std::endl;
150 for (std::size_t i = 0; i < activate.size(); ++i)
152 controllers_[activate[i]] |= ACTIVE;
153 std::cout <<
"Activated controller " << activate[i] << std::endl;
virtual bool switchControllers(const std::vector< std::string > &activate, const std::vector< std::string > &deactivate)
virtual void getControllersList(std::vector< std::string > &names)
std::map< std::string, std::vector< std::string > > controller_joints_
virtual moveit_controller_manager::ExecutionStatus getLastExecutionStatus()
virtual void getControllerJoints(const std::string &name, std::vector< std::string > &joints)
TestMoveItControllerHandle(const std::string &name)
virtual void getActiveControllers(std::vector< std::string > &names)
virtual bool sendTrajectory(const moveit_msgs::RobotTrajectory &trajectory)
std::map< std::string, int > controllers_
TestMoveItControllerManager()
virtual moveit_controller_manager::MoveItControllerHandlePtr getControllerHandle(const std::string &name)
virtual bool cancelExecution()
MoveItControllerHandle(const std::string &name)
virtual moveit_controller_manager::MoveItControllerManager::ControllerState getControllerState(const std::string &name)
virtual bool waitForExecution(const ros::Duration &timeout=ros::Duration(0))