def __init__(self):
    super().__init__('pybullet_ros', automatically_declare_parameters_from_overrides=True)
    ex = MultiThreadedExecutor()
    self.executor = ex
    self.pb = importlib.import_module('pybullet')
    self.loop_rate = self.get_parameter('loop_rate').value
    self.get_logger().info('Loop rate: {}'.format(self.loop_rate))
    is_gui_needed = self.get_parameter('pybullet_gui').value
    self.pause_simulation = self.get_parameter('pause_simulation').value
    print('\x1b[34m')
    physicsClient = self.start_gui(gui=is_gui_needed)
    self.create_service(Empty, 'reset_simulation', self.handle_reset_simulation)
    self.create_service(Empty, 'pause_physics', self.handle_pause_physics)
    self.create_service(Empty, 'unpause_physics', self.handle_unpause_physics)
    self.pb.setAdditionalSearchPath(pybullet_data.getDataPath())
    env_plugin = self.get_parameter('environment').value
    plugin_import_prefix = self.get_parameter('plugin_import_prefix').value
    self.environment = getattr(importlib.import_module(f'{plugin_import_prefix}.{env_plugin}'), 'Environment')(self)
    self.robot = self.init_pybullet_robot()
    self.connected_to_physics_server = None
    if (not self.robot):
        self.connected_to_physics_server = False
        return
    else:
        self.connected_to_physics_server = True
    (rev_joint_index_name_dic, prismatic_joint_index_name_dic, fixed_joint_index_name_dic, link_names_to_ids_dic) = self.get_properties()
    self.plugins = []
    plugins = self.get_parameter('plugins').value
    if (not plugins):
        self.get_logger().warn('No plugins found, forgot to set param plugins?')
    print('\x1b[0m')
    for plugin in plugins:
        (module_, class_) = plugin.split(':')
        params_ = {'module': module_, 'class': class_}
        self.get_logger().info('loading plugin: {} class from {}'.format(class_, module_))
        obj = getattr(importlib.import_module(module_), class_)(self.pb, self.robot, rev_joints=rev_joint_index_name_dic, prism_joints=prismatic_joint_index_name_dic, fixed_joints=fixed_joint_index_name_dic, link_ids=link_names_to_ids_dic, **params_)
        self.plugins.append(obj)
        self.executor.add_node(obj)
    self.get_logger().info('pybullet ROS wrapper initialized')
    self.timer = self.create_timer((1.0 / self.loop_rate), self.wrapper_callback)
    self.executor.add_node(self)
    try:
        self.executor.spin()
    finally:
        self.executor.shutdown()
        self.destroy_node()
        for node in self.plugins:
            node.destroy_node()
def __init__(self):
    super().__init__('pybullet_ros', automatically_declare_parameters_from_overrides=True)
    ex = MultiThreadedExecutor()
    self.executor = ex
    self.pb = importlib.import_module('pybullet')
    self.loop_rate = self.get_parameter('loop_rate').value
    self.get_logger().info('Loop rate: {}'.format(self.loop_rate))
    is_gui_needed = self.get_parameter('pybullet_gui').value
    self.pause_simulation = self.get_parameter('pause_simulation').value
    print('\x1b[34m')
    physicsClient = self.start_gui(gui=is_gui_needed)
    self.create_service(Empty, 'reset_simulation', self.handle_reset_simulation)
    self.create_service(Empty, 'pause_physics', self.handle_pause_physics)
    self.create_service(Empty, 'unpause_physics', self.handle_unpause_physics)
    self.pb.setAdditionalSearchPath(pybullet_data.getDataPath())
    env_plugin = self.get_parameter('environment').value
    plugin_import_prefix = self.get_parameter('plugin_import_prefix').value
    self.environment = getattr(importlib.import_module(f'{plugin_import_prefix}.{env_plugin}'), 'Environment')(self)
    self.robot = self.init_pybullet_robot()
    self.connected_to_physics_server = None
    if (not self.robot):
        self.connected_to_physics_server = False
        return
    else:
        self.connected_to_physics_server = True
    (rev_joint_index_name_dic, prismatic_joint_index_name_dic, fixed_joint_index_name_dic, link_names_to_ids_dic) = self.get_properties()
    self.plugins = []
    plugins = self.get_parameter('plugins').value
    if (not plugins):
        self.get_logger().warn('No plugins found, forgot to set param plugins?')
    print('\x1b[0m')
    for plugin in plugins:
        (module_, class_) = plugin.split(':')
        params_ = {'module': module_, 'class': class_}
        self.get_logger().info('loading plugin: {} class from {}'.format(class_, module_))
        obj = getattr(importlib.import_module(module_), class_)(self.pb, self.robot, rev_joints=rev_joint_index_name_dic, prism_joints=prismatic_joint_index_name_dic, fixed_joints=fixed_joint_index_name_dic, link_ids=link_names_to_ids_dic, **params_)
        self.plugins.append(obj)
        self.executor.add_node(obj)
    self.get_logger().info('pybullet ROS wrapper initialized')
    self.timer = self.create_timer((1.0 / self.loop_rate), self.wrapper_callback)
    self.executor.add_node(self)
    try:
        self.executor.spin()
    finally:
        self.executor.shutdown()
        self.destroy_node()
        for node in self.plugins:
            node.destroy_node()