convert object map tf server to c++ (#65)

auv_navigation/auv_mapping/CMakeLists.txt

@@ -1,11 +1,49 @@
 cmake_minimum_required(VERSION 3.10.2)
 project(auv_mapping)
 
-find_package(catkin REQUIRED)
+add_compile_options(-std=c++17)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+
+find_package(catkin REQUIRED
+  roscpp
+  tf2
+  tf2_ros
+  geometry_msgs
+  auv_msgs
+)
 
 catkin_package()
 
 include_directories(
-# include
-# ${catkin_INCLUDE_DIRS}
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+
+add_executable(object_map_tf_server_node
+  src/object_map_tf_server_node.cpp
+)
+
+# add_dependencies(object_map_tf_server_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg)
+
+target_link_libraries(object_map_tf_server_node
+  ${catkin_LIBRARIES}
+)
+
+install(TARGETS object_map_tf_server_node
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE
+)
+
+install(DIRECTORY
+  launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )

auv_navigation/auv_mapping/include/auv_mapping/object_map_tf_server_ros.hpp

@@ -0,0 +1,155 @@
+#pragma once
+
+#include <auv_msgs/SetObjectTransform.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <ros/ros.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <mutex>
+#include <optional>
+#include <unordered_map>
+
+namespace auv_mapping {
+class ObjectMapTFServerROS {
+  using TransformMap =
+      std::unordered_map<std::string, geometry_msgs::TransformStamped>;
+
+ public:
+  ObjectMapTFServerROS(const ros::NodeHandle &nh)
+      : nh_{nh},
+        tf_buffer_{},
+        tf_listener_{tf_buffer_},
+        rate_{10.0},
+        static_frame_{""},
+        transforms_{},
+        tf_broadcaster_{} {
+    auto node_handler_private = ros::NodeHandle{"~"};
+
+    node_handler_private.param<std::string>("static_frame", static_frame_,
+                                            "odom");
+    node_handler_private.param<double>("rate", rate_, 10.0);
+
+    service_ = nh_.advertiseService(
+        "set_object_transform", &ObjectMapTFServerROS::set_transform_handler,
+        this);
+
+    ROS_INFO("ObjectMapTFServerROS initialized. Static frame: %s",
+             static_frame_.c_str());
+  }
+
+  bool set_transform_handler(auv_msgs::SetObjectTransform::Request &req,
+                             auv_msgs::SetObjectTransform::Response &res) {
+    const auto parent_frame = req.transform.header.frame_id;
+    const auto target_frame = req.transform.child_frame_id;
+    const auto static_to_parent_transform =
+        get_transform(static_frame_, parent_frame, ros::Duration(4.0));
+
+    if (!static_to_parent_transform.has_value()) {
+      ROS_ERROR("Error occurred while looking up transform:(ozanhakantunca)");
+      res.success = false;
+      res.message = "Failed to capture transform TODO (ozanhakantunca)";
+      return false;
+    }
+
+    const auto parent_to_target_quaternion = tf2::Quaternion(
+        req.transform.transform.rotation.x, req.transform.transform.rotation.y,
+        req.transform.transform.rotation.z, req.transform.transform.rotation.w);
+
+    auto parent_to_target_orientation =
+        tf2::Matrix3x3{parent_to_target_quaternion};
+
+    auto parent_to_target_translation =
+        tf2::Vector3{req.transform.transform.translation.x,
+                     req.transform.transform.translation.y,
+                     req.transform.transform.translation.z};
+
+    auto static_to_parent_quaternion = tf2::Quaternion{};
+    tf2::fromMsg(static_to_parent_transform->transform.rotation,
+                 static_to_parent_quaternion);
+
+    const auto static_to_parent_orientation =
+        tf2::Matrix3x3{static_to_parent_quaternion};
+
+    const auto static_to_parent_translation =
+        tf2::Vector3{static_to_parent_transform->transform.translation.x,
+                     static_to_parent_transform->transform.translation.y,
+                     static_to_parent_transform->transform.translation.z};
+
+    const auto static_to_target_orientation = tf2::Matrix3x3{
+        static_to_parent_orientation * parent_to_target_orientation};
+
+    const auto static_to_target_translation = tf2::Vector3{
+        static_to_parent_translation +
+        static_to_parent_orientation * parent_to_target_translation};
+
+    auto static_transform = geometry_msgs::TransformStamped{};
+    static_transform.header.stamp = ros::Time::now();
+    static_transform.header.frame_id = static_frame_;
+    static_transform.child_frame_id = target_frame;
+
+    static_transform.transform.translation.x = static_to_target_translation.x();
+    static_transform.transform.translation.y = static_to_target_translation.y();
+    static_transform.transform.translation.z = static_to_target_translation.z();
+
+    auto static_to_target_quaternion = tf2::Quaternion{};
+    static_to_target_orientation.getRotation(static_to_target_quaternion);
+    static_transform.transform.rotation =
+        tf2::toMsg(static_to_target_quaternion);
+
+    {
+      auto lock = std::scoped_lock(mutex_);
+      transforms_[target_frame] = static_transform;
+    }
+
+    ROS_INFO_STREAM("Stored static transform from " << static_frame_ << " to "
+                                                    << target_frame);
+    res.success = true;
+    res.message = "Stored transform for frame: " + target_frame;
+    return true;
+  }
+
+  void publishTransforms() {
+    auto rate = ros::Rate{rate_};
+    while (ros::ok()) {
+      {
+        auto lock = std::scoped_lock(mutex_);
+
+        for (const auto &entry : transforms_) {
+          auto transform = entry.second;
+          transform.header.stamp = ros::Time::now();
+          tf_broadcaster_.sendTransform(transform);
+        }
+      }
+      rate.sleep();
+    }
+  }
+
+ private:
+  std::optional<geometry_msgs::TransformStamped> get_transform(
+      const std::string &target_frame, const std::string &source_frame,
+      const ros::Duration timeout) {
+    try {
+      auto transform = tf_buffer_.lookupTransform(target_frame, source_frame,
+                                                  ros::Time(0), timeout);
+      return transform;
+    } catch (tf2::TransformException &ex) {
+      ROS_WARN_STREAM("Transform lookup failed: " << ex.what());
+      return std::nullopt;
+    }
+  }
+
+  ros::NodeHandle nh_;
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+  double rate_;
+  std::string static_frame_;
+  TransformMap transforms_;
+  tf2_ros::TransformBroadcaster tf_broadcaster_;
+  //
+  std::mutex mutex_;
+  ros::ServiceServer service_;
+};
+
+}  // namespace auv_mapping

auv_navigation/auv_mapping/launch/start.launch

@@ -4,7 +4,7 @@
   <arg name="control_rate" default="10" />
 
   <group ns="$(arg namespace)">
-    <node pkg="auv_mapping" type="object_map_tf_server.py" name="object_map_tf_server">
+    <node pkg="auv_mapping" type="object_map_tf_server_node" name="object_map_tf_server_node">
       <param name="rate" value="$(arg control_rate)" />
       <param name="static_frame" value="odom" />
 
@@ -13,8 +13,4 @@
   </group>
 
   <node pkg="auv_control" type="prop_transform_publisher.py" name="prop_transform_publisher" output="screen" />
-
-  <!--
-  <node pkg="auv_control" type="bin_transform_publisher.py" name="bin_transform_publisher" />
-  </include> -->
 </launch>

auv_navigation/auv_mapping/package.xml

@@ -7,6 +7,10 @@
   <author email="senceryazici@gmail.com">Sencer Yazici</author>
   <license>BSD-3-Clause</license>
   <buildtool_depend>catkin</buildtool_depend>
+  <depend>roscpp</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>geometry_msgs</depend>
   <depend>auv_msgs</depend>
   <export>
 

auv_navigation/auv_mapping/src/object_map_tf_server_node.cpp

@@ -0,0 +1,15 @@
+#include "auv_mapping/object_map_tf_server_ros.hpp"
+
+int main(int argc, char **argv) {
+  ros::init(argc, argv, "object_map_tf_server");
+  auto node_handle = ros::NodeHandle{};
+  auto server = auv_mapping::ObjectMapTFServerROS{node_handle};
+
+  auto spinner = ros::AsyncSpinner{2};
+  spinner.start();
+
+  server.publishTransforms();
+  ros::waitForShutdown();
+
+  return 0;
+}

auv_software/package.xml

@@ -13,7 +13,6 @@
   <depend>auv_navigation</depend>
   <depend>auv_localization</depend>
   <depend>auv_vision</depend>
-
   <export>
 
   </export>