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

change trajectory message to separate x y and z values

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-15 12:36:44 +02:00
parent f6873d553e
commit 71ba5cb171
3 changed files with 78 additions and 26 deletions
Download Patch File Download Diff File 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,29 +161,35 @@ 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++)
{
if (request->values[i] != NAN)
{
velocity[i] += request->values[i];
} else {
velocity[i] = request->values[i];
}
}
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(), "Got 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];
}
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(), "Got 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;
@@ -276,9 +287,21 @@ private:
{
auto msg = px4_msgs::msg::TrajectorySetpoint();
msg.velocity[0] = velocity[0];
msg.velocity[1] = velocity[1];
msg.velocity[2] = D_SPEED(velocity[2]);
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);
}
@@ -287,9 +310,20 @@ private:
{
auto msg = px4_msgs::msg::TrajectorySetpoint();
for (int i = 0; i < 3; i++)
if (trajectory_valid_x)
{
msg.position[i] = position[i];
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);
@@ -297,11 +331,14 @@ private:
void publish_trajectory_setpoint(px4_msgs::msg::TrajectorySetpoint msg)
{
msg.yaw = last_angle;
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);
}
/**

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

@@ -77,8 +77,14 @@ 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
if (y != NAN):
self.traj_req.y = y
if (z != NAN):
self.traj_req.z = z
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)
@@ -87,8 +93,14 @@ 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
if (y != NAN):
self.traj_req.y = y
if (z != NAN):
self.traj_req.z = z
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)