Skip to content

Commit

Permalink
Merge pull request #342 from TAMS-Group/quotes_for_controller_names
Browse files Browse the repository at this point in the history 
Added quotes for controller name and controller type in warnings and errors
  • Loading branch information
@graiola
graiola authored Aug 16, 2018
2 parents 17c4d0f + c4f50e9 commit 7202777
Show file tree
Hide file tree
Showing 3 changed files with 52 additions and 44 deletions.
38 changes: 19 additions & 19 deletions controller_manager/src/controller_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -207,7 +207,7 @@ bool ControllerManager::loadController(const std::string& name)
}
catch (const std::runtime_error &ex)
{
ROS_ERROR("Could not load class %s: %s", type.c_str(), ex.what());
ROS_ERROR("Could not load class '%s': %s", type.c_str(), ex.what());
}
}
else
Expand All @@ -234,11 +234,11 @@ bool ControllerManager::loadController(const std::string& name)
initialized = c->initRequest(robot_hw_, root_nh_, c_nh, claimed_resources);
}
catch(std::exception &e){
ROS_ERROR("Exception thrown while initializing controller %s.\n%s", name.c_str(), e.what());
ROS_ERROR("Exception thrown while initializing controller '%s'.\n%s", name.c_str(), e.what());
initialized = false;
}
catch(...){
ROS_ERROR("Exception thrown while initializing controller %s", name.c_str());
ROS_ERROR("Exception thrown while initializing controller '%s'", name.c_str());
initialized = false;
}
if (!initialized)
Expand Down Expand Up @@ -299,7 +299,7 @@ bool ControllerManager::unloadController(const std::string &name)
if (from[i].info.name == name){
if (from[i].c->isRunning()){
to.clear();
ROS_ERROR("Could not unload controller with name %s because it is still running",
ROS_ERROR("Could not unload controller with name '%s' because it is still running",
name.c_str());
return false;
}
Expand All @@ -313,7 +313,7 @@ bool ControllerManager::unloadController(const std::string &name)
if (!removed)
{
to.clear();
ROS_ERROR("Could not unload controller with name %s because no controller with this name exists",
ROS_ERROR("Could not unload controller with name '%s' because no controller with this name exists",
name.c_str());
return false;
}
Expand Down Expand Up @@ -353,9 +353,9 @@ bool ControllerManager::switchController(const std::vector<std::string>& start_c

ROS_DEBUG("switching controllers:");
for (unsigned int i=0; i<start_controllers.size(); i++)
ROS_DEBUG(" - starting controller %s", start_controllers[i].c_str());
ROS_DEBUG(" - starting controller '%s'", start_controllers[i].c_str());
for (unsigned int i=0; i<stop_controllers.size(); i++)
ROS_DEBUG(" - stopping controller %s", stop_controllers[i].c_str());
ROS_DEBUG(" - stopping controller '%s'", stop_controllers[i].c_str());

// lock controllers
boost::recursive_mutex::scoped_lock guard(controllers_lock_);
Expand All @@ -367,18 +367,18 @@ bool ControllerManager::switchController(const std::vector<std::string>& start_c
ct = getControllerByName(stop_controllers[i]);
if (ct == NULL){
if (strictness == controller_manager_msgs::SwitchController::Request::STRICT){
ROS_ERROR("Could not stop controller with name %s because no controller with this name exists",
ROS_ERROR("Could not stop controller with name '%s' because no controller with this name exists",
stop_controllers[i].c_str());
stop_request_.clear();
return false;
}
else{
ROS_DEBUG("Could not stop controller with name %s because no controller with this name exists",
ROS_DEBUG("Could not stop controller with name '%s' because no controller with this name exists",
stop_controllers[i].c_str());
}
}
else{
ROS_DEBUG("Found controller %s that needs to be stopped in list of controllers",
ROS_DEBUG("Found controller '%s' that needs to be stopped in list of controllers",
stop_controllers[i].c_str());
stop_request_.push_back(ct);
}
Expand All @@ -391,19 +391,19 @@ bool ControllerManager::switchController(const std::vector<std::string>& start_c
ct = getControllerByName(start_controllers[i]);
if (ct == NULL){
if (strictness == controller_manager_msgs::SwitchController::Request::STRICT){
ROS_ERROR("Could not start controller with name %s because no controller with this name exists",
ROS_ERROR("Could not start controller with name '%s' because no controller with this name exists",
start_controllers[i].c_str());
stop_request_.clear();
start_request_.clear();
return false;
}
else{
ROS_DEBUG("Could not start controller with name %s because no controller with this name exists",
ROS_DEBUG("Could not start controller with name '%s' because no controller with this name exists",
start_controllers[i].c_str());
}
}
else{
ROS_DEBUG("Found controller %s that needs to be started in list of controllers",
ROS_DEBUG("Found controller '%s' that needs to be started in list of controllers",
start_controllers[i].c_str());
start_request_.push_back(ct);
}
Expand Down Expand Up @@ -548,7 +548,7 @@ bool ControllerManager::reloadControllerLibrariesSrv(
}
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",
ROS_ERROR("Controller manager: Cannot reload controller libraries because failed to unload controller '%s'",
controllers[i].c_str());
resp.ok = false;
return true;
Expand All @@ -562,7 +562,7 @@ bool ControllerManager::reloadControllerLibrariesSrv(
for(std::list<ControllerLoaderInterfaceSharedPtr>::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());
ROS_INFO("Controller manager: reloaded controller libraries for '%s'", (*it)->getName().c_str());
}

resp.ok = true;
Expand Down Expand Up @@ -650,13 +650,13 @@ bool ControllerManager::loadControllerSrv(
controller_manager_msgs::LoadController::Response &resp)
{
// lock services
ROS_DEBUG("loading service called for controller %s ",req.name.c_str());
ROS_DEBUG("loading service called for controller '%s' ",req.name.c_str());
boost::mutex::scoped_lock guard(services_lock_);
ROS_DEBUG("loading service locked");

resp.ok = loadController(req.name);

ROS_DEBUG("loading service finished for controller %s ",req.name.c_str());
ROS_DEBUG("loading service finished for controller '%s' ",req.name.c_str());
return true;
}

Expand All @@ -666,13 +666,13 @@ bool ControllerManager::unloadControllerSrv(
controller_manager_msgs::UnloadController::Response &resp)
{
// lock services
ROS_DEBUG("unloading service called for controller %s ",req.name.c_str());
ROS_DEBUG("unloading service called for controller '%s' ",req.name.c_str());
boost::mutex::scoped_lock guard(services_lock_);
ROS_DEBUG("unloading service locked");

resp.ok = unloadController(req.name);

ROS_DEBUG("unloading service finished for controller %s ",req.name.c_str());
ROS_DEBUG("unloading service finished for controller '%s' ",req.name.c_str());
return true;
}

Expand Down
36 changes: 22 additions & 14 deletions controller_manager/src/controller_manager/controller_manager_interface.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,12 +2,14 @@
import rospy
from controller_manager_msgs.srv import *


def list_controller_types():
rospy.wait_for_service('controller_manager/list_controller_types')
s = rospy.ServiceProxy('controller_manager/list_controller_types', ListControllerTypes)
resp = s.call(ListControllerTypesRequest())
for t in resp.types:
print t
print(t)


def reload_libraries(force_kill, restore = False):
rospy.wait_for_service('controller_manager/reload_controller_libraries')
Expand All @@ -17,16 +19,16 @@ def reload_libraries(force_kill, restore = False):
load_srv = rospy.ServiceProxy('controller_manager/load_controller', LoadController)
switch_srv = rospy.ServiceProxy('controller_manager/switch_controller', SwitchController)

print "Restore:", restore
print("Restore: " + str(restore))
if restore:
originally = list_srv.call(ListControllersRequest())

resp = s.call(ReloadControllerLibrariesRequest(force_kill))
if resp.ok:
print "Successfully reloaded libraries"
print("Successfully reloaded libraries")
result = True
else:
print "Failed to reload libraries. Do you still have controllers loaded?"
print("Failed to reload libraries. Do you still have controllers loaded?")
result = False

if restore:
Expand All @@ -36,7 +38,7 @@ def reload_libraries(force_kill, restore = False):
switch_srv(start_controllers=to_start,
stop_controllers=[],
strictness=SwitchControllerRequest.BEST_EFFORT)
print "Controllers restored to original state"
print("Controllers restored to original state")
return result


Expand All @@ -45,58 +47,64 @@ def list_controllers():
s = rospy.ServiceProxy('controller_manager/list_controllers', ListControllers)
resp = s.call(ListControllersRequest())
if len(resp.controller) == 0:
print "No controllers are loaded in mechanism control"
print("No controllers are loaded in mechanism control")
else:
for c in resp.controller:
hwi = list(set(r.hardware_interface for r in c.claimed_resources))
print '%s - %s ( %s )'%(c.name, '+'.join(hwi), c.state)
print("'%s' - '%s' ( %s )" % (c.name, "+".join(hwi), c.state))


def load_controller(name):
rospy.wait_for_service('controller_manager/load_controller')
s = rospy.ServiceProxy('controller_manager/load_controller', LoadController)
resp = s.call(LoadControllerRequest(name))
if resp.ok:
print "Loaded", name
print("Loaded \'" + name + "\'")
return True
else:
print "Error when loading", name
print("Error when loading \'" + name + "\'")
return False


def unload_controller(name):
rospy.wait_for_service('controller_manager/unload_controller')
s = rospy.ServiceProxy('controller_manager/unload_controller', UnloadController)
resp = s.call(UnloadControllerRequest(name))
if resp.ok == 1:
print "Unloaded %s successfully" % name
print("Unloaded \'" + name + "\' successfully")
return True
else:
print "Error when unloading", name
print("Error when unloading \'" + name + "\'")
return False


def start_controller(name):
return start_stop_controllers(start_controllers=[name])


def start_controllers(names):
return start_stop_controllers(start_controllers=names)


def stop_controller(name):
return start_stop_controllers(stop_controllers=[name])


def stop_controllers(names):
return start_stop_controllers(stop_controllers=names)


def start_stop_controllers(start_controllers=[], stop_controllers=[]):
rospy.wait_for_service('controller_manager/switch_controller')
s = rospy.ServiceProxy('controller_manager/switch_controller', SwitchController)
strictness = SwitchControllerRequest.STRICT
resp = s.call(SwitchControllerRequest(start_controllers, stop_controllers, strictness))
if resp.ok == 1:
if start_controllers:
print "Started {} successfully".format(start_controllers)
print("Started {} successfully".format(start_controllers))
if stop_controllers:
print "Stopped {} successfully".format(stop_controllers)
print("Stopped {} successfully".format(stop_controllers))
return True
else:
print "Error when starting {} and stopping {}".format(start_controllers, stop_controllers)
print("Error when starting {} and stopping {}".format(start_controllers, stop_controllers))
return False
22 changes: 11 additions & 11 deletions controller_manager_tests/test/controller_manager_scripts.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,17 +4,17 @@
import subprocess

# output of controller_manager list, will by combined dynamically
myc1_running='my_controller1 - hardware_interface::EffortJointInterface ( running )\n'
myc1_stopped='my_controller1 - hardware_interface::EffortJointInterface ( stopped )\n'
myc2_running='my_controller2 - hardware_interface::EffortJointInterface ( running )\n'
myc2_stopped='my_controller2 - hardware_interface::EffortJointInterface ( stopped )\n'
myc1_running='\'my_controller1\' - \'hardware_interface::EffortJointInterface\' ( running )\n'
myc1_stopped='\'my_controller1\' - \'hardware_interface::EffortJointInterface\' ( stopped )\n'
myc2_running='\'my_controller2\' - \'hardware_interface::EffortJointInterface\' ( running )\n'
myc2_stopped='\'my_controller2\' - \'hardware_interface::EffortJointInterface\' ( stopped )\n'


# output of other commands
loaded_fmt = 'Loaded %s\n'
unloaded_fmt = 'Unloaded %s successfully\n'
stopped_fmt = "Stopped ['%s'] successfully\n"
started_fmt = "Started ['%s'] successfully\n"
loaded_fmt = 'Loaded \'%s\'\n'
unloaded_fmt = 'Unloaded \'%s\' successfully\n'
stopped_fmt = "Stopped [\'%s\'] successfully\n"
started_fmt = "Started [\'%s\'] successfully\n"

no_controllers = 'No controllers are loaded in mechanism control\n'
reload_response = 'Restore: False\nSuccessfully reloaded libraries\n'
Expand Down Expand Up @@ -124,15 +124,15 @@ def test_scripts(self):
# spawn
self.assertEqual(
run_cg('spawn group1'),
'Loaded my_controller1\n'
'Loaded my_controller3\n'
'Loaded \'my_controller1\'\n'
'Loaded \'my_controller3\'\n'
'Started [\'my_controller1\'] successfully\n'
'Started [\'my_controller3\'] successfully\n')

# switch
self.assertEqual(
run_cg('switch group2'),
'Loaded my_controller2\n'
'Loaded \'my_controller2\'\n'
'Started [\'my_controller2\'] successfully\n'
'Stopped [\'my_controller1\'] successfully\n')

Expand Down

0 comments on commit 7202777

Please sign in to comment.