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

change setting thrust and angles

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-03 15:34:57 +02:00
parent db1322e2de
commit adac3eb022
2 changed files with 19 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
src/px4_connection/src/px4_controller.cpp
View File

@@ -83,18 +83,22 @@ private:
{ {
if (armed) if (armed)
{ {
if (request->yaw == 0 && request->pitch = 0 && request->roll = 0) if (request->yaw == 0 && request->pitch = 0 && request->roll = 0 && request->thrust = 0)
{ {
last_setpoint[0] = degrees_to_radians(request->yaw); last_setpoint[0] = degrees_to_radians(request->yaw);
last_setpoint[1] = degrees_to_radians(request->pitch); last_setpoint[1] = degrees_to_radians(request->pitch);
last_setpoint[2] = degrees_to_radians(request->roll); last_setpoint[2] = degrees_to_radians(request->roll);
last_thrust = request->thrust;
} else { } else {
last_setpoint[0] += degrees_to_radians(request->yaw); last_setpoint[0] += degrees_to_radians(request->yaw);
last_setpoint[1] += degrees_to_radians(request->pitch); last_setpoint[1] += degrees_to_radians(request->pitch);
last_setpoint[2] += degrees_to_radians(request->roll); last_setpoint[2] += degrees_to_radians(request->roll);
last_thrust += request->thrust;
if (last_thrust > 1)
last_thrust = 1;
else if (last_thrust < 0)
last_thrust = 0;
} }
last_thrust = request->thrust;
RCLCPP_INFO(this->get_logger(), "New setpoint: yaw:%f pitch:%f roll:%f thrust:%f", last_setpoint[0], last_setpoint[1], last_setpoint[2], last_thrust); RCLCPP_INFO(this->get_logger(), "New setpoint: yaw:%f pitch:%f roll:%f thrust:%f", last_setpoint[0], last_setpoint[1], last_setpoint[2], last_thrust);

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

@@ -11,9 +11,9 @@ class TestController(Node):
def __init__(self): def __init__(self):
super().__init__('test_controller') super().__init__('test_controller')
# self.cli = self.create_client(SetAttitude, 'drone/set_attitude') self.cli = self.create_client(SetAttitude, 'drone/set_attitude')
# while not self.cli.wait_for_service(timeout_sec=1.0): while not self.cli.wait_for_service(timeout_sec=1.0):
# self.get_logger().info('service not available, waiting again...') self.get_logger().info('service not available, waiting again...')
self.req = SetAttitude.Request() self.req = SetAttitude.Request()
self.get_logger().info("Controls:\nW - forward\nS - backward\nA - left\nD - right\nQ - rotate left\nE - rotate right\nSpace - up\nShift - down\nEsc - exit") self.get_logger().info("Controls:\nW - forward\nS - backward\nA - left\nD - right\nQ - rotate left\nE - rotate right\nSpace - up\nShift - down\nEsc - exit")
@@ -45,16 +45,22 @@ class TestController(Node):
# self.get_logger().info('pressed ' + char) # self.get_logger().info('pressed ' + char)
if char == 'w': if char == 'w':
self.get_logger().info('forward') self.get_logger().info('forward')
self.send_request(pitch=-10, yaw=0,roll=0, thrust=0)
if char == 's': if char == 's':
self.get_logger().info('backward') self.get_logger().info('backward')
self.send_request(pitch=10, yaw=0,roll=0, thrust=0)
if char == 'a': if char == 'a':
self.get_logger().info('left') self.get_logger().info('left')
self.send_request(pitch=0, yaw=0,roll=-10, thrust=0)
if char == 'd': if char == 'd':
self.get_logger().info('right') self.get_logger().info('right')
self.send_request(pitch=0, yaw=0,roll=10, thrust=0)
if char == 'q': if char == 'q':
self.get_logger().info('rotate left') self.get_logger().info('rotate left')
self.send_request(pitch=0, yaw=-10,roll=0, thrust=0)
if char == 'e': if char == 'e':
self.get_logger().info('rotate right') self.get_logger().info('rotate right')
self.send_request(pitch=0, yaw=10,roll=0, thrust=0)
else: else:
try: try:
# known keys like spacebar, ctrl # known keys like spacebar, ctrl
@@ -67,8 +73,11 @@ class TestController(Node):
# self.get_logger().info('pressed {} ({})'.format(name, vk)) # self.get_logger().info('pressed {} ({})'.format(name, vk))
if vk == 32: if vk == 32:
self.get_logger().info('up') self.get_logger().info('up')
self.send_request(pitch=0, yaw=0,roll=0, thrust=0.1)
if vk == 65505: if vk == 65505:
self.get_logger().info('down') self.get_logger().info('down')
self.send_request(pitch=0, yaw=0,roll=0, thrust=-0.1)
except Exception as e: except Exception as e:
self.get_logger().error(str(e)) self.get_logger().error(str(e))
raise raise