From 71e36f7b083b42b1468c6cf8398e7ad22a4c59f2 Mon Sep 17 00:00:00 2001 From: ozanhakantunca Date: Sat, 19 Oct 2024 17:49:03 +0300 Subject: [PATCH] convert object map tf server to c++ --- .../scripts/object_map_tf_server.cpp | 130 +++++++++++++++++ .../scripts/object_map_tf_server.py | 134 ------------------ 2 files changed, 130 insertions(+), 134 deletions(-) create mode 100755 auv_navigation/auv_mapping/scripts/object_map_tf_server.cpp delete mode 100755 auv_navigation/auv_mapping/scripts/object_map_tf_server.py diff --git a/auv_navigation/auv_mapping/scripts/object_map_tf_server.cpp b/auv_navigation/auv_mapping/scripts/object_map_tf_server.cpp new file mode 100755 index 00000000..06c8481f --- /dev/null +++ b/auv_navigation/auv_mapping/scripts/object_map_tf_server.cpp @@ -0,0 +1,130 @@ +#include +#include +#include +#include +#include +#include +#include + +class ObjectMapTFServer { +public: + ObjectMapTFServer() : nh_("~"), rate_(10.0) { + // Load parameters + nh_.param("static_frame", static_frame_, "odom"); + nh_.param("rate", rate_, 10.0); + + // Initialize the service + service_ = nh_.advertiseService("set_object_transform", &ObjectMapTFServer::handleSetTransform, this); + + // Initialize the transform listener and broadcaster + tf_listener_ = std::make_shared(); + ROS_INFO("ObjectMapTFServer initialized. Static frame: %s", static_frame_.c_str()); + } + + bool handleSetTransform(auv_msgs::SetObjectTransform::Request &req, + auv_msgs::SetObjectTransform::Response &res) { + std::string parent_frame = req.transform.header.frame_id; + std::string target_frame = req.transform.child_frame_id; + + try { + // Wait for the transform from static_frame to parent_frame + tf_listener_->waitForTransform(static_frame_, parent_frame, ros::Time(0), ros::Duration(4.0)); + tf::StampedTransform tf_transform; + tf_listener_->lookupTransform(static_frame_, parent_frame, ros::Time(0), tf_transform); + + // Convert the provided transform to the static frame + geometry_msgs::TransformStamped static_transform; + static_transform.header.stamp = ros::Time::now(); + static_transform.header.frame_id = static_frame_; + static_transform.child_frame_id = target_frame; + + // Parent to target transformation matrix + tf::Matrix3x3 parent_to_target( + tf::Quaternion(req.transform.transform.rotation.x, + req.transform.transform.rotation.y, + req.transform.transform.rotation.z, + req.transform.transform.rotation.w) + ); + tf::Vector3 parent_to_target_translation( + req.transform.transform.translation.x, + req.transform.transform.translation.y, + req.transform.transform.translation.z + ); + + // Static frame to parent transformation matrix + tf::Matrix3x3 static_to_parent(tf_transform.getRotation()); + tf::Vector3 static_to_parent_translation = tf_transform.getOrigin(); + + // Combined transformation: static frame to target + tf::Matrix3x3 static_to_target = static_to_parent * parent_to_target; + tf::Vector3 combined_translation = static_to_parent_translation + static_to_parent * parent_to_target_translation; + + // Extract translation and rotation + static_transform.transform.translation.x = combined_translation.x(); + static_transform.transform.translation.y = combined_translation.y(); + static_transform.transform.translation.z = combined_translation.z(); + + tf::Quaternion combined_rotation; + static_to_target.getRotation(combined_rotation); + static_transform.transform.rotation.x = combined_rotation.x(); + static_transform.transform.rotation.y = combined_rotation.y(); + static_transform.transform.rotation.z = combined_rotation.z(); + static_transform.transform.rotation.w = combined_rotation.w(); + + // Store or update the transform in the map + { + boost::mutex::scoped_lock lock(mutex_); + transforms_[target_frame] = static_transform; + } + + ROS_INFO("Stored static transform for frame: %s", target_frame.c_str()); + res.success = true; + res.message = "Stored transform for frame: " + target_frame; + return true; + } + catch (tf::TransformException &ex) { + ROS_ERROR("Error occurred while looking up transform: %s", ex.what()); + res.success = false; + res.message = "Failed to capture transform: " + std::string(ex.what()); + return false; + } + } + + void publishTransforms() { + ros::Rate rate(rate_); + while (ros::ok()) { + { + boost::mutex::scoped_lock lock(mutex_); + for (const auto &entry : transforms_) { + geometry_msgs::TransformStamped transform = entry.second; + transform.header.stamp = ros::Time::now(); + tf_broadcaster_.sendTransform(transform); + } + } + rate.sleep(); + } + } + +private: + ros::NodeHandle nh_; + ros::ServiceServer service_; + std::shared_ptr tf_listener_; + tf2_ros::TransformBroadcaster tf_broadcaster_; + boost::mutex mutex_; + std::unordered_map transforms_; + std::string static_frame_; + double rate_; +}; + +int main(int argc, char** argv) { + ros::init(argc, argv, "object_map_tf_server"); + ObjectMapTFServer server; + + ros::AsyncSpinner spinner(2); + spinner.start(); + + server.publishTransforms(); + ros::waitForShutdown(); + + return 0; +} diff --git a/auv_navigation/auv_mapping/scripts/object_map_tf_server.py b/auv_navigation/auv_mapping/scripts/object_map_tf_server.py deleted file mode 100755 index dccf7aad..00000000 --- a/auv_navigation/auv_mapping/scripts/object_map_tf_server.py +++ /dev/null @@ -1,134 +0,0 @@ -#!/usr/bin/env python3 - -import rospy -import tf -import tf2_ros -import threading -from geometry_msgs.msg import TransformStamped -from auv_msgs.srv import SetObjectTransform, SetObjectTransformResponse - - -class ObjectMapTFServer: - def __init__(self): - rospy.init_node("object_map_tf_server", anonymous=True) - - # Load parameters - self.static_frame = rospy.get_param("~static_frame", "odom") - self.update_rate = rospy.get_param("~rate", 10.0) # Hz - - # Transform storage - self.transforms = {} # Dictionary to store target frames and their transforms - - # Lock for thread-safe access to transforms - self.lock = threading.Lock() - - # ROS service to set object transform - self.service = rospy.Service( - "set_object_transform", SetObjectTransform, self.handle_set_transform - ) - - # Transform broadcaster - self.tf_broadcaster = tf2_ros.TransformBroadcaster() - - # Transform listener to get odom->base_link transform - self.tf_listener = tf.TransformListener() - - rospy.loginfo( - "ObjectMapTFServer initialized. Static frame: %s", self.static_frame - ) - - def handle_set_transform(self, req): - parent_frame = req.transform.header.frame_id - target_frame = req.transform.child_frame_id - - try: - # Wait for the transform from static_frame (odom) to parent_frame (base_link) - self.tf_listener.waitForTransform( - self.static_frame, parent_frame, rospy.Time(0), rospy.Duration(4.0) - ) - (trans, rot) = self.tf_listener.lookupTransform( - self.static_frame, parent_frame, rospy.Time(0) - ) - - # Convert the provided transform to the static frame - static_transform = TransformStamped() - static_transform.header.stamp = rospy.Time.now() - static_transform.header.frame_id = self.static_frame - static_transform.child_frame_id = target_frame - - # Parent to target transformation matrix - parent_to_target = tf.transformations.quaternion_matrix( - ( - req.transform.transform.rotation.x, - req.transform.transform.rotation.y, - req.transform.transform.rotation.z, - req.transform.transform.rotation.w, - ) - ) - parent_to_target[0:3, 3] = [ - req.transform.transform.translation.x, - req.transform.transform.translation.y, - req.transform.transform.translation.z, - ] - - # Static frame to parent transformation matrix - static_to_parent = tf.transformations.quaternion_matrix(rot) - static_to_parent[0:3, 3] = trans - - # Combined transformation: static frame to target - static_to_target = tf.transformations.concatenate_matrices( - static_to_parent, parent_to_target - ) - - # Extract translation and rotation - static_translation = tf.transformations.translation_from_matrix( - static_to_target - ) - static_rotation = tf.transformations.quaternion_from_matrix( - static_to_target - ) - - static_transform.transform.translation.x = static_translation[0] - static_transform.transform.translation.y = static_translation[1] - static_transform.transform.translation.z = static_translation[2] - static_transform.transform.rotation.x = static_rotation[0] - static_transform.transform.rotation.y = static_rotation[1] - static_transform.transform.rotation.z = static_rotation[2] - static_transform.transform.rotation.w = static_rotation[3] - - # Store or update the transform in the dictionary using the lock - with self.lock: - self.transforms[target_frame] = static_transform - - rospy.loginfo("Stored static transform for frame: %s", target_frame) - - return SetObjectTransformResponse( - success=True, message=f"Stored transform for frame: {target_frame}" - ) - - except ( - tf.LookupException, - tf.ConnectivityException, - tf.ExtrapolationException, - ) as e: - rospy.logerr("Error occurred while looking up transform: %s", str(e)) - return SetObjectTransformResponse( - success=False, message=f"Failed to capture transform: {str(e)}" - ) - - def publish_transforms(self): - rate = rospy.Rate(self.update_rate) - while not rospy.is_shutdown(): - with self.lock: - for transform in self.transforms.values(): - transform.header.stamp = rospy.Time.now() - self.tf_broadcaster.sendTransform(transform) - rate.sleep() - - -if __name__ == "__main__": - try: - server = ObjectMapTFServer() - server.publish_transforms() - except rospy.ROSInterruptException: - pass