Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Simplified logic in turtle_tf2_listener #75

Open
wants to merge 1 commit into
base: rolling
Choose a base branch
from
Open

Simplified logic in turtle_tf2_listener #75

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
144 changes: 61 additions & 83 deletions turtle_tf2_cpp/src/turtle_tf2_listener.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::string>("target_frame", "turtle1");

tf_buffer_ =
std::make_unique<tf2_ros::Buffer>(this->get_clock());
tf_listener_ =
std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

// Create a client to spawn a turtle
spawner_ =
this->create_client<turtlesim::srv::Spawn>("spawn");
spawner_ = this->create_client<turtlesim::srv::Spawn>("spawn");

// Create turtle2 velocity publisher
publisher_ =
this->create_publisher<geometry_msgs::msg::Twist>("turtle2/cmd_vel", 1);
publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("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<turtlesim::srv::Spawn::Request>();
request->name = "turtle2";
request->x = 4.0;
request->y = 2.0;
request->theta = 0.0;

// Call request
using ServiceResponseFuture =
rclcpp::Client<turtlesim::srv::Spawn>::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<turtlesim::srv::Spawn::Request>();
request->name = "turtle2";
request->x = 4.0;
request->y = 2.0;
request->theta = 0.0;

// Call request
using ServiceResponseFuture =
rclcpp::Client<turtlesim::srv::Spawn>::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<turtlesim::srv::Spawn>::SharedPtr spawner_{nullptr};
rclcpp::TimerBase::SharedPtr timer_{nullptr};
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_{nullptr};
Expand All @@ -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<FrameListener>());
Expand Down