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

add ability to arm

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

8
src/test_controls/test_controls/test_controller.py
View File

@@ -7,6 +7,7 @@ import asyncio
from drone_services.srv import SetAttitude
from drone_services.srv import SetTrajectory
from drone_services.srv import SetVehicleControl
from std_msgs.msg import Empty
class TestController(Node):
@@ -28,7 +29,9 @@ class TestController(Node):
self.vehicle_control_req = SetVehicleControl.Request()
self.traj_req = SetTrajectory.Request()
self.get_logger().info("Controls:1 - Attitude control\n2 - Velocity control\n3 - Position control\nW - forward\nS - backward\nA - left\nD - right\nQ - rotate left\nE - rotate right\nSpace - up\nZ - down\nV - Down nudge\nF - Up nudge\nN - emergency stop\nEsc - exit")
self.arm_publisher = self.create_publisher(Empty, '/drone/arm', 10)
self.get_logger().info("Controls:1 - Attitude control\n2 - Velocity control\n3 - Position control\n/ - Arm drone\nW - forward\nS - backward\nA - left\nD - right\nQ - rotate left\nE - rotate right\nSpace - up\nZ - down\nV - Down nudge\nF - Up nudge\nN - emergency stop\nEsc - exit")
def spin(self):
while rclpy.ok():
@@ -222,6 +225,9 @@ class TestController(Node):
if key == '1':
self.get_logger().info('position control')
self.control_mode = 3
if key == '/':
self.get_logger().info('arming')
self.arm_publisher.publish(Empty())
except Exception as e:
self.get_logger().error(str(e))