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

Compare commits

merge into: sem:position_changer
Branches Tags
sem:main sem:api sem:position_changer sem:stable_flight sem:gps_control sem:send_setpoints sem:beacons sem:build_on_laptop sem:build_on_pi_main sem:install_ros2_px4_bridge
...
pull from: sem:gps_control
Branches Tags
sem:main sem:api sem:position_changer sem:stable_flight sem:gps_control sem:send_setpoints sem:beacons sem:build_on_laptop sem:build_on_pi_main sem:install_ros2_px4_bridge

10 Commits
position_c ... gps_contro

Author SHA1 Message Date
Sem van der Hoeven
634a00a837 remove checking of nan 2023-05-15 15:47:45 +02:00
Sem van der Hoeven
da5ee7fc88 remove checking of nan 2023-05-15 15:46:36 +02:00
Sem van der Hoeven
d8305499ab add check for NAN 2023-05-15 15:41:48 +02:00
Sem van der Hoeven
7d316ce740 add only sending setpoint when new setpoint has arrived 2023-05-15 15:33:58 +02:00
Sem van der Hoeven
1dc9234400 log incoming request 2023-05-15 15:11:18 +02:00
Sem van der Hoeven
71ba5cb171 change trajectory message to separate x y and z values 2023-05-15 12:36:44 +02:00
Sem van der Hoeven
f6873d553e add checking for NaN when handling velocity 2023-05-15 11:28:05 +02:00
Sem van der Hoeven
1ba1d21487 change values that don't need to be changed to NaNA 2023-05-15 11:23:36 +02:00
Sem van der Hoeven
5afec281a2 try continuous sending of setpoints 2023-05-15 11:10:19 +02:00
Sem van der Hoeven
fa4af5b39f add logs of which setpoint is being sent 2023-05-15 11:02:30 +02:00
3 changed files with 117 additions and 44 deletions
Expand all files Collapse all files

5
src/drone_services/srv/SetTrajectory.srv
View File

@@ -1,7 +1,10 @@
# control mode velocity or position, refer to drone_services/msg/DroneControlMode.msg
int8 control_mode
# x, y, z values
float32[3] values
#float32[3] values
float32 x
float32 y
float32 z
float32 yaw
---
bool success

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

@@ -96,6 +96,11 @@ private:
float base_q[4] = {0, 0, 0, 0};
int base_q_amount = 0;
bool trajectory_valid_x = false;
bool trajectory_valid_y = false;
bool trajectory_valid_z = false;
bool trajectory_valid_yaw = false;
char current_control_mode = CONTROL_MODE_ATTITUDE; // start with attitude control
// result quaternion
@@ -156,24 +161,37 @@ private:
else
{
RCLCPP_INFO(this->get_logger(), "Got new trajectory setpoint with control mode: %d", request->control_mode);
// trajectory_valid_x = request->x != NAN;
// trajectory_valid_y = request->y != NAN;
// trajectory_valid_z = request->z != NAN;
// trajectory_valid_yaw = request->yaw != NAN;
if (request->control_mode == CONTROL_MODE_VELOCITY)
{
for (int i = 0; i < 3; i++)
{
velocity[i] += request->values[i];
}
RCLCPP_INFO(this->get_logger(), "Got new velocity setpoint. %f %f %f", velocity[0], velocity[1], velocity[2]);
RCLCPP_INFO(this->get_logger(), "Got new velocity setpoint. %f %f %f", request->x, request->y, request->z);
// if (trajectory_valid_x)
velocity[0] += request->x;
// if (trajectory_valid_y)
velocity[1] += request->y;
// if (trajectory_valid_z)
velocity[2] += request->z;
RCLCPP_INFO(this->get_logger(), "Set new velocity setpoint. %f %f %f", velocity[0], velocity[1], velocity[2]);
}
else if (request->control_mode == CONTROL_MODE_POSITION)
{
for (int i = 0; i < 3; i++)
{
position[i] = request->values[i];
}
RCLCPP_INFO(this->get_logger(), "Got new position setpoint: %f %f %f", position[0], position[1], position[2]);
RCLCPP_INFO(this->get_logger(), "Got new position setpoint. %f %f %f", request->x, request->y, request->z);
// if (trajectory_valid_x)
position[0] = request->x;
// if (trajectory_valid_y)
position[1] = request->y;
// if (trajectory_valid_z)
position[2] = request->z;
RCLCPP_INFO(this->get_logger(), "Set new position setpoint: %f %f %f", position[0], position[1], position[2]);
}
last_angle = request->yaw;
if (trajectory_valid_yaw)
last_angle = request->yaw;
new_setpoint = true;
RCLCPP_INFO(this->get_logger(), "Yaw: %f", last_angle);
response->success = true;
@@ -271,9 +289,21 @@ private:
{
auto msg = px4_msgs::msg::TrajectorySetpoint();
// if (trajectory_valid_x)
// {
msg.velocity[0] = velocity[0];
trajectory_valid_x = false;
// }
// if (trajectory_valid_y)
// {
msg.velocity[1] = velocity[1];
trajectory_valid_y = false;
// }
// if (trajectory_valid_z)
// {
msg.velocity[2] = D_SPEED(velocity[2]);
trajectory_valid_z = false;
// }
publish_trajectory_setpoint(msg);
}
@@ -282,17 +312,32 @@ private:
{
auto msg = px4_msgs::msg::TrajectorySetpoint();
for (int i = 0; i < 3; i++)
{
msg.position[i] = position[i];
}
// if (trajectory_valid_x)
// {
msg.position[0] = position[0];
trajectory_valid_x = false;
// }
// if (trajectory_valid_y)
// {
msg.position[1] = position[1];
trajectory_valid_y = false;
// }
// if (trajectory_valid_z)
// {
msg.position[2] = position[2];
trajectory_valid_z = false;
// }
publish_trajectory_setpoint(msg);
}
void publish_trajectory_setpoint(px4_msgs::msg::TrajectorySetpoint msg)
{
// if (trajectory_valid_yaw)
// {
msg.yaw = last_angle;
trajectory_valid_yaw = false;
// }
msg.yawspeed = DEFAULT_YAW_SPEED;
msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
trajectory_setpoint_publisher->publish(msg);
@@ -343,12 +388,12 @@ private:
}
if (current_control_mode == CONTROL_MODE_VELOCITY)
{
// RCLCPP_INFO(this->get_logger(), "Sending velocity setpoint");
RCLCPP_INFO(this->get_logger(), "velocity %f %f %f", velocity[0], velocity[1], velocity[2]);
send_velocity_setpoint();
}
else if (current_control_mode == CONTROL_MODE_POSITION)
{
// RCLCPP_INFO(this->get_logger(), "Sending position setpoint");
RCLCPP_INFO(this->get_logger(), "position %f %f %f", position[0], position[1], position[2]);
send_position_setpoint();
}
new_setpoint = false;

77
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
import asyncio
from numpy import NAN
from drone_services.srv import SetAttitude
from drone_services.srv import SetTrajectory
@@ -76,8 +77,20 @@ class TestController(Node):
def send_velocity_request(self, x, y, z, angle):
self.traj_req.control_mode = 2
self.traj_req.yaw = angle
self.traj_req.values = [x, y, z]
if (angle != NAN):
self.traj_req.yaw = angle
if (x != NAN):
self.traj_req.x = x
else:
self.traj_req.x = 0.0
if (y != NAN):
self.traj_req.y = y
else:
self.traj_req.y = 0.0
if (z != NAN):
self.traj_req.z = z
else:
self.traj_req.z = 0.0
self.get_logger().info('set request to %f %f %f %f' % (x, y, z, angle))
self.future = self.traj_cli.call_async(self.traj_req)
rclpy.spin_until_future_complete(self, self.future)
@@ -86,8 +99,20 @@ class TestController(Node):
def send_position_request(self, x, y, z, angle):
self.traj_req.control_mode = 3
self.traj_req.yaw = angle
self.traj_req.values = [x, y, z]
if (angle != NAN):
self.traj_req.yaw = angle
if (x != NAN):
self.traj_req.x = x
else:
self.traj_req.x = 0.0
if (y != NAN):
self.traj_req.y = y
else:
self.traj_req.y = 0.0
if (z != NAN):
self.traj_req.z = z
else:
self.traj_req.z = 0.0
self.get_logger().info('set request to %f %f %f %f' % (x, y, z, angle))
self.future = self.traj_cli.call_async(self.traj_req)
rclpy.spin_until_future_complete(self, self.future)
@@ -104,99 +129,99 @@ class TestController(Node):
self.send_attitude_request(pitch=0.0, yaw=0.0,
roll=0.0, thrust=0.05)
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:
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):
if (self.control_mode == CONTROL_MODE_ATTITUDE):
self.send_attitude_request(pitch=0.0, yaw=0.0,
roll=1.0, thrust=0.0)
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:
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):
if (self.control_mode == CONTROL_MODE_ATTITUDE):
self.send_attitude_request(pitch=0.0, yaw=0.0,
roll=0.0, thrust=-0.05)
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:
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):
if (self.control_mode == CONTROL_MODE_ATTITUDE):
self.send_attitude_request(pitch=0.0, yaw=0.0,
roll=-1.0, thrust=0.0)
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:
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):
if (self.control_mode == CONTROL_MODE_ATTITUDE):
self.send_attitude_request(pitch=0.0, yaw=1.0,
roll=0.0, thrust=0.0)
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:
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):
if (self.control_mode == CONTROL_MODE_ATTITUDE):
self.send_attitude_request(pitch=0.0, yaw=-1.0,
roll=0.0, thrust=0.0)
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:
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):
if (self.control_mode == CONTROL_MODE_ATTITUDE):
self.send_attitude_request(pitch=-1.0, yaw=0.0,
roll=0.0, thrust=0.0)
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:
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):
if (self.control_mode == CONTROL_MODE_ATTITUDE):
self.send_attitude_request(pitch=1.0, yaw=0.0,
roll=0.0, thrust=0.0)
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:
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):
if (self.control_mode == CONTROL_MODE_ATTITUDE):
self.send_attitude_request(pitch=0.0, yaw=0.0,
roll=0.0, thrust=0.0)
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:
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):
if (self.control_mode == CONTROL_MODE_ATTITUDE):
self.send_attitude_request(pitch=0.0, yaw=0.0,
roll=0.0, thrust=0.01)
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:
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):
if (self.control_mode == CONTROL_MODE_ATTITUDE):
self.send_attitude_request(pitch=0.0, yaw=0.0,
roll=0.0, thrust=-0.01)
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:
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):
try: