def __init__( self, worker_id: int, name: str, policy_type: str = 'DQN') -> None: """Initialize Worker Node class.""" super().__init__(worker_id, name, policy_type) self.episode = 0 # Set timer flags, futures, and callback groups. self.flag = Flags(True, False, False, False, True) self._future_weights = rclpy.Future() self._future_gradients = rclpy.Future() self._cb_group = ReentrantCallbackGroup() self._total_reward = 0 self._update = 10 # Create ROS service clients and publisher. self._cli: Dict[str, Client] = { 'weights': self.create_client( Weights, '/weights', callback_group=self._cb_group), 'gradients': self.create_client( Gradients, '/gradient', callback_group=self._cb_group), } self._pub = self.create_publisher( Float32, 'total_reward', 10, callback_group=self._cb_group) # Check if ROS service servers are available. while not self._cli['weights'].wait_for_service(timeout_sec=1.0): self.get_logger().info( '`Weights` service not available, waiting again...') while not self._cli['gradients'].wait_for_service(timeout_sec=1.0): self.get_logger().info( '`Gradients` service not available, waiting again...')
def main(): rclpy.init(args=None) node = rclpy.create_node('minimal_action_node') executor = MultiThreadedExecutor() exit_future = rclpy.Future() asw = ActionServerWrapperThread(node, exit_future) asw.start() time.sleep( 0.5 ) # without this instruction this error is thrown at the end of the program: # PyCapsule_GetPointer called with invalid PyCapsule object #3 acw = ActionClientWrapperThread(node, exit_future) acw.start() while not exit_future.done(): rclpy.spin_until_future_complete(node, exit_future, executor=executor, timeout_sec=1) acw.destroy() asw.destroy() executor.shutdown() node.destroy_node() rclpy.shutdown()
def __init__(self, name, **kwargs): """Class constructor.""" super().__init__(name, allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True, **kwargs) # This flag will be set to true once the thruster allocation matrix is # available self._ready = False # Acquiring the namespace of the vehicle self.namespace = self.get_namespace() if self.namespace != '': if self.namespace[-1] != '/': self.namespace += '/' if self.namespace[0] != '/': self.namespace = '/' + self.namespace # Load all parameters self.config = self.get_parameters_by_prefix('thruster_manager') if len(self.config) == 0: raise RuntimeError('Thruster manager parameters not ' 'initialized for uuv_name=' + self.namespace) self.config = parse_nested_params_to_dict(self.config, '.') # Indicates whether we could get the robot description for the thrust axes self.use_robot_descr = False self.axes = {} self.robot_description_subscription = None if self.config['update_rate'].value < 0: self.config['update_rate'].value = 50.0 self.base_link_ned_to_enu = None self.tf_buffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tf_buffer, self) # Initialize some variables self.output_dir = None self.n_thrusters = 0 # Thruster objects used to calculate the right angular velocity command self.thrusters = list() # Thrust forces vector self.thrust = None # Thruster allocation matrix: transform thruster inputs to force/torque self.configuration_matrix = None self.inverse_configuration_matrix = None self.init_future = rclpy.Future() self.init_thread = threading.Thread(target=self._init_async, daemon=True) self.init_thread.start()
def main(): parsed_args = parse_init_args() rclpy.init() executor = SingleThreadedExecutor() # executes the ROS callbacks # executor = MultiThreadedExecutor() # executes the ROS callbacks exit_event = rclpy.Future() sender_node = Sender(parsed_args.name, parsed_args.topic, parsed_args.msg_size, parsed_args.pub_time, parsed_args.num_iters, parsed_args.halt_time, exit_event) rclpy.spin_until_future_complete(sender_node, exit_event, executor=executor) sender_node.destroy_node() rclpy.shutdown()
def main(): rclpy.init(args=None) exit_future = rclpy.Future() asw = ActionServerWrapperProcess(exit_future) asw.start() time.sleep( 0.5 ) # without this instruction this error is thrown at the end of the program: # PyCapsule_GetPointer called with invalid PyCapsule object #3 acw = ActionClientWrapperProcess(exit_future) acw.start() acw.send_goal() acw.join() asw.join() rclpy.shutdown()
def __init__(self, node_name, **kwargs): """Class constructor.""" super().__init__(node_name, allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True, **kwargs) #Default sim_time to True # sim_time = rclpy.parameter.Parameter('use_sim_time', rclpy.Parameter.Type.BOOL, True) # self.set_parameters([sim_time]) # This flag will be set to true once the thruster allocation matrix is # available self._ready = False # Acquiring the namespace of the vehicle self.namespace = self.get_namespace() if self.namespace != '': if self.namespace[-1] != '/': self.namespace += '/' if self.namespace[0] != '/': self.namespace = '/' + self.namespace self.config = self.get_parameters_by_prefix('thruster_manager') if len(self.config) == 0: #if not self.has_parameter('thruster_manager'): raise RuntimeError('Thruster manager parameters not ' 'initialized for uuv_name=' + self.namespace) self.config = parse_nested_params_to_dict(self.config, '.') # Load all parameters #self.config = self.get_parameter('thruster_manager').value #TODO To change in foxy #robot_description_param = self.namespace + 'robot_description' robot_description_param = 'urdf_file' self.use_robot_descr = False self.axes = {} if self.has_parameter(robot_description_param): urdf_file = self.get_parameter(robot_description_param).value if urdf_file != "": self.use_robot_descr = True self.parse_urdf(urdf_file) if self.config['update_rate'].value < 0: self.config['update_rate'].value = 50.0 self.base_link_ned_to_enu = None self.tf_buffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tf_buffer, self) # Initialize some variables self.output_dir = None self.n_thrusters = 0 # Thruster objects used to calculate the right angular velocity command self.thrusters = list() # Thrust forces vector self.thrust = None # Thruster allocation matrix: transform thruster inputs to force/torque self.configuration_matrix = None self.inverse_configuration_matrix = None self.init_future = rclpy.Future() self.init_thread = threading.Thread(target=self._init_async, daemon=True) self.init_thread.start()
def __init__(self, name, world_ned_to_enu=None, **kwargs): super().__init__(name, allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True, **kwargs) # sim_time = rclpy.parameter.Parameter('use_sim_time', rclpy.Parameter.Type.BOOL, True) # self.set_parameters([sim_time]) self.namespace = self.get_namespace().replace('/', '') self.get_logger().info('Initialize control for vehicle <%s>' % self.namespace) self.local_planner = DPControllerLocalPlanner( self, full_dof=True, thrusters_only=False, stamped_pose_only=False, tf_trans_world_ned_to_enu=world_ned_to_enu) self.base_link = self.get_parameter_or_helper( 'base_link', 'base_link').get_parameter_value().string_value # Reading the minimum thrust generated self.min_thrust = self.get_parameter_or_helper('min_thrust', 0.0).value assert self.min_thrust >= 0 self.get_logger().info('Min. thrust [N]=%.2f' % self.min_thrust) # Reading the maximum thrust generated self.max_thrust = self.get_parameter_or_helper('max_thrust', 0.0).value assert self.max_thrust > 0 and self.max_thrust > self.min_thrust self.get_logger().info('Max. thrust [N]=%.2f' % self.max_thrust) # Reading the thruster topic self.thruster_topic = self.get_parameter_or_helper( 'thruster_topic', 'thrusters/id_0/input').get_parameter_value().string_value assert len(self.thruster_topic) > 0 # Reading the thruster gain self.p_gain_thrust = self.get_parameter_or_helper( 'thrust_p_gain', 0.0).value assert self.p_gain_thrust > 0 self.d_gain_thrust = self.get_parameter_or_helper( 'thrust_d_gain', 0.0).value assert self.d_gain_thrust >= 0 # Reading the roll gain self.p_roll = self.get_parameter_or_helper('p_roll', 0.0).value assert self.p_roll > 0 # Reading the pitch P gain self.p_pitch = self.get_parameter_or_helper('p_pitch', 0.0).value assert self.p_pitch > 0 # Reading the pitch D gain self.d_pitch = self.get_parameter_or_helper('d_pitch', 0.0).value assert self.d_pitch >= 0 # Reading the yaw P gain self.p_yaw = self.get_parameter_or_helper('p_yaw', 0.0).value assert self.p_yaw > 0 # Reading the yaw D gain self.d_yaw = self.get_parameter_or_helper('d_yaw', 0.0).value assert self.d_yaw >= 0 # Reading the saturation for the desired pitch self.desired_pitch_limit = self.get_parameter_or_helper( 'desired_pitch_limit', 15 * np.pi / 180).value assert self.desired_pitch_limit > 0 # Reading the saturation for yaw error self.yaw_error_limit = self.get_parameter_or_helper( 'yaw_error_limit', 1.0).value assert self.yaw_error_limit > 0 # Reading the number of fins self.n_fins = self.get_parameter_or_helper( 'n_fins', 0).get_parameter_value().integer_value assert self.n_fins > 0 # Reading the mapping for roll commands self.map_roll = self.get_parameter_or_helper('map_roll', [0, 0, 0, 0]).value assert isinstance(self.map_roll, list) assert len(self.map_roll) == self.n_fins # Reading the mapping for the pitch commands self.map_pitch = self.get_parameter_or_helper('map_pitch', [0, 0, 0, 0]).value assert isinstance(self.map_pitch, list) assert len(self.map_pitch) == self.n_fins # Reading the mapping for the yaw commands self.map_yaw = self.get_parameter_or_helper('map_yaw', [0, 0, 0, 0]).value assert isinstance(self.map_yaw, list) assert len(self.map_yaw) == self.n_fins # Retrieve the thruster configuration parameters self.thruster_config = self.get_parameters_by_prefix('thruster_config') #Parse parameters to dictionary and unpack params to values self.thruster_config = parse_nested_params_to_dict( self.thruster_config, '.', True) # Check if all necessary thruster model parameter are available thruster_params = [ 'conversion_fcn_params', 'conversion_fcn', 'topic_prefix', 'topic_suffix', 'frame_base', 'max_thrust' ] for p in thruster_params: if p not in self.thruster_config: raise RuntimeError( 'Parameter <%s> for thruster conversion function is ' 'missing' % p) # Setting up the thruster topic name self.thruster_topic = self.build_thruster_topic_name( self.namespace, self.thruster_config['topic_prefix'], 0, self.thruster_config['topic_suffix']) # self.thruster_topic = '/%s/%s/id_%d/%s' % (self.namespace, # self.thruster_config['topic_prefix'], 0, # self.thruster_config['topic_suffix']) self.max_fin_angle = self.get_parameter_or_helper( 'max_fin_angle', 0.0).value assert self.max_fin_angle > 0 # Reading the fin input topic prefix self.fin_topic_prefix = self.get_parameter_or_helper( 'fin_topic_prefix', 'fins').get_parameter_value().string_value self.fin_topic_suffix = self.get_parameter_or_helper( 'fin_topic_suffix', 'input').get_parameter_value().string_value self.rpy_to_fins = np.vstack( (self.map_roll, self.map_pitch, self.map_yaw)).T self.pub_cmd = list() self.odometry_sub = None self.reference_pub = None self.error_pub = None self.tf_buffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tf_buffer, self) self._ready = False self.init_future = rclpy.Future() self.init_thread = threading.Thread(target=self._init_async, daemon=True) self.init_thread.start()