wrench_transformer.hpp

// Copyright (c) 2025, ros2_control development team
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
/*
 * Authors: Julia Jia
 */
 
#ifndef FORCE_TORQUE_SENSOR_BROADCASTER__WRENCH_TRANSFORMER_HPP_
#define FORCE_TORQUE_SENSOR_BROADCASTER__WRENCH_TRANSFORMER_HPP_
 
#include <memory>
#include <string>
#include <unordered_map>
#include <vector>
 
#include "geometry_msgs/msg/wrench_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/buffer.hpp"
#include "tf2_ros/transform_listener.hpp"
 
// auto-generated by generate_parameter_library
#include "force_torque_sensor_broadcaster/wrench_transformer_parameters.hpp"
 
namespace force_torque_sensor_broadcaster
{
 
class WrenchTransformer : public rclcpp::Node
{
public:
  explicit WrenchTransformer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
 
  void init();
 
  ~WrenchTransformer() = default;
 
private:
  void wrench_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg);
  bool transform_wrench(
    const geometry_msgs::msg::WrenchStamped & input_wrench, const std::string & target_frame,
    geometry_msgs::msg::WrenchStamped & output_wrench);
 
  void setup_subscriber();
  void setup_publishers();
 
  std::string normalize_namespace_for_topics() const;
 
  std::shared_ptr<force_torque_wrench_transformer::ParamListener> param_listener_;
  force_torque_wrench_transformer::Params params_;
 
  rclcpp::Subscription<geometry_msgs::msg::WrenchStamped>::SharedPtr wrench_subscriber_;
  std::unordered_map<std::string, rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>::SharedPtr>
    transformed_wrench_publishers_;
 
  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
 
  std::string input_topic_;
  std::string output_topic_suffix_;  // e.g., "" or "_filtered" based on input topic
  std::vector<std::string> target_frames_;
};
 
// Function to run wrench transformer (extracted from main for testability)
int run_wrench_transformer(int argc, char ** argv);
 
}  // namespace force_torque_sensor_broadcaster
 
#endif  // FORCE_TORQUE_SENSOR_BROADCASTER__WRENCH_TRANSFORMER_HPP_