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()
