Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(components): make all possible attributes private #162

Merged
merged 2 commits into from
Nov 11, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions CHANGELOG.md
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@ Release Versions:

- fix(controllers): change predicate rate to double type (#163)
- fix(controllers): trigger check (#164)
- fix(components): make all possible attributes private (#162)

## 5.0.1

Expand Down
16 changes: 8 additions & 8 deletions source/modulo_components/modulo_components/component.py
Original file line number Diff line number Diff line change
Expand Up @@ -30,10 +30,10 @@ def _step(self):
Step function that is called periodically.
"""
try:
self._evaluate_periodic_callbacks()
self._ComponentInterface__evaluate_periodic_callbacks()
self.on_step_callback()
self._publish_outputs()
self._publish_predicates()
self._ComponentInterface__publish_outputs()
self._ComponentInterface__publish_predicates()
except Exception as e:
self.get_logger().error(f"Failed to execute step function: {e}", throttle_duration_sec=1.0)
self.raise_error()
Expand Down Expand Up @@ -88,12 +88,12 @@ def add_output(self, signal_name: str, data: str, message_type: MsgT,
:param publish_on_step: If true, the output is published periodically on step
"""
try:
parsed_signal_name = self._create_output(signal_name, data, message_type, clproto_message_type,
default_topic, fixed_topic, publish_on_step)
parsed_signal_name = self._ComponentInterface__create_output(
signal_name, data, message_type, clproto_message_type, default_topic, fixed_topic, publish_on_step)
topic_name = self.get_parameter_value(parsed_signal_name + "_topic")
self.get_logger().debug(f"Adding output '{parsed_signal_name}' with topic name '{topic_name}'.")
publisher = self.create_publisher(message_type, topic_name, self._qos)
self._outputs[parsed_signal_name]["publisher"] = publisher
publisher = self.create_publisher(message_type, topic_name, self.get_qos())
self._ComponentInterface__outputs[parsed_signal_name]["publisher"] = publisher
except Exception as e:
self.get_logger().error(f"Failed to add output '{signal_name}': {e}")

Expand All @@ -103,4 +103,4 @@ def raise_error(self):
"""
super().raise_error()
self.set_predicate("in_error_state", True)
self._finalize_interfaces()
self._ComponentInterface__finalize_interfaces()
191 changes: 102 additions & 89 deletions source/modulo_components/modulo_components/component_interface.py

Large diffs are not rendered by default.

42 changes: 22 additions & 20 deletions source/modulo_components/modulo_components/lifecycle_component.py
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,7 @@ def __init__(self, node_name: str, *args, **kwargs):
lifecycle_node_kwargs = {key: value for key, value in kwargs.items() if key in LIFECYCLE_NODE_MIXIN_KWARGS}
ComponentInterface.__init__(self, node_name, *args, **kwargs)
LifecycleNodeMixin.__init__(self, *args, **lifecycle_node_kwargs)
self._has_error = False
self.__has_error = False

def get_lifecycle_state(self) -> LifecycleState:
"""
Expand Down Expand Up @@ -226,12 +226,13 @@ def on_shutdown(self, previous_state: LifecycleState) -> TransitionCallbackRetur
TRANSITION_CALLBACK_SUCCESS transitions to 'Finalized'
TRANSITION_CALLBACK_FAILURE, TRANSITION_CALLBACK_ERROR or any uncaught exceptions to 'ErrorProcessing'
"""

def error_processing(self):
self.get_logger().error("Entering into the error processing transition state.")
return TransitionCallbackReturn.ERROR

self.get_logger().debug(f"on_shutdown called from previous state {previous_state.label}.")
if not self._has_error:
if not self.__has_error:
if previous_state.state_id == State.PRIMARY_STATE_FINALIZED:
return TransitionCallbackReturn.SUCCESS
if previous_state.state_id == State.PRIMARY_STATE_ACTIVE:
Expand All @@ -243,20 +244,20 @@ def error_processing(self):
return error_processing(self)
if not self.__handle_shutdown():
return error_processing(self)
self._finalize_interfaces()
self._finalize_component_interfaces()
return TransitionCallbackReturn.SUCCESS
if previous_state.state_id == State.PRIMARY_STATE_INACTIVE:
if not self.__handle_cleanup():
self.get_logger().debug("Shutdown failed during intermediate cleanup!")
return error_processing(self)
if not self.__handle_shutdown():
return error_processing(self)
self._finalize_interfaces()
self._finalize_component_interfaces()
return TransitionCallbackReturn.SUCCESS
if previous_state.state_id == State.PRIMARY_STATE_UNCONFIGURED:
if not self.__handle_shutdown():
return error_processing(self)
self._finalize_interfaces()
self._finalize_component_interfaces()
return TransitionCallbackReturn.SUCCESS
self.get_logger().warn(f"Invalid transition 'shutdown' from state {previous_state.label}.")
return error_processing(self)
Expand Down Expand Up @@ -303,9 +304,9 @@ def on_error(self, previous_state: LifecycleState) -> TransitionCallbackReturn:
error_handled = False
if not error_handled:
self.get_logger().error("Error processing failed! Entering into the finalized state.")
self._finalize_interfaces()
self._ComponentInterface__finalize_component_interfaces()
return TransitionCallbackReturn.ERROR
self._has_error = False
self.__has_error = False
return TransitionCallbackReturn.SUCCESS

def __handle_error(self) -> bool:
Expand All @@ -332,10 +333,10 @@ def _step(self):
"""
try:
if self.get_lifecycle_state().state_id == State.PRIMARY_STATE_ACTIVE:
self._evaluate_periodic_callbacks()
self._ComponentInterface__evaluate_periodic_callbacks()
self.on_step_callback()
self._publish_outputs()
self._publish_predicates()
self._ComponentInterface__publish_outputs()
self._ComponentInterface__publish_predicates()
except Exception as e:
self.get_logger().error(f"Failed to execute step function: {e}")
self.raise_error()
Expand All @@ -347,13 +348,14 @@ def __configure_outputs(self) -> bool:
:return: True if configuration was successful
"""
success = True
for signal_name, output_dict in self._outputs.items():
for signal_name, output_dict in self._ComponentInterface__outputs.items():
self.get_logger().error(f"{signal_name}, {output_dict}")
try:
topic_name = self.get_parameter_value(signal_name + "_topic")
self.get_logger().debug(f"Configuring output '{signal_name}' with topic name '{topic_name}'.")
self.get_logger().error(f"Configuring output '{signal_name}' with topic name '{topic_name}'.")
publisher = self.create_lifecycle_publisher(msg_type=output_dict["message_type"], topic=topic_name,
qos_profile=self._qos)
self._outputs[signal_name]["publisher"] = publisher
qos_profile=self.get_qos())
self._ComponentInterface__set_output_publisher(signal_name, publisher)
except Exception as e:
success = False
self.get_logger().debug(f"Failed to configure output '{signal_name}': {e}")
Expand All @@ -378,8 +380,8 @@ def add_output(self, signal_name: str, data: str, message_type: MsgT,
throttle_duration_sec=1.0)
return
try:
parsed_signal_name = self._create_output(signal_name, data, message_type, clproto_message_type,
default_topic, fixed_topic, publish_on_step)
parsed_signal_name = self._ComponentInterface__create_output(
signal_name, data, message_type, clproto_message_type, default_topic, fixed_topic, publish_on_step)
topic_name = self.get_parameter_value(parsed_signal_name + "_topic")
self.get_logger().debug(f"Adding output '{parsed_signal_name}' with topic name '{topic_name}'.")
except AddSignalError as e:
Expand All @@ -388,7 +390,7 @@ def add_output(self, signal_name: str, data: str, message_type: MsgT,
def __activate_outputs(self):
success = True
state = self.get_lifecycle_state().state_id
for signal_name, output_dict in self._outputs.items():
for signal_name, output_dict in self._ComponentInterface__outputs.items():
try:
output_dict["publisher"].on_activate(state)
except Exception as e:
Expand All @@ -400,19 +402,19 @@ def __activate_outputs(self):
def __deactivate_outputs(self):
success = True
state = self.get_lifecycle_state().state_id
for signal_name, output_dict in self._outputs.items():
for signal_name, output_dict in self._ComponentInterface__outputs.items():
try:
output_dict["publisher"].on_deactivate(state)
except Exception as e:
success = False
self.get_logger().error(f"Failed to deactivate output '{signal_name}': {e}")
self.get_logger().debug("All outputs deactivated.")
return success

def raise_error(self):
"""
Trigger the shutdown and error transitions.
"""
super().raise_error()
self._has_error = True
self.__has_error = True
self.trigger_shutdown()
Original file line number Diff line number Diff line change
Expand Up @@ -251,6 +251,7 @@ TYPED_TEST(ComponentInterfaceTest, TF) {
this->component_->add_tf_listener();
auto send_tf = state_representation::CartesianPose::Random("test", "world");
EXPECT_NO_THROW(this->component_->send_transform(send_tf));
sleep(1);
state_representation::CartesianPose lookup_tf;
EXPECT_NO_THROW(lookup_tf = this->component_->lookup_transform("test", "world"));
auto identity = send_tf * lookup_tf.inverse();
Expand Down
10 changes: 5 additions & 5 deletions source/modulo_components/test/python/test_component.py
Original file line number Diff line number Diff line change
Expand Up @@ -11,23 +11,23 @@ def component(ros_context):

def test_add_remove_output(component):
component.add_output("8_teEsTt_#1@3", "test", Bool)
assert "test_13" in component._outputs.keys()
assert "test_13" in component._ComponentInterface__outputs.keys()
assert component.get_parameter_value("test_13_topic") == "~/test_13"
with pytest.raises(CoreError):
component.publish_output("test_13")

component.add_output("9_tEestT_#1@5", "test", Bool, default_topic="/topic")
assert "test_15" in component._outputs.keys()
assert "test_15" in component._ComponentInterface__outputs.keys()
assert component.get_parameter_value("test_15_topic") == "/topic"

component.add_output("test_13", "test", String)
assert component._outputs["test_13"]["message_type"] == Bool
assert component._ComponentInterface__outputs["test_13"]["message_type"] == Bool

component.remove_output("test_13")
assert "test_13" not in component._outputs.keys()
assert "test_13" not in component._ComponentInterface__outputs.keys()

component.add_output("8_teEsTt_#1@3", "test", Bool, publish_on_step=False)
assert not component._periodic_outputs["test_13"]
assert not component._ComponentInterface__periodic_outputs["test_13"]
component.publish_output("8_teEsTt_#1@3")
component.publish_output("test_13")
with pytest.raises(CoreError):
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@ def test_input_output_manual(ros_exec, random_pose, minimal_cartesian_output, mi

@pytest.mark.parametrize("minimal_sensor_input", [[Component, "/topic"]], indirect=True)
@pytest.mark.parametrize("minimal_sensor_output", [[Component, "/topic", False]], indirect=True)
def test_input_output_manual(ros_exec, random_sensor, minimal_sensor_output, minimal_sensor_input):
def test_input_output_manual_sensor(ros_exec, random_sensor, minimal_sensor_output, minimal_sensor_input):
ros_exec.add_node(minimal_sensor_input)
ros_exec.add_node(minimal_sensor_output)
ros_exec.spin_until_future_complete(minimal_sensor_input.received_future, timeout_sec=0.5)
Expand Down
Loading
Loading