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

add ready drone to api listener

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-06-06 16:42:22 +02:00
parent 41209ceab0
commit b9270f7745
2 changed files with 20 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
src/api_communication/api_communication/api_listener.py
View File

@@ -8,7 +8,7 @@ from drone_services.srv import MovePosition
from drone_services.srv import EnableFailsafe from drone_services.srv import EnableFailsafe
from drone_services.srv import ArmDrone from drone_services.srv import ArmDrone
from drone_services.srv import DisarmDrone from drone_services.srv import DisarmDrone
from drone_services.srv import SetAttitude from drone_services.srv import ReadyDrone
import asyncio import asyncio
import websockets.server import websockets.server
@@ -94,6 +94,10 @@ class ApiListener(Node):
self.wait_for_service(self.disarm_drone_client, "Disarm drone") self.wait_for_service(self.disarm_drone_client, "Disarm drone")
self.disarm_drone_request = DisarmDrone.Request() self.disarm_drone_request = DisarmDrone.Request()
self.ready_drone_client = self.create_client(ReadyDrone, "/drone/ready")
self.wait_for_service(self.ready_drone_client, "Ready drone")
self.ready_drone_request = ReadyDrone.Request()
self.status_data = {} self.status_data = {}
self.status_data_received = False self.status_data_received = False
self.last_message = "" self.last_message = ""
@@ -365,8 +369,20 @@ class ApiListener(Node):
future.add_done_callback(partial(self.disarm_service_callback)) future.add_done_callback(partial(self.disarm_service_callback))
else: else:
self.get_logger().info('Arm command received') self.get_logger().info('Arm command received')
future = self.arm_drone_client.call_async(self.arm_drone_request) future = self.ready_drone_client.call_async(
future.add_done_callback(partial(self.arm_service_callback)) self.ready_drone_request)
future.add_done_callback(partial(self.ready_drone_callback))
def ready_drone_callback(self, future):
try:
result = future.result()
if result.success:
self.get_logger().info('Ready service call success')
else:
self.get_logger().error('Ready service call failed')
except Exception as e:
self.get_logger().error(
'Something went wrong while calling the ready service!\n' + str(e))
def arm_service_callback(self, future): def arm_service_callback(self, future):
try: try:

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

@@ -52,7 +52,7 @@ public:
// vehicle_local_position_subscription_ = this->create_subscription<px4_msgs::msg::VehicleLocalPosition>("/fmu/out/vehicle_local_position", qos, std::bind(&PX4Controller::on_local_position_receive, this, std::placeholders::_1)); // vehicle_local_position_subscription_ = this->create_subscription<px4_msgs::msg::VehicleLocalPosition>("/fmu/out/vehicle_local_position", qos, std::bind(&PX4Controller::on_local_position_receive, this, std::placeholders::_1));
this->odometry_subscription = this->create_subscription<px4_msgs::msg::VehicleOdometry>("/fmu/out/vehicle_odometry", qos, std::bind(&PositionChanger::handle_odometry_message, this, std::placeholders::_1)); this->odometry_subscription = this->create_subscription<px4_msgs::msg::VehicleOdometry>("/fmu/out/vehicle_odometry", qos, std::bind(&PositionChanger::handle_odometry_message, this, std::placeholders::_1));
this->move_position_service = this->create_service<drone_services::srv::MovePosition>("/drone/move_position", std::bind(&PositionChanger::handle_move_position_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); this->move_position_service = this->create_service<drone_services::srv::MovePosition>("/drone/move_position", std::bind(&PositionChanger::handle_move_position_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
this->ready_drone_service = this->create_service<drone_services::srv::ReadyDrone>("/drone/ready_drone", std::bind(&PositionChanger::handle_ready_drone_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); this->ready_drone_service = this->create_service<drone_services::srv::ReadyDrone>("/drone/ready", std::bind(&PositionChanger::handle_ready_drone_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
this->failsafe_publisher = this->create_publisher<drone_services::msg::FailsafeMsg>("/drone/failsafe", 10); this->failsafe_publisher = this->create_publisher<drone_services::msg::FailsafeMsg>("/drone/failsafe", 10);