sem/5g_drone_ROS2
1
0
Fork 0
Code Issues 1 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

add callback to trajectory message service call

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-25 12:25:06 +02:00
parent 06f833bda0
commit 8700014e11
1 changed files with 17 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
src/drone_controls/src/PositionChanger.cpp
View File

@@ -66,9 +66,6 @@ public:
auto status = future.wait_for(1s); auto status = future.wait_for(1s);
if (status == std::future_status::ready) if (status == std::future_status::ready)
{ {
// uncomment below line if using Empty() message
// RCLCPP_INFO(this->get_logger(), "Result: success");
// comment below line if using Empty() message
RCLCPP_INFO(this->get_logger(), "Vehicle control mode set result: %d", future.get()->success); RCLCPP_INFO(this->get_logger(), "Vehicle control mode set result: %d", future.get()->success);
} }
else else
@@ -76,13 +73,26 @@ public:
RCLCPP_INFO(this->get_logger(), "Service In-Progress..."); RCLCPP_INFO(this->get_logger(), "Service In-Progress...");
} }
} }
void trajectory_message_callback(rclcpp::Client<drone_services::srv::SetTrajectory>::SharedFuture future)
{
auto status = future.wait_for(1s);
if (status == std::future_status::ready)
{
RCLCPP_INFO(this->get_logger(), "Seet trajectory set result: %d", future.get()->success);
}
else
{
RCLCPP_INFO(this->get_logger(), "Service In-Progress...");
}
}
void send_trajectory_message() void send_trajectory_message()
{ {
this->trajectory_request->x = this->current_speed_x; this->trajectory_request->values[0] = this->current_speed_x;
this->trajectory_request->y = this->current_speed_y; this->trajectory_request->values[1] = this->current_speed_y;
this->trajectory_request->z = this->current_z_speed; this->trajectory_request->values[2] = this->current_z_speed;
this->trajectory_request->yaw = this->current_yaw; this->trajectory_request->yaw = this->current_yaw;
auto trajectory_response = this->trajectory_client->async_send_request(this->trajectory_request); auto trajectory_response = this->trajectory_client->async_send_request(this->trajectory_requeststd::bind(&PositionChanger::trajectory_message_callback, this, std::placeholders::_1));
if (rclcpp::spin_until_future_complete(this, trajectory_response) == if (rclcpp::spin_until_future_complete(this, trajectory_response) ==
rclcpp::FutureReturnCode::SUCCESS) rclcpp::FutureReturnCode::SUCCESS)