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

change values that don't need to be changed to NaNA

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-15 11:23:36 +02:00
parent 5afec281a2
commit 1ba1d21487
2 changed files with 24 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -296,6 +296,7 @@ private:
msg.yawspeed = DEFAULT_YAW_SPEED; msg.yawspeed = DEFAULT_YAW_SPEED;
msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
trajectory_setpoint_publisher->publish(msg); trajectory_setpoint_publisher->publish(msg);
} }
/** /**

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

@@ -3,6 +3,7 @@ from rclpy.node import Node
from sshkeyboard import listen_keyboard_manual from sshkeyboard import listen_keyboard_manual
import asyncio import asyncio
from numpy import NAN
from drone_services.srv import SetAttitude from drone_services.srv import SetAttitude
from drone_services.srv import SetTrajectory from drone_services.srv import SetTrajectory
@@ -104,99 +105,99 @@ class TestController(Node):
self.send_attitude_request(pitch=0.0, yaw=0.0, self.send_attitude_request(pitch=0.0, yaw=0.0,
roll=0.0, thrust=0.05) roll=0.0, thrust=0.05)
elif (self.control_mode == CONTROL_MODE_VELOCITY): elif (self.control_mode == CONTROL_MODE_VELOCITY):
self.send_velocity_request(x=0.0, y=0.0, z=1.0, angle=0.0) self.send_velocity_request(x=NAN, y=NAN, z=1.0, angle=NAN)
else: else:
self.send_position_request(x=0.0, y=0.0, z=1.0, angle=0.0) self.send_position_request(x=NAN, y=NAN, z=1.0, angle=NAN)
def move_right(self): def move_right(self):
if (self.control_mode == CONTROL_MODE_ATTITUDE): if (self.control_mode == CONTROL_MODE_ATTITUDE):
self.send_attitude_request(pitch=0.0, yaw=0.0, self.send_attitude_request(pitch=0.0, yaw=0.0,
roll=1.0, thrust=0.0) roll=1.0, thrust=0.0)
elif (self.control_mode == CONTROL_MODE_VELOCITY): elif (self.control_mode == CONTROL_MODE_VELOCITY):
self.send_velocity_request(x=1.0, y=0.0, z=0.0, angle=0.0) self.send_velocity_request(x=1.0, y=NAN, z=NAN, angle=NAN)
else: else:
self.send_position_request(x=1.0, y=0.0, z=0.0, angle=0.0) self.send_position_request(x=1.0, y=NAN, z=NAN, angle=NAN)
def move_down(self): def move_down(self):
if (self.control_mode == CONTROL_MODE_ATTITUDE): if (self.control_mode == CONTROL_MODE_ATTITUDE):
self.send_attitude_request(pitch=0.0, yaw=0.0, self.send_attitude_request(pitch=0.0, yaw=0.0,
roll=0.0, thrust=-0.05) roll=0.0, thrust=-0.05)
elif (self.control_mode == CONTROL_MODE_VELOCITY): elif (self.control_mode == CONTROL_MODE_VELOCITY):
self.send_velocity_request(x=0.0, y=0.0, z=-1.0, angle=0.0) self.send_velocity_request(x=NAN, y=NAN, z=-1.0, angle=NAN)
else: else:
self.send_position_request(x=0.0, y=0.0, z=-1.0, angle=0.0) self.send_position_request(x=NAN, y=NAN, z=-1.0, angle=NAN)
def move_left(self): def move_left(self):
if (self.control_mode == CONTROL_MODE_ATTITUDE): if (self.control_mode == CONTROL_MODE_ATTITUDE):
self.send_attitude_request(pitch=0.0, yaw=0.0, self.send_attitude_request(pitch=0.0, yaw=0.0,
roll=-1.0, thrust=0.0) roll=-1.0, thrust=0.0)
elif (self.control_mode == CONTROL_MODE_VELOCITY): elif (self.control_mode == CONTROL_MODE_VELOCITY):
self.send_velocity_request(x=-1.0, y=0.0, z=0.0, angle=0.0) self.send_velocity_request(x=-1.0, y=NAN, z=NAN, angle=NAN)
else: else:
self.send_position_request(x=-1.0, y=0.0, z=0.0, angle=0.0) self.send_position_request(x=-1.0, y=NAN, z=NAN, angle=NAN)
def rotate_right(self): def rotate_right(self):
if (self.control_mode == CONTROL_MODE_ATTITUDE): if (self.control_mode == CONTROL_MODE_ATTITUDE):
self.send_attitude_request(pitch=0.0, yaw=1.0, self.send_attitude_request(pitch=0.0, yaw=1.0,
roll=0.0, thrust=0.0) roll=0.0, thrust=0.0)
elif (self.control_mode == CONTROL_MODE_VELOCITY): elif (self.control_mode == CONTROL_MODE_VELOCITY):
self.send_velocity_request(x=0.0, y=0.0, z=0.0, angle=1.0) self.send_velocity_request(x=NAN, y=NAN, z=NAN, angle=1.0)
else: else:
self.send_position_request(x=0.0, y=0.0, z=0.0, angle=1.0) self.send_position_request(x=NAN, y=NAN, z=NAN, angle=1.0)
def rotate_left(self): def rotate_left(self):
if (self.control_mode == CONTROL_MODE_ATTITUDE): if (self.control_mode == CONTROL_MODE_ATTITUDE):
self.send_attitude_request(pitch=0.0, yaw=-1.0, self.send_attitude_request(pitch=0.0, yaw=-1.0,
roll=0.0, thrust=0.0) roll=0.0, thrust=0.0)
elif (self.control_mode == CONTROL_MODE_VELOCITY): elif (self.control_mode == CONTROL_MODE_VELOCITY):
self.send_velocity_request(x=0.0, y=0.0, z=0.0, angle=-1.0) self.send_velocity_request(x=NAN, y=NAN, z=NAN, angle=-1.0)
else: else:
self.send_position_request(x=0.0, y=0.0, z=0.0, angle=-1.0) self.send_position_request(x=NAN, y=NAN, z=NAN, angle=-1.0)
def move_forward(self): def move_forward(self):
if (self.control_mode == CONTROL_MODE_ATTITUDE): if (self.control_mode == CONTROL_MODE_ATTITUDE):
self.send_attitude_request(pitch=-1.0, yaw=0.0, self.send_attitude_request(pitch=-1.0, yaw=0.0,
roll=0.0, thrust=0.0) roll=0.0, thrust=0.0)
elif (self.control_mode == CONTROL_MODE_VELOCITY): elif (self.control_mode == CONTROL_MODE_VELOCITY):
self.send_velocity_request(x=0.0, y=1.0, z=0.0, angle=0.0) self.send_velocity_request(x=NAN, y=1.0, z=NAN, angle=NAN)
else: else:
self.send_position_request(x=0.0, y=1.0, z=0.0, angle=0.0) self.send_position_request(x=NAN, y=1.0, z=NAN, angle=NAN)
def move_backward(self): def move_backward(self):
if (self.control_mode == CONTROL_MODE_ATTITUDE): if (self.control_mode == CONTROL_MODE_ATTITUDE):
self.send_attitude_request(pitch=1.0, yaw=0.0, self.send_attitude_request(pitch=1.0, yaw=0.0,
roll=0.0, thrust=0.0) roll=0.0, thrust=0.0)
elif (self.control_mode == CONTROL_MODE_VELOCITY): elif (self.control_mode == CONTROL_MODE_VELOCITY):
self.send_velocity_request(x=0.0, y=-1.0, z=0.0, angle=0.0) self.send_velocity_request(x=NAN, y=-1.0, z=NAN, angle=NAN)
else: else:
self.send_position_request(x=0.0, y=-1.0, z=0.0, angle=0.0) self.send_position_request(x=NAN, y=-1.0, z=NAN, angle=NAN)
def stop(self): def stop(self):
if (self.control_mode == CONTROL_MODE_ATTITUDE): if (self.control_mode == CONTROL_MODE_ATTITUDE):
self.send_attitude_request(pitch=0.0, yaw=0.0, self.send_attitude_request(pitch=0.0, yaw=0.0,
roll=0.0, thrust=0.0) roll=0.0, thrust=0.0)
elif (self.control_mode == CONTROL_MODE_VELOCITY): elif (self.control_mode == CONTROL_MODE_VELOCITY):
self.send_velocity_request(x=0.0, y=0.0, z=0.0, angle=0.0) self.send_velocity_request(x=0.0, y=0.0, z=0.0, angle=NAN)
else: else:
self.send_position_request(x=0.0, y=0.0, z=0.0, angle=0.0) self.send_position_request(x=0.0, y=0.0, z=0.0, angle=NAN)
def move_up_little(self): def move_up_little(self):
if (self.control_mode == CONTROL_MODE_ATTITUDE): if (self.control_mode == CONTROL_MODE_ATTITUDE):
self.send_attitude_request(pitch=0.0, yaw=0.0, self.send_attitude_request(pitch=0.0, yaw=0.0,
roll=0.0, thrust=0.01) roll=0.0, thrust=0.01)
elif (self.control_mode == CONTROL_MODE_VELOCITY): elif (self.control_mode == CONTROL_MODE_VELOCITY):
self.send_velocity_request(x=0.0, y=0.0, z=0.5, angle=0.0) self.send_velocity_request(x=NAN, y=NAN, z=0.5, angle=NAN)
else: else:
self.send_position_request(x=0.0, y=0.0, z=1.0, angle=0.0) self.send_position_request(x=NAN, y=NAN, z=1.0, angle=NAN)
def move_down_little(self): def move_down_little(self):
if (self.control_mode == CONTROL_MODE_ATTITUDE): if (self.control_mode == CONTROL_MODE_ATTITUDE):
self.send_attitude_request(pitch=0.0, yaw=0.0, self.send_attitude_request(pitch=0.0, yaw=0.0,
roll=0.0, thrust=-0.01) roll=0.0, thrust=-0.01)
elif (self.control_mode == CONTROL_MODE_VELOCITY): elif (self.control_mode == CONTROL_MODE_VELOCITY):
self.send_velocity_request(x=0.0, y=0.0, z=-0.5, angle=0.0) self.send_velocity_request(x=NAN, y=NAN, z=-0.5, angle=NAN)
else: else:
self.send_position_request(x=0.0, y=0.0, z=-1.0, angle=0.0) self.send_position_request(x=NAN, y=NAN, z=-1.0, angle=NAN)
def on_press(self, key): def on_press(self, key):
try: try: