def check_deps(self):
controllers_still_waiting = []
for i, (controller_name, deps,
kls) in enumerate(self.waiting_meta_controllers):
if not set(deps).issubset(self.controllers.keys()):
controllers_still_waiting.append(
self.waiting_meta_controllers[i])
self.get_logger().warning(
'[%s] not all dependencies started, still waiting for %s...'
%
(controller_name,
str(list(set(deps).difference(self.controllers.keys())))))
else:
dependencies = [
self.controllers[dep_name] for dep_name in deps
]
controller = kls(controller_name, dependencies)
if controller.initialize():
controller.start()
self.controllers[controller_name] = controller
rclpy.get_global_executor().add_node(controller)
self.waiting_meta_controllers = controllers_still_waiting[:]
def _get_node(cls):
# https://en.wikipedia.org/wiki/Double-checked_locking
if not cls.__node:
with cls.__node_lock:
if not cls.__node:
cls.__node = rclpy.create_node('Spawner')
rclpy.get_global_executor().add_node(cls.__node)
return cls.__node
def launch_node(type, *args, **kargs):
global node
sleep = 0.1 if 'sleep' not in kargs else kargs['sleep']
if use_ros_1:
node = type(*args)
if hasattr(type, 'on_run') and callable(getattr(type, 'on_run')):
rate = rospy.Rate(1.0 / sleep)
while not rospy.is_shutdown():
node.on_run()
rate.sleep()
else:
rospy.spin()
else:
rclpy.init()
node = type(*args)
if hasattr(type, 'on_run') and callable(getattr(type, 'on_run')):
executor = rclpy.get_global_executor()
try:
executor.add_node(node)
while rclpy.ok():
executor.spin_once(timeout_sec=sleep)
node.on_run()
finally:
executor.remove_node(node)
else:
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
def main():
rclpy.init()
executor = rclpy.get_global_executor()
node = Car()
executor.add_node(node)
executor.spin()
rclpy.shutdown()
def __init__(self):
# initialize ROS 2
ros2.init()
self.__executor = ros2.get_global_executor()
self.__rate_node = ros2.create_node("rate_node")
# Modules
self.camera = camera_real.CameraReal()
self.controller = controller_real.ControllerReal(self)
self.display = display_real.DisplayReal()
self.drive = drive_real.DriveReal()
self.lidar = lidar_real.LidarReal()
self.physics = physics_real.PhysicsReal()
# Add all nodes to the executor
rate_added = self.__executor.add_node(self.__rate_node)
camera_added = self.__executor.add_node(self.camera.node)
lidar_added = self.__executor.add_node(self.lidar.node)
controller_added = self.__executor.add_node(self.controller.node)
physics_added = self.__executor.add_node(self.physics.node)
assert rate_added and lidar_added and camera_added and controller_added, (
"Issues initializing Racecar nodes. Node status: \n"
f"Rate operational: {rate_added} | "
f"Camera operational: {camera_added} | "
f"Lidar operational: {lidar_added} | "
f"Controller operational: {controller_added} | "
f"Physics operational: {physics_added} | ")
# User provided start and update functions
self.__user_start = None
self.__user_update = None
self.__user_update_slow = None
# True if the main thread should be running
self.__running = False
# Variables relating to the run thread
self.__run_thread = None
self.__cur_update = self.__default_update
self.__cur_update_slow = None
self.__cur_frame_time = datetime.now()
self.__last_frame_time = datetime.now()
self.__cur_update_counter = 0
self.__max_update_counter = 1
self.set_update_slow_time(self.__DEFAULT_UPDATE_SLOW_TIME)
# Start run_thread in default drive mode
self.__handle_back()
self.__run_thread = threading.Thread(target=self.__run)
self.__run_thread.daemon = True
self.__run_thread.start()
# Print welcome message
print(">> Racecar initialization successful")
print(">> Controls:\n"
" START button = run your program\n"
" BACK button = enter default drive mode\n"
" BACK + START buttons simultaneously = exit the program\n"
" CTRL + Z on keyboard = force quit the program")
def stop_controller(self, req, res):
controller_name = req.controller_name
self.stop_controller_lock.acquire()
if controller_name in self.controllers:
self.controllers[controller_name].stop()
rclpy.get_global_executor().remove_node(
controllers[controller_name])
del self.controllers[controller_name]
self.stop_controller_lock.release()
res.success = True
res.reason = 'controller %s successfully stopped.' % controller_name
return res
else:
self.self.stop_controller_lock.release()
res.success = False
res.reason = 'controller %s was not running.' % controller_name
return res
def _run(self):
executor = rclpy.get_global_executor()
try:
executor.add_node(self.__launch_ros_node)
while not self.__shutting_down:
# TODO(wjwwood): switch this to `spin()` when it considers
# asynchronously added subscriptions.
# see: https://github.com/ros2/rclpy/issues/188
executor.spin_once(timeout_sec=1.0)
except KeyboardInterrupt:
pass
finally:
executor.remove_node(self.__launch_ros_node)
def main(args=None):
rclpy.init(args=args)
manager = None
try:
manager = ControllerManager()
#rclpy.spin(manager)
executor = rclpy.get_global_executor()
executor.add_node(manager)
for node in manager.serial_proxies.values():
executor.add_node(node)
executor.spin()
finally:
if manager is not None:
manager.on_shutdown()
rclpy.shutdown()
def spin_until_future_complete(self, future: rclpy.task.Future, timeout_sec: float):
"""
This replicates rclpy.executors.spin_until_future_complete until a bugfix for
the timeout calculation is accepted.
.. seealso: https://github.com/ros2/rclpy/pull/372
Args:
future: the future to block on
timeout_sec: time to block on futures
"""
executor = rclpy.get_global_executor()
executor.add_node(self.node)
start = time.monotonic()
end = start + timeout_sec
timeout_left = timeout_sec
while executor._context.ok() and not future.done():
executor.spin_once(timeout_sec=timeout_left)
now = time.monotonic()
if now >= end:
break
timeout_left = end - now
def start_controller(self, req, res):
port_name = req.port_name
package_path = req.package_path
module_name = req.module_name
class_name = req.class_name
controller_name = req.controller_name
parameters = [Parameter.from_parameter_msg(p) for p in req.parameters]
self.start_controller_lock.acquire()
if controller_name in self.controllers:
self.start_controller_lock.release()
res.success = False
res.reason = 'Controller [%s] already started. If you want to restart it, call restart.' % controller_name
return res
try:
if module_name not in sys.modules:
# import if module not previously imported
package_module = __import__(package_path, globals(), locals(),
[module_name])
else:
# reload module if previously imported
package_module = reload(sys.modules[package_path])
controller_module = getattr(package_module, module_name)
except ImportError as ie:
self.start_controller_lock.release()
res.success = False
res.reason = 'Cannot find controller module. Unable to start controller %s\n%s' % (
module_name, str(ie))
return res
except SyntaxError as se:
self.start_controller_lock.release()
res.success = False
res.reason = 'Syntax error in controller module. Unable to start controller %s\n%s' % (
module_name, str(se))
return res
except Exception as e:
self.start_controller_lock.release()
res.success = False
res.reason = 'Unknown error has occured. Unable to start controller %s\n%s' % (
module_name, str(e))
return res
kls = getattr(controller_module, class_name)
if port_name == 'meta':
self.waiting_meta_controllers.append(
(controller_name, req.dependencies, kls))
self.check_deps()
self.start_controller_lock.release()
res.success = True
res.reason = ''
return res
if port_name != 'meta' and (port_name not in self.serial_proxies):
self.start_controller_lock.release()
res.success = False
res.reason = 'Specified port [%s] not found, available ports are %s. Unable to start controller %s' % (
port_name, str(self.serial_proxies.keys()), controller_name)
return res
controller = kls(self.serial_proxies[port_name].dxl_io,
controller_name,
port_name,
node_namespace=self.get_namespace(),
parameters=parameters)
if controller.initialize():
controller.start()
self.controllers[controller_name] = controller
rclpy.get_global_executor().add_node(controller)
self.check_deps()
self.start_controller_lock.release()
res.success = True
res.reason = 'Controller %s successfully started.' % controller_name
return res
else:
self.start_controller_lock.release()
res.success = False
res.reason = 'Initialization failed. Unable to start controller %s' % controller_name
return res
