TEST_F(TestController, start_stop_best_effort)
{
// check initial state
EXPECT_EQ(controllerState("controller1"), _running);
EXPECT_EQ(controllerState("controller2"), _running);
EXPECT_EQ(controllerState("controller3"), _stopped);
EXPECT_EQ(controllerState("controller4"), _stopped);
EXPECT_EQ(controllerState("controller5"), _stopped);
EXPECT_EQ(controllerState("controller6"), _unloaded);
EXPECT_EQ(controllerState("controller7"), _unloaded);
EXPECT_EQ(controllerState("controller8"), _unloaded);
EXPECT_EQ(controllerState("controller9"), _unloaded);
// starting already started controller
std::vector<std::string> start, stop;
start.push_back("controller1");
EXPECT_TRUE(switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::BEST_EFFORT));
EXPECT_EQ(controllerState("controller1"), _running);
// starting unloaded, started and stopped controller
start.push_back("controller3");
start.push_back("controller6");
EXPECT_TRUE(switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::BEST_EFFORT));
EXPECT_EQ(controllerState("controller1"), _running);
EXPECT_EQ(controllerState("controller3"), _running);
EXPECT_EQ(controllerState("controller6"), _unloaded);
SUCCEED();
}
bool DynamicMovementPrimitiveControllerClient::sendCommand(const dmp::ICRA2009DMPMsg& msg,
const std::string& controller_name,
bool wait_for_success)
{
ROS_ASSERT(initialized_);
dmp::ICRA2009DMPMsg dmp_msg = msg;
if ((msg.dmp.parameters.type == dynamic_movement_primitive::TypeMsg::DISCRETE_JOINT_SPACE)
|| (msg.dmp.parameters.type == dynamic_movement_primitive::TypeMsg::DISCRETE_CARTESIAN_SPACE)
|| (msg.dmp.parameters.type == dynamic_movement_primitive::TypeMsg::DISCRETE_CARTESIAN_AND_JOINT_SPACE))
{
if (!switchController(controller_name))
{
return false;
}
ControllerMapIterator mi = icra2009_controller_clients_.find(controller_name);
if(mi == icra2009_controller_clients_.end())
{
ROS_ERROR("There is no DMP controller with name >%s<. Controllers are:", controller_name.c_str());
ControllerMapIterator it;
for(it = icra2009_controller_clients_.begin(); it != icra2009_controller_clients_.end(); ++it)
{
ROS_ERROR(">%s<", it->first.c_str());
}
return false;
}
return mi->second->sendCommand(dmp_msg, wait_for_success);
}
else
{
ROS_ERROR("DMP message has invalid type >%i<. Cannot send it to the DMP controller.", msg.dmp.parameters.type);
return false;
}
return true;
}
bool ControllerManager::reloadControllerLibrariesSrv(
controller_manager_msgs::ReloadControllerLibraries::Request &req,
controller_manager_msgs::ReloadControllerLibraries::Response &resp)
{
// lock services
ROS_DEBUG("reload libraries service called");
boost::mutex::scoped_lock guard(services_lock_);
ROS_DEBUG("reload libraries service locked");
// only reload libraries if no controllers are running
std::vector<std::string> controllers;
getControllerNames(controllers);
if (!controllers.empty() && !req.force_kill){
ROS_ERROR("Controller manager: Cannot reload controller libraries because there are still %i controllers running", (int)controllers.size());
resp.ok = false;
return true;
}
// kill running controllers if requested
if (!controllers.empty()){
ROS_INFO("Controller manager: Killing all running controllers");
std::vector<std::string> empty;
if (!switchController(empty,controllers, controller_manager_msgs::SwitchController::Request::BEST_EFFORT)){
ROS_ERROR("Controller manager: Cannot reload controller libraries because failed to stop running controllers");
resp.ok = false;
return true;
}
for (unsigned int i=0; i<controllers.size(); i++){
if (!unloadController(controllers[i])){
ROS_ERROR("Controller manager: Cannot reload controller libraries because failed to unload controller %s",
controllers[i].c_str());
resp.ok = false;
return true;
}
}
getControllerNames(controllers);
}
assert(controllers.empty());
// Force a reload on all the PluginLoaders (internally, this recreates the plugin loaders)
for(std::list<LoaderPtr>::iterator it = controller_loaders_.begin(); it != controller_loaders_.end(); ++it)
{
(*it)->reload();
ROS_INFO("Controller manager: reloaded controller libraries for %s", (*it)->getName().c_str());
}
resp.ok = true;
ROS_DEBUG("reload libraries service finished");
return true;
}
void randomBombardment(unsigned int times)
{
while (!bombardment_started_)
ros::Duration(0.1).sleep();
for (unsigned int i=0; i<times; i++){
unsigned int random = rand();
random = random % 4;
switch (random){
case 0:{
loadController(randomController());
break;
}
case 1:{
unloadController(randomController());
break;
}
case 2:{
std::vector<std::string> start, stop;
unsigned int start_size = rand() %10;
unsigned int stop_size = rand() %10;
for (unsigned int i=0; i<start_size; i++)
start.push_back(randomController());
for (unsigned int i=0; i<stop_size; i++)
stop.push_back(randomController());
if (rand() %1 == 0)
switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT);
else
switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::BEST_EFFORT);
break;
}
case 3:{
controllerState(randomController());
break;
}
}
}
}
bool ControllerManager::switchControllerSrv(
controller_manager_msgs::SwitchController::Request &req,
controller_manager_msgs::SwitchController::Response &resp)
{
// lock services
ROS_DEBUG("switching service called");
boost::mutex::scoped_lock guard(services_lock_);
ROS_DEBUG("switching service locked");
resp.ok = switchController(req.start_controllers, req.stop_controllers, req.strictness);
ROS_DEBUG("switching service finished");
return true;
}
void View::handleAction(QString oldSensor, QString newSensor){
m_menu->setVisible(false);
if (oldSensor==newSensor) return;
QList<QAction*> tmpActions = m_menu->actions();
for (int i=0; i<tmpActions.length(); i++){
if (tmpActions.at(i)->text() == oldSensor){
tmpActions.at(i)->setEnabled(true);
continue;
}
if (tmpActions.at(i)->text() == newSensor){
tmpActions.at(i)->setEnabled(false);
}
}
m_currentSensor = newSensor;
switchController(m_currentSensor);
}
TEST_F(TestController, start_stop_strict)
{
// check initial state
EXPECT_EQ(controllerState("controller1"), _running);
EXPECT_EQ(controllerState("controller2"), _unloaded);
EXPECT_EQ(controllerState("controller3"), _running);
EXPECT_EQ(controllerState("controller4"), _running);
EXPECT_EQ(controllerState("controller5"), _unloaded);
EXPECT_EQ(controllerState("controller6"), _unloaded);
EXPECT_EQ(controllerState("controller7"), _unloaded);
EXPECT_EQ(controllerState("controller8"), _unloaded);
EXPECT_EQ(controllerState("controller9"), _unloaded);
// starting already started controller
std::vector<std::string> start, stop;
start.push_back("controller1");
EXPECT_TRUE(switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
EXPECT_EQ(controllerState("controller1"), _running);
// starting unloaded controller
start.push_back("controller2");
EXPECT_FALSE(switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
EXPECT_EQ(controllerState("controller1"), _running);
EXPECT_EQ(controllerState("controller2"), _unloaded);
// starting one already stated, 1 stopped
EXPECT_TRUE(loadController("controller2"));
EXPECT_TRUE(switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
EXPECT_EQ(controllerState("controller1"), _running);
EXPECT_EQ(controllerState("controller2"), _running);
// start and stop same controller
stop.push_back("controller2");
EXPECT_TRUE(switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
EXPECT_EQ(controllerState("controller1"), _running);
EXPECT_EQ(controllerState("controller2"), _running);
// stop unloaded controller
stop.push_back("controller5");
EXPECT_FALSE(switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
EXPECT_EQ(controllerState("controller1"), _running);
EXPECT_EQ(controllerState("controller2"), _running);
EXPECT_EQ(controllerState("controller5"), _unloaded);
// stop unloaded and running controller
stop.push_back("controller4");
EXPECT_FALSE(switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
EXPECT_EQ(controllerState("controller1"), _running);
EXPECT_EQ(controllerState("controller2"), _running);
EXPECT_EQ(controllerState("controller4"), _running);
EXPECT_EQ(controllerState("controller5"), _unloaded);
// stop running and stopped controller
EXPECT_TRUE(loadController("controller5"));
EXPECT_TRUE(switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
EXPECT_EQ(controllerState("controller1"), _running);
EXPECT_EQ(controllerState("controller2"), _running);
EXPECT_EQ(controllerState("controller4"), _stopped);
EXPECT_EQ(controllerState("controller5"), _stopped);
// stop 2 stopped controllers, and 1 running controller
stop.push_back("controller3");
EXPECT_TRUE(switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
EXPECT_EQ(controllerState("controller1"), _running);
EXPECT_EQ(controllerState("controller2"), _running);
EXPECT_EQ(controllerState("controller3"), _stopped);
EXPECT_EQ(controllerState("controller4"), _stopped);
EXPECT_EQ(controllerState("controller5"), _stopped);
SUCCEED();
}
