From 69da977b9cd84fdd9236ef10776e5d395f332653 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 28 Jun 2024 13:09:50 +0200 Subject: [PATCH 1/2] Used turtlesim_msgs MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- turtle_tf2_cpp/CMakeLists.txt | 6 +++--- turtle_tf2_cpp/package.xml | 4 ++-- turtle_tf2_cpp/src/turtle_tf2_broadcaster.cpp | 8 ++++---- turtle_tf2_cpp/src/turtle_tf2_listener.cpp | 12 ++++++------ turtle_tf2_py/package.xml | 2 +- .../turtle_tf2_py/turtle_tf2_broadcaster.py | 2 +- turtle_tf2_py/turtle_tf2_py/turtle_tf2_listener.py | 2 +- .../turtle_tf2_py/turtle_tf2_message_broadcaster.py | 6 +++--- 8 files changed, 21 insertions(+), 21 deletions(-) diff --git a/turtle_tf2_cpp/CMakeLists.txt b/turtle_tf2_cpp/CMakeLists.txt index 6107836..8b537b7 100644 --- a/turtle_tf2_cpp/CMakeLists.txt +++ b/turtle_tf2_cpp/CMakeLists.txt @@ -21,7 +21,7 @@ find_package(rclcpp REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) -find_package(turtlesim REQUIRED) +find_package(turtlesim_msgs REQUIRED) if(TARGET tf2_geometry_msgs::tf2_geometry_msgs) get_target_property(_include_dirs tf2_geometry_msgs::tf2_geometry_msgs INTERFACE_INCLUDE_DIRECTORIES) @@ -52,7 +52,7 @@ ament_target_dependencies( rclcpp tf2 tf2_ros - turtlesim + turtlesim_msgs ) add_executable(turtle_tf2_listener src/turtle_tf2_listener.cpp) @@ -62,7 +62,7 @@ ament_target_dependencies( rclcpp tf2 tf2_ros - turtlesim + turtlesim_msgs ) add_executable(fixed_frame_tf2_broadcaster src/fixed_frame_tf2_broadcaster.cpp) diff --git a/turtle_tf2_cpp/package.xml b/turtle_tf2_cpp/package.xml index 8d6f07a..738b412 100644 --- a/turtle_tf2_cpp/package.xml +++ b/turtle_tf2_cpp/package.xml @@ -13,7 +13,7 @@ Shyngyskhan Abilkassov ament_cmake - + geometry_msgs launch launch_ros @@ -22,7 +22,7 @@ tf2 tf2_geometry_msgs tf2_ros - turtlesim + turtlesim_msgs ament_lint_auto ament_lint_common diff --git a/turtle_tf2_cpp/src/turtle_tf2_broadcaster.cpp b/turtle_tf2_cpp/src/turtle_tf2_broadcaster.cpp index 858a5ec..032baba 100644 --- a/turtle_tf2_cpp/src/turtle_tf2_broadcaster.cpp +++ b/turtle_tf2_cpp/src/turtle_tf2_broadcaster.cpp @@ -21,7 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "tf2/LinearMath/Quaternion.h" #include "tf2_ros/transform_broadcaster.h" -#include "turtlesim/msg/pose.hpp" +#include "turtlesim_msgs/msg/pose.hpp" class FramePublisher : public rclcpp::Node { @@ -42,7 +42,7 @@ class FramePublisher : public rclcpp::Node stream << "/" << turtlename_.c_str() << "/pose"; std::string topic_name = stream.str(); - auto handle_turtle_pose = [this](const std::shared_ptr msg) { + auto handle_turtle_pose = [this](const std::shared_ptr msg) { geometry_msgs::msg::TransformStamped t; // Read message content and assign it to @@ -70,13 +70,13 @@ class FramePublisher : public rclcpp::Node // Send the transformation tf_broadcaster_->sendTransform(t); }; - subscription_ = this->create_subscription( + subscription_ = this->create_subscription( topic_name, 10, handle_turtle_pose); } private: - rclcpp::Subscription::SharedPtr subscription_; + rclcpp::Subscription::SharedPtr subscription_; std::unique_ptr tf_broadcaster_; std::string turtlename_; }; diff --git a/turtle_tf2_cpp/src/turtle_tf2_listener.cpp b/turtle_tf2_cpp/src/turtle_tf2_listener.cpp index c83f5c5..92e3eb6 100644 --- a/turtle_tf2_cpp/src/turtle_tf2_listener.cpp +++ b/turtle_tf2_cpp/src/turtle_tf2_listener.cpp @@ -23,7 +23,7 @@ #include "tf2/exceptions.h" #include "tf2_ros/transform_listener.h" #include "tf2_ros/buffer.h" -#include "turtlesim/srv/spawn.hpp" +#include "turtlesim_msgs/srv/spawn.hpp" using namespace std::chrono_literals; @@ -45,7 +45,7 @@ class FrameListener : public rclcpp::Node // Create a client to spawn a turtle spawner_ = - this->create_client("spawn"); + this->create_client("spawn"); // Create turtle2 velocity publisher publisher_ = @@ -102,8 +102,8 @@ class FrameListener : public rclcpp::Node // Check if the service is ready if (spawner_->service_is_ready()) { // Initialize request with turtle name and coordinates - // Note that x, y and theta are defined as floats in turtlesim/srv/Spawn - auto request = std::make_shared(); + // Note that x, y and theta are defined as floats in turtlesim_msgs/srv/Spawn + auto request = std::make_shared(); request->x = 4.0; request->y = 2.0; request->theta = 0.0; @@ -111,7 +111,7 @@ class FrameListener : public rclcpp::Node // Call request using ServiceResponseFuture = - rclcpp::Client::SharedFuture; + rclcpp::Client::SharedFuture; auto response_received_callback = [this](ServiceResponseFuture future) { auto result = future.get(); if (strcmp(result->name.c_str(), "turtle2") == 0) { @@ -132,7 +132,7 @@ class FrameListener : public rclcpp::Node bool turtle_spawning_service_ready_; // if the turtle was successfully spawned bool turtle_spawned_; - rclcpp::Client::SharedPtr spawner_{nullptr}; + rclcpp::Client::SharedPtr spawner_{nullptr}; rclcpp::TimerBase::SharedPtr timer_{nullptr}; rclcpp::Publisher::SharedPtr publisher_{nullptr}; std::shared_ptr tf_listener_{nullptr}; diff --git a/turtle_tf2_py/package.xml b/turtle_tf2_py/package.xml index e2845af..532b12e 100644 --- a/turtle_tf2_py/package.xml +++ b/turtle_tf2_py/package.xml @@ -19,7 +19,7 @@ python3-numpy rclpy tf2_ros - turtlesim + turtlesim_msgs ament_copyright ament_flake8 diff --git a/turtle_tf2_py/turtle_tf2_py/turtle_tf2_broadcaster.py b/turtle_tf2_py/turtle_tf2_py/turtle_tf2_broadcaster.py index fe63510..797cc2b 100644 --- a/turtle_tf2_py/turtle_tf2_py/turtle_tf2_broadcaster.py +++ b/turtle_tf2_py/turtle_tf2_py/turtle_tf2_broadcaster.py @@ -23,7 +23,7 @@ from tf2_ros import TransformBroadcaster -from turtlesim.msg import Pose +from turtlesim_msgs.msg import Pose # This function is a stripped down version of the code in diff --git a/turtle_tf2_py/turtle_tf2_py/turtle_tf2_listener.py b/turtle_tf2_py/turtle_tf2_py/turtle_tf2_listener.py index 93af18d..14e57e8 100644 --- a/turtle_tf2_py/turtle_tf2_py/turtle_tf2_listener.py +++ b/turtle_tf2_py/turtle_tf2_py/turtle_tf2_listener.py @@ -94,7 +94,7 @@ def on_timer(self): else: if self.spawner.service_is_ready(): # Initialize request with turtle name and coordinates - # Note that x, y and theta are defined as floats in turtlesim/srv/Spawn + # Note that x, y and theta are defined as floats in turtlesim_msgs/srv/Spawn request = Spawn.Request() request.name = 'turtle2' request.x = 4.0 diff --git a/turtle_tf2_py/turtle_tf2_py/turtle_tf2_message_broadcaster.py b/turtle_tf2_py/turtle_tf2_py/turtle_tf2_message_broadcaster.py index c2ccbe3..594c7d3 100644 --- a/turtle_tf2_py/turtle_tf2_py/turtle_tf2_message_broadcaster.py +++ b/turtle_tf2_py/turtle_tf2_py/turtle_tf2_message_broadcaster.py @@ -18,8 +18,8 @@ import rclpy from rclpy.node import Node -from turtlesim.msg import Pose -from turtlesim.srv import Spawn +from turtlesim_msgs.msg import Pose +from turtlesim_msgs.srv import Spawn class PointPublisher(Node): @@ -53,7 +53,7 @@ def on_timer(self): else: if self.spawner.service_is_ready(): # Initialize request with turtle name and coordinates - # Note that x, y and theta are defined as floats in turtlesim/srv/Spawn + # Note that x, y and theta are defined as floats in turtlesim_msgs/srv/Spawn request = Spawn.Request() request.name = 'turtle3' request.x = 4.0 From 81e628de28ee7bf90642a83d869840cd5cf11816 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 10 Jul 2024 10:35:54 +0200 Subject: [PATCH 2/2] Removed deprecation MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- turtle_tf2_cpp/src/turtle_tf2_message_filter.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/turtle_tf2_cpp/src/turtle_tf2_message_filter.cpp b/turtle_tf2_cpp/src/turtle_tf2_message_filter.cpp index e21f609..2e8fbc3 100644 --- a/turtle_tf2_cpp/src/turtle_tf2_message_filter.cpp +++ b/turtle_tf2_cpp/src/turtle_tf2_message_filter.cpp @@ -52,7 +52,9 @@ class PoseDrawer : public rclcpp::Node tf2_listener_ = std::make_shared(*tf2_buffer_); - point_sub_.subscribe(this, "/turtle3/turtle_point_stamped"); + point_sub_.subscribe( + this, "/turtle3/turtle_point_stamped", + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default))); tf2_filter_ = std::make_shared>( point_sub_, *tf2_buffer_, target_frame_, 100, this->get_node_logging_interface(), this->get_node_clock_interface(), buffer_timeout);