diff --git a/turtle_tf2_cpp/src/turtle_tf2_listener.cpp b/turtle_tf2_cpp/src/turtle_tf2_listener.cpp index 749f997..81a830f 100644 --- a/turtle_tf2_cpp/src/turtle_tf2_listener.cpp +++ b/turtle_tf2_cpp/src/turtle_tf2_listener.cpp @@ -31,107 +31,85 @@ class FrameListener : public rclcpp::Node { public: FrameListener() - : Node("turtle_tf2_frame_listener"), - turtle_spawning_service_ready_(false), - turtle_spawned_(false) + : Node("turtle_tf2_frame_listener") { // Declare and acquire `target_frame` parameter target_frame_ = this->declare_parameter("target_frame", "turtle1"); - tf_buffer_ = - std::make_unique(this->get_clock()); - tf_listener_ = - std::make_shared(*tf_buffer_); + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); // Create a client to spawn a turtle - spawner_ = - this->create_client("spawn"); + spawner_ = this->create_client("spawn"); // Create turtle2 velocity publisher - publisher_ = - this->create_publisher("turtle2/cmd_vel", 1); + publisher_ = this->create_publisher("turtle2/cmd_vel", 1); - // Call on_timer function every second - timer_ = this->create_wall_timer( - 1s, std::bind(&FrameListener::on_timer, this)); + // Call the spawn_turtle function immediately in another thread + std::thread t_([&]() + { this->spawn_turtle(); }); + t_.detach(); } private: - void on_timer() + void spawn_turtle() + { + auto request = std::make_shared(); + request->name = "turtle2"; + request->x = 4.0; + request->y = 2.0; + request->theta = 0.0; + + // Call request + using ServiceResponseFuture = + rclcpp::Client::SharedFuture; + auto response_received_callback = [this]( + ServiceResponseFuture future) + { + auto result = future.get(); + if (result->name != "turtle2") { + RCLCPP_ERROR(this->get_logger(), "Service callback result mismatch"); + return; + } + RCLCPP_ERROR(this->get_logger(), "Successfully spawned"); + start_timer(); + }; + spawner_->wait_for_service(); + spawner_->async_send_request(request, response_received_callback); + } + void start_timer() + { + // Call handle_transformations function every second + timer_ = this->create_wall_timer(1s, std::bind(&FrameListener::handle_transformations, this)); + } + + void handle_transformations() { - // Store frame names in variables that will be used to - // compute transformations std::string fromFrameRel = target_frame_.c_str(); std::string toFrameRel = "turtle2"; - if (turtle_spawning_service_ready_) { - if (turtle_spawned_) { - geometry_msgs::msg::TransformStamped t; - - // Look up for the transformation between target_frame and turtle2 frames - // and send velocity commands for turtle2 to reach target_frame - try { - t = tf_buffer_->lookupTransform( - toFrameRel, fromFrameRel, - tf2::TimePointZero); - } catch (const tf2::TransformException & ex) { - RCLCPP_INFO( - this->get_logger(), "Could not transform %s to %s: %s", - toFrameRel.c_str(), fromFrameRel.c_str(), ex.what()); - return; - } - - geometry_msgs::msg::Twist msg; - - static const double scaleRotationRate = 1.0; - msg.angular.z = scaleRotationRate * atan2( - t.transform.translation.y, - t.transform.translation.x); - - static const double scaleForwardSpeed = 0.5; - msg.linear.x = scaleForwardSpeed * sqrt( - pow(t.transform.translation.x, 2) + - pow(t.transform.translation.y, 2)); - - publisher_->publish(msg); - } else { - RCLCPP_INFO(this->get_logger(), "Successfully spawned"); - turtle_spawned_ = true; - } - } else { - // 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(); - request->name = "turtle2"; - request->x = 4.0; - request->y = 2.0; - request->theta = 0.0; - - // Call request - using ServiceResponseFuture = - rclcpp::Client::SharedFuture; - auto response_received_callback = [this](ServiceResponseFuture future) { - auto result = future.get(); - if (strcmp(result->name.c_str(), "turtle2") == 0) { - turtle_spawning_service_ready_ = true; - } else { - RCLCPP_ERROR(this->get_logger(), "Service callback result mismatch"); - } - }; - auto result = spawner_->async_send_request(request, response_received_callback); - } else { - RCLCPP_INFO(this->get_logger(), "Service is not ready"); - } + geometry_msgs::msg::TransformStamped t; + try { + t = tf_buffer_->lookupTransform(toFrameRel, fromFrameRel, tf2::TimePointZero); + } catch (const tf2::TransformException &ex) { + RCLCPP_INFO( + this->get_logger(), "Could not transform %s to %s: %s", + toFrameRel.c_str(), fromFrameRel.c_str(), ex.what()); + return; } + + geometry_msgs::msg::Twist msg; + + static const double scaleRotationRate = 1.0; + msg.angular.z = scaleRotationRate * atan2(t.transform.translation.y, t.transform.translation.x); + + static const double scaleForwardSpeed = 0.5; + msg.linear.x = scaleForwardSpeed * sqrt( + pow(t.transform.translation.x, 2) + pow(t.transform.translation.y, 2)); + + publisher_->publish(msg); } - // Boolean values to store the information - // if the service for spawning turtle is available - bool turtle_spawning_service_ready_; - // if the turtle was successfully spawned - bool turtle_spawned_; rclcpp::Client::SharedPtr spawner_{nullptr}; rclcpp::TimerBase::SharedPtr timer_{nullptr}; rclcpp::Publisher::SharedPtr publisher_{nullptr}; @@ -140,7 +118,7 @@ class FrameListener : public rclcpp::Node std::string target_frame_; }; -int main(int argc, char * argv[]) +int main(int argc, char *argv[]) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared());