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

add service callback

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-25 12:20:40 +02:00
parent 6ca7597703
commit 680b7a2792
1 changed files with 21 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -57,7 +57,8 @@ public:
this->vehicle_control_request = std::make_shared<drone_services::srv::SetVehicleControl::Request>();
this->vehicle_control_request->control = DEFAULT_CONTROL_MODE;
auto control_mode_response = this->vehicle_control_client->async_send_request(this->vehicle_control_request);
auto control_mode_response = this->vehicle_control_client->async_send_request(this->vehicle_control_request,
std::bind(&PositionChanger::vehicle_control_service_callback, this, std::placeholders::_1));
if (rclcpp::spin_until_future_complete(this, control_mode_response) ==
rclcpp::FutureReturnCode::SUCCESS)
@@ -69,6 +70,22 @@ public:
RCLCPP_ERROR(this->get_logger(), "Failed to call service to set vehicle control");
}
}
void vehicle_control_service_callback(rclcpp::Client<drone_services::srv::SetVehicleControl>::SharedFuture future)
{
auto status = future.wait_for(1s);
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);
}
else
{
RCLCPP_INFO(this->get_logger(), "Service In-Progress...");
}
}
void send_trajectory_message()
{
this->trajectory_request->x = this->current_speed_x;