From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Wed, 20 Oct 2021 18:47:23 +0000 (-0300)
Subject: Recover pose_stamped2tf.cpp and port it to Galactic.
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=2dd83cf296fbf193fe2a02d23a7d6088903161a3;p=trajectory_conversions.git

Recover pose_stamped2tf.cpp and port it to Galactic.
---

diff --git a/CMakeLists.txt b/CMakeLists.txt
index cce1fd2..d3e7f35 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -10,6 +10,7 @@ find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(tf2_msgs REQUIRED)
 find_package(trajectory_msgs REQUIRED)
 find_package(tf2_kdl REQUIRED)
 find_package(urdf REQUIRED)
@@ -33,7 +34,20 @@ ament_target_dependencies(
   "orocos_kdl"
 )
 
-install(TARGETS pose_stamped2joint
+add_executable(pose_stamped2tf src/pose_stamped2tf.cpp)
+target_include_directories(pose_stamped2tf PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+target_compile_features(pose_stamped2tf PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+ament_target_dependencies(
+  pose_stamped2tf
+  "rclcpp"
+  "geometry_msgs"
+  "tf2_msgs"
+  "tf2_kdl"
+)
+
+install(TARGETS pose_stamped2joint pose_stamped2tf
   DESTINATION lib/${PROJECT_NAME})
 
 if(BUILD_TESTING)
diff --git a/package.xml b/package.xml
index 903c0d4..431ef94 100644
--- a/package.xml
+++ b/package.xml
@@ -12,6 +12,7 @@
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>
   <depend>geometry_msgs</depend>
+  <depend>tf2_msgs</depend>
   <depend>trajectory_msgs</depend>
   <depend>urdf</depend>
   <depend>tf2_kdl</depend>
diff --git a/src/pose_stamped2tf.cpp b/src/pose_stamped2tf.cpp
new file mode 100644
index 0000000..03d4c5f
--- /dev/null
+++ b/src/pose_stamped2tf.cpp
@@ -0,0 +1,75 @@
+/******************************************************************************
+	               ROS 2 PoseStamped to tf Converter
+          Copyright (C) 2018, 2021 Walter Fetter Lages <w.fetter@ieee.org>
+
+        This program is free software: you can redistribute it and/or modify
+        it under the terms of the GNU General Public License as published by
+        the Free Software Foundation, either version 3 of the License, or
+        (at your option) any later version.
+
+        This program is distributed in the hope that it will be useful, but
+        WITHOUT ANY WARRANTY; without even the implied warranty of
+        MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+        General Public License for more details.
+
+        You should have received a copy of the GNU General Public License
+        along with this program.  If not, see
+        <http://www.gnu.org/licenses/>.
+        
+*******************************************************************************/
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <tf2_msgs/msg/tf_message.hpp>
+
+class PoseStampedToTf: public rclcpp::Node
+{
+	public:
+	PoseStampedToTf(const char *node_name,const char *child_frame_id);
+
+	private:
+	rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr poseSubscriber_;
+	rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr tfPublisher_;
+	std::string child_frame_id_;
+	void poseStampedCB(const geometry_msgs::msg::PoseStamped::SharedPtr poseMsg) const;
+};
+
+PoseStampedToTf::PoseStampedToTf(const char *node_name,const char *child_frame_id): Node(node_name)
+{
+	child_frame_id_=child_frame_id;
+	using std::placeholders::_1;
+	poseSubscriber_=create_subscription<geometry_msgs::msg::PoseStamped>("pose",10,std::bind(&PoseStampedToTf::poseStampedCB,this,_1));
+	tfPublisher_=create_publisher<tf2_msgs::msg::TFMessage>("/tf",10);
+}
+
+void PoseStampedToTf::poseStampedCB(const geometry_msgs::msg::PoseStamped::SharedPtr poseMsg) const
+{
+	tf2_msgs::msg::TFMessage tfMsg;
+
+	tfMsg.transforms.resize(1);
+	tfMsg.transforms[0].header.stamp=poseMsg->header.stamp;
+	tfMsg.transforms[0].header.frame_id=poseMsg->header.frame_id;
+	tfMsg.transforms[0].child_frame_id=child_frame_id_;
+	tfMsg.transforms[0].transform.translation.x=poseMsg->pose.position.x;
+	tfMsg.transforms[0].transform.translation.y=poseMsg->pose.position.y;
+	tfMsg.transforms[0].transform.translation.z=poseMsg->pose.position.z;
+	tfMsg.transforms[0].transform.rotation=poseMsg->pose.orientation;
+	
+        tfPublisher_->publish(tfMsg);
+}
+
+int main(int argc,char* argv[])
+{
+	rclcpp::init(argc,argv);
+	
+	if(argc < 2)
+	{
+		RCLCPP_ERROR_STREAM(rclcpp::get_logger("pose_stamped2tf"),"pose_stamped2tf: No child frame id.\n");
+		return -1;
+	}
+	
+	rclcpp::spin(std::make_shared<PoseStampedToTf>("pose_stamped2tf",argv[1]));
+	
+	return 0;
+}