Merge branch 'main' into feat/lifecycle-cleanup

CHANGELOG.md

@@ -22,10 +22,8 @@ Release Versions:
 
  - fix(components): remove incorrect log line (#166)
  - fix(controllers): move predicate publishing rate parameter to BaseControllerInterface (#168)
- - feat(controllers): add TF listener in BaseControllerInterface (#169)
- - feat(controllers): add TF broadcaster in BaseControllerInterface (#170)
- - test(controllers): add TF listener and broadcaster tests (#172)
  - feat(components): get clproto message type from attribute (#175)
+ - fix(components): add missing test case (#181)
  - fix(components): clean up lifecycle nodes properly (#178)
 
 ## 5.0.2

source/modulo_components/src/ComponentInterface.cpp

@@ -537,7 +537,7 @@ geometry_msgs::msg::TransformStamped ComponentInterface::lookup_ros_transform(
     const std::string& frame, const std::string& reference_frame, const tf2::TimePoint& time_point,
     const tf2::Duration& duration) {
   if (this->tf_buffer_ == nullptr || this->tf_listener_ == nullptr) {
-    throw exceptions::LookupTransformException("Failed to lookup transform: No TF buffer / listener configured.");
+    throw exceptions::LookupTransformException("Failed to lookup transform: To TF buffer / listener configured.");
   }
   try {
     return this->tf_buffer_->lookupTransform(reference_frame, frame, time_point, duration);

source/modulo_components/test/python/conftest.py

@@ -79,7 +79,8 @@ def publish(self):
 
         component = component_type("minimal_sensor_output")
         component._output = random_sensor
-        component.add_output("sensor_state", "_output", JointState, default_topic=topic, publish_on_step=publish_on_step)
+        component.add_output("sensor_state", "_output", JointState,
+                             default_topic=topic, publish_on_step=publish_on_step)
         component.publish = publish.__get__(component)
         return component
 

source/modulo_components/test/python/test_lifecycle_component_communication.py

@@ -48,6 +48,29 @@ def test_input_output_manual(ros_exec, make_lifecycle_change_client, random_pose
     assert random_pose.dist(minimal_cartesian_input.input) < 1e-3
 
 
+@pytest.mark.parametrize("minimal_sensor_input", [[LifecycleComponent, "/topic"]], indirect=True)
+@pytest.mark.parametrize("minimal_sensor_output", [[LifecycleComponent, "/topic", False]], indirect=True)
+def test_input_output_manual_sensor(
+        ros_exec, make_lifecycle_change_client, random_sensor, minimal_sensor_output, minimal_sensor_input):
+    input_change_client = make_lifecycle_change_client("minimal_sensor_input")
+    output_change_client = make_lifecycle_change_client("minimal_sensor_output")
+    ros_exec.add_node(input_change_client)
+    ros_exec.add_node(output_change_client)
+    ros_exec.add_node(minimal_sensor_input)
+    ros_exec.add_node(minimal_sensor_output)
+    input_change_client.configure(ros_exec)
+    output_change_client.configure(ros_exec)
+    input_change_client.activate(ros_exec)
+    output_change_client.activate(ros_exec)
+    ros_exec.spin_until_future_complete(minimal_sensor_input.received_future, timeout_sec=0.5)
+    assert not minimal_sensor_input.received_future.done()
+    minimal_sensor_output.publish()
+    ros_exec.spin_until_future_complete(minimal_sensor_input.received_future, timeout_sec=0.5)
+    assert minimal_sensor_input.received_future.done() and minimal_sensor_input.received_future.result()
+    for key in random_sensor.get_fields_and_field_types().keys():
+        assert getattr(random_sensor, key) == getattr(minimal_sensor_input.input, key)
+
+
 @pytest.mark.parametrize("minimal_cartesian_input", [[LifecycleComponent, "/topic"]], indirect=True)
 @pytest.mark.parametrize("minimal_joint_output", [[LifecycleComponent, "/topic", True]], indirect=True)
 def test_input_output_invalid_type(ros_exec, minimal_joint_output, minimal_cartesian_input):

source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp

@@ -7,10 +7,6 @@
 #include <controller_interface/helpers.hpp>
 #include <realtime_tools/realtime_buffer.h>
 #include <realtime_tools/realtime_publisher.h>
-#include <tf2_ros/buffer.h>
-#include <tf2_ros/static_transform_broadcaster.h>
-#include <tf2_ros/transform_broadcaster.h>
-#include <tf2_ros/transform_listener.h>
 
 #include <state_representation/parameters/ParameterMap.hpp>
 
@@ -314,73 +310,6 @@ class BaseControllerInterface : public controller_interface::ControllerInterface
       const std::string& service_name,
       const std::function<ControllerServiceResponse(const std::string& string)>& callback);
 
-  /**
-   * @brief Configure a transform buffer and listener.
-   */
-  void add_tf_listener();
-
-  /**
-   * @brief Look up a transform from TF.
-   * @param frame The desired frame of the transform
-   * @param reference_frame The desired reference frame of the transform
-   * @param time_point The time at which the value of the transform is desired
-   * @param duration How long to block the lookup call before failing
-   * @throws modulo_core::exceptions::LookupTransformException if TF buffer/listener are unconfigured or
-   * if the lookupTransform call failed
-   * @return If it exists, the requested transform
-   */
-  [[nodiscard]] state_representation::CartesianPose lookup_transform(
-      const std::string& frame, const std::string& reference_frame, const tf2::TimePoint& time_point,
-      const tf2::Duration& duration);
-
-  /**
-   * @brief Look up a transform from TF.
-   * @param frame The desired frame of the transform
-   * @param reference_frame The desired reference frame of the transform
-   * @param validity_period The validity period of the latest transform from the time of lookup in seconds
-   * @param duration How long to block the lookup call before failing
-   * @throws modulo_core::exceptions::LookupTransformException if TF buffer/listener are unconfigured,
-   * if the lookupTransform call failed, or if the transform is too old
-   * @return If it exists and is still valid, the requested transform
-   */
-  [[nodiscard]] state_representation::CartesianPose lookup_transform(
-      const std::string& frame, const std::string& reference_frame = "world", double validity_period = -1.0,
-      const tf2::Duration& duration = tf2::Duration(std::chrono::microseconds(10)));
-
-  /**
-   *@brief Configure a transform broadcaster.
-   */
-  void add_tf_broadcaster();
-
-  /**
-   * @brief Configure a static transform broadcaster.
-   */
-  void add_static_tf_broadcaster();
-
-  /**
-   * @brief Send a transform to TF.
-   * @param transform The transform to send
-   */
-  void send_transform(const state_representation::CartesianPose& transform);
-
-  /**
-   * @brief Send a vector of transforms to TF.
-   * @param transforms The vector of transforms to send
-   */
-  void send_transforms(const std::vector<state_representation::CartesianPose>& transforms);
-
-  /**
-   * @brief Send a static transform to TF.
-   * @param transform The transform to send
-   */
-  void send_static_transform(const state_representation::CartesianPose& transform);
-
-  /**
-   * @brief Send a vector of static transforms to TF.
-   * @param transforms The vector of transforms to send
-   */
-  void send_static_transforms(const std::vector<state_representation::CartesianPose>& transforms);
-
   /**
    * @brief Getter of the Quality of Service attribute.
    * @return The Quality of Service attribute
@@ -405,12 +334,6 @@ class BaseControllerInterface : public controller_interface::ControllerInterface
    */
   std::timed_mutex& get_command_mutex();
 
-  /**
-   * @brief Check if the node has been initialized or not.
-   * @return True if the node is initialized, false otherwise
-   */
-  bool is_node_initialized() const;
-
 private:
   /**
    * @brief Parameter validation function
@@ -524,32 +447,6 @@ class BaseControllerInterface : public controller_interface::ControllerInterface
    */
   std::string validate_service_name(const std::string& service_name, const std::string& type) const;
 
-  /**
-   * @brief Helper method to look up a transform from TF.
-   * @param frame The desired frame of the transform
-   * @param reference_frame The desired reference frame of the transform
-   * @param time_point The time at which the value of the transform is desired
-   * @param duration How long to block the lookup call before failing
-   * @throws modulo_core::exceptions::LookupTransformException if TF buffer/listener are unconfigured or
-   * if the lookupTransform call failed
-   * @return If it exists, the requested transform
-   */
-  [[nodiscard]] geometry_msgs::msg::TransformStamped lookup_ros_transform(
-      const std::string& frame, const std::string& reference_frame, const tf2::TimePoint& time_point,
-      const tf2::Duration& duration);
-
-  /**
-   * @brief Helper function to send a vector of transforms through a transform broadcaster
-   * @tparam T The type of the broadcaster (tf2_ros::TransformBroadcaster or tf2_ros::StaticTransformBroadcaster)
-   * @param transforms The transforms to send
-   * @param tf_broadcaster A pointer to a configured transform broadcaster object
-   * @param is_static If true, treat the broadcaster as a static frame broadcaster for the sake of log messages
-   */
-  template<typename T>
-  void publish_transforms(
-      const std::vector<state_representation::CartesianPose>& transforms, const std::shared_ptr<T>& tf_broadcaster,
-      bool is_static = false);
-
   state_representation::ParameterMap parameter_map_;///< ParameterMap for handling parameters
   std::unordered_map<std::string, bool> read_only_parameters_;
   std::shared_ptr<rclcpp::node_interfaces::PreSetParametersCallbackHandle>
@@ -585,11 +482,6 @@ class BaseControllerInterface : public controller_interface::ControllerInterface
       custom_output_configuration_callables_;
   std::map<std::string, std::function<void(const std::string&, const std::string&)>>
       custom_input_configuration_callables_;
-
-  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;                                ///< TF buffer
-  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;                   ///< TF listener
-  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;             ///< TF broadcaster
-  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_tf_broadcaster_;///< TF static broadcaster
 };
 
 template<typename T>
@@ -975,33 +867,4 @@ inline void BaseControllerInterface::write_output(const std::string& name, const
   write_std_output<StringPublishers, std_msgs::msg::String, std::string>(name, data);
 }
 
-template<typename T>
-inline void BaseControllerInterface::publish_transforms(
-    const std::vector<state_representation::CartesianPose>& transforms, const std::shared_ptr<T>& tf_broadcaster,
-    bool is_static) {
-  if (!is_node_initialized()) {
-    throw modulo_core::exceptions::CoreException("Failed send transform(s): Node is not initialized yet.");
-  }
-  std::string modifier = is_static ? "static " : "";
-  if (tf_broadcaster == nullptr) {
-    RCLCPP_ERROR_STREAM_THROTTLE(
-        this->get_node()->get_logger(), *this->get_node()->get_clock(), 1000,
-        "Failed to send " << modifier << "transform: No " << modifier << "TF broadcaster configured.");
-    return;
-  }
-  try {
-    std::vector<geometry_msgs::msg::TransformStamped> transform_messages;
-    transform_messages.reserve(transforms.size());
-    for (const auto& tf : transforms) {
-      geometry_msgs::msg::TransformStamped transform_message;
-      modulo_core::translators::write_message(transform_message, tf, this->get_node()->get_clock()->now());
-      transform_messages.emplace_back(transform_message);
-    }
-    tf_broadcaster->sendTransform(transform_messages);
-  } catch (const std::exception& ex) {
-    RCLCPP_ERROR_STREAM_THROTTLE(
-        this->get_node()->get_logger(), *this->get_node()->get_clock(), 1000,
-        "Failed to send " << modifier << "transform: " << ex.what());
-  }
-}
 }// namespace modulo_controllers

source/modulo_controllers/package.xml

@@ -11,7 +11,6 @@
 
   <depend>controller_interface</depend>
   <depend>hardware_interface</depend>
-  <depend>kdl_parser</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_lifecycle</depend>

source/modulo_controllers/src/BaseControllerInterface.cpp

@@ -2,13 +2,9 @@
 
 #include <chrono>
 
-#include <console_bridge/console.h>
-
 #include <lifecycle_msgs/msg/state.hpp>
 
-#include <modulo_core/exceptions.hpp>
 #include <modulo_core/translators/message_readers.hpp>
-#include <stdexcept>
 
 template<class... Ts>
 struct overloaded : Ts... {
@@ -543,101 +539,6 @@ void BaseControllerInterface::add_service(
   }
 }
 
-void BaseControllerInterface::add_tf_listener() {
-  if (!is_node_initialized()) {
-    throw modulo_core::exceptions::CoreException("Failed to add TF buffer and listener: Node is not initialized yet.");
-  }
-  if (this->tf_buffer_ == nullptr || this->tf_listener_ == nullptr) {
-    RCLCPP_DEBUG(this->get_node()->get_logger(), "Adding TF buffer and listener.");
-    console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE);
-    this->tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_node()->get_clock());
-    this->tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*this->tf_buffer_);
-  } else {
-    RCLCPP_DEBUG(this->get_node()->get_logger(), "TF buffer and listener already exist.");
-  }
-}
-
-state_representation::CartesianPose BaseControllerInterface::lookup_transform(
-    const std::string& frame, const std::string& reference_frame, const tf2::TimePoint& time_point,
-    const tf2::Duration& duration) {
-  auto transform = this->lookup_ros_transform(frame, reference_frame, time_point, duration);
-  state_representation::CartesianPose result(frame, reference_frame);
-  translators::read_message(result, transform);
-  return result;
-}
-
-state_representation::CartesianPose BaseControllerInterface::lookup_transform(
-    const std::string& frame, const std::string& reference_frame, double validity_period,
-    const tf2::Duration& duration) {
-  auto transform =
-      this->lookup_ros_transform(frame, reference_frame, tf2::TimePoint(std::chrono::microseconds(0)), duration);
-  if (validity_period > 0.0
-      && (this->get_node()->get_clock()->now() - transform.header.stamp).seconds() > validity_period) {
-    throw modulo_core::exceptions::LookupTransformException("Failed to lookup transform: Latest transform is too old!");
-  }
-  state_representation::CartesianPose result(frame, reference_frame);
-  translators::read_message(result, transform);
-  return result;
-}
-
-geometry_msgs::msg::TransformStamped BaseControllerInterface::lookup_ros_transform(
-    const std::string& frame, const std::string& reference_frame, const tf2::TimePoint& time_point,
-    const tf2::Duration& duration) {
-  if (this->tf_buffer_ == nullptr || this->tf_listener_ == nullptr) {
-    throw modulo_core::exceptions::LookupTransformException(
-        "Failed to lookup transform: No TF buffer / listener configured.");
-  }
-  try {
-    return this->tf_buffer_->lookupTransform(reference_frame, frame, time_point, duration);
-  } catch (const tf2::TransformException& ex) {
-    throw modulo_core::exceptions::LookupTransformException(
-        std::string("Failed to lookup transform: ").append(ex.what()));
-  }
-}
-
-void BaseControllerInterface::add_tf_broadcaster() {
-  if (!is_node_initialized()) {
-    throw modulo_core::exceptions::CoreException("Failed to add TF broadcaster: Node is not initialized yet.");
-  }
-  if (this->tf_broadcaster_ == nullptr) {
-    RCLCPP_DEBUG(this->get_node()->get_logger(), "Adding TF broadcaster.");
-    console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE);
-    this->tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(get_node());
-  } else {
-    RCLCPP_DEBUG(this->get_node()->get_logger(), "TF broadcaster already exists.");
-  }
-}
-
-void BaseControllerInterface::add_static_tf_broadcaster() {
-  if (!is_node_initialized()) {
-    throw modulo_core::exceptions::CoreException("Failed to add static TF broadcaster: Node is not initialized yet.");
-  }
-  if (this->static_tf_broadcaster_ == nullptr) {
-    RCLCPP_DEBUG(this->get_node()->get_logger(), "Adding static TF broadcaster.");
-    console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE);
-    this->static_tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(get_node());
-  } else {
-    RCLCPP_DEBUG(this->get_node()->get_logger(), "Static TF broadcaster already exists.");
-  }
-}
-
-void BaseControllerInterface::send_transform(const state_representation::CartesianPose& transform) {
-  this->send_transforms(std::vector<state_representation::CartesianPose>{transform});
-}
-
-void BaseControllerInterface::send_transforms(const std::vector<state_representation::CartesianPose>& transforms) {
-  this->publish_transforms(transforms, this->tf_broadcaster_);
-}
-
-void BaseControllerInterface::send_static_transform(const state_representation::CartesianPose& transform) {
-  this->send_static_transforms(std::vector<state_representation::CartesianPose>{transform});
-}
-
-void BaseControllerInterface::send_static_transforms(
-    const std::vector<state_representation::CartesianPose>& transforms) {
-  this->publish_transforms(transforms, this->static_tf_broadcaster_, true);
-}
-
 rclcpp::QoS BaseControllerInterface::get_qos() const {
   return qos_;
 }
@@ -654,13 +555,4 @@ std::timed_mutex& BaseControllerInterface::get_command_mutex() {
   return command_mutex_;
 }
 
-bool BaseControllerInterface::is_node_initialized() const {
-  try {
-    get_node();
-    return true;
-  } catch (const std::runtime_error&) {
-    return false;
-  }
-}
-
 }// namespace modulo_controllers