def on_shutdown(self):
for controller_name in self.controllers.keys():
print("Stopping controller: ", controller_name)
req = StopController()
req.controller_name = controller_name
self.stop_controller(req)
for serial_proxy in self.serial_proxies.values():
serial_proxy.disconnect()
def restart_controller(self, req):
stop_req = StopController()
stop_req.controller_name = req.controller_name
response1 = self.stop_controller(stop_req)
response2 = self.start_controller(req)
res = RestartControllerResponse()
res.success = response1.success and response2.success
res.reason = '%s\n%s' % (response1.reason, response2.reason)
return res
def restart_controller(self, req):
response1 = self.stop_controller(StopController(req.controller_name))
response2 = self.start_controller(req)
return RestartControllerResponse(
response1.success and response2.success,
'%s\n%s' % (response1.reason, response2.reason))
def restart_controller(self, req, res):
response1 = self.stop_controller(StopController(req.controller_name))
response2 = self.start_controller(req)
res.success = response1.success and response2.success
res.reason = '%s\n%s' % (response1.reason, response2.reason)
return res
def manage_controller(node, controller_name, port_namespace, controller_type, command, deps, start, stop, restart):
try:
package_path = node.get_parameter(controller_name + '.controller.package').value
module_name = node.get_parameter(controller_name + '.controller.module').value
class_name = node.get_parameter(controller_name + '.controller.type').value
except rclpy.exceptions.ParameterNotDeclaredException as e:
node.get_logger().error('[%s] configuration error: could not find controller parameters on parameter server' % controller_name)
raise
except Exception as e:
node.get_logger().error('[%s]: %s' % (controller_name, e))
raise
if command.lower() == 'start':
try:
req = StartController.Request()
req.port_name = port_namespace
req.package_path = package_path
req.module_name = module_name
req.class_name = class_name
req.controller_name = controller_name
req.dependencies = deps
for name, param in node.get_parameters_by_prefix(controller_name).items():
if not name.startswith('controller.'):
param = rclpy.parameter.Parameter(name, param.type_, param.value)
req.parameters.append(param.to_parameter_msg())
node.get_logger().info('[%s] passing %d parameters' % (controller_name, len(req.parameters)))
future = start.call_async(req)
rclpy.spin_until_future_complete(node, future)
response = future.result()
if response.success: node.get_logger().info(response.reason)
else: node.get_logger().error(response.reason)
except Exception as e:
node.get_logger().error('Service call failed: %s' % e)
elif command.lower() == 'stop':
try:
req = StopController.Request()
req.controller_name = controller_name
future = stop.call_async(req)
rclpy.spin_until_future_complete(node, future)
response = future.result()
if response.success: node.get_logger().info(response.reason)
else: node.get_logger().error(response.reason)
except Exception as e:
node.get_logger().error('Service call failed: %s' % e)
elif command.lower() == 'restart':
try:
req = RestartController.Request()
req.port_name = port_namespace
req.package_path = package_path
req.module_name = module_name
req.class_name = class_name
req.controller_name = controller_name
req.dependencies = deps
for name, param in node.get_parameters_by_prefix(controller_name).items():
if not name.startswith('controller'):
param = rclpy.parameter.Parameter(name, param.type_, param.value)
req.parameters.append(param.to_parameter_msg())
node.get_logger().info('[%s] passing %d parameters' % (controller_name, len(req.parameters)))
future = restart.call_async(req)
rclpy.spin_until_future_complete(node, future)
response = future.result()
if response.success: node.get_logger().info(response.reason)
else: node.get_logger().error(response.reason)
except Exception as e:
node.get_logger().error('Service call failed: %s' % e)
else:
node.get_logger().error('Invalid command.')
parser.print_help()