From 9404b199065be410ef15e9149b1f3461cd7e141a Mon Sep 17 00:00:00 2001 From: Julian Kent Date: Fri, 3 Apr 2020 14:55:00 +0200 Subject: [PATCH] Fix bezier scaling --- src/lib/avoidance/ObstacleAvoidance.cpp | 6 +++--- src/lib/avoidance/ObstacleAvoidanceTest.cpp | 21 ++++++++++++++------- 2 files changed, 17 insertions(+), 10 deletions(-) diff --git a/src/lib/avoidance/ObstacleAvoidance.cpp b/src/lib/avoidance/ObstacleAvoidance.cpp index d18a54a246..067f6fbafd 100644 --- a/src/lib/avoidance/ObstacleAvoidance.cpp +++ b/src/lib/avoidance/ObstacleAvoidance.cpp @@ -153,9 +153,9 @@ void ObstacleAvoidance::_generateBezierSetpoints(matrix::Vector3f &position, mat bezier::calculateBezierPosVel(bezier_points, bezier_order, T, position, velocity) && bezier::calculateBezierYaw(bezier_yaws, bezier_order, T, yaw, yaw_velocity) ) { - // translate velocities into real velocities - yaw_velocity *= duration_s; - velocity *= duration_s; + // translate bezier velocities T [0;1] into real velocities m/s + yaw_velocity /= duration_s; + velocity /= duration_s; } else { PX4_WARN("Obstacle Avoidance system failed, bad trajectory"); diff --git a/src/lib/avoidance/ObstacleAvoidanceTest.cpp b/src/lib/avoidance/ObstacleAvoidanceTest.cpp index bd6a9e99b2..20ff48f89a 100644 --- a/src/lib/avoidance/ObstacleAvoidanceTest.cpp +++ b/src/lib/avoidance/ObstacleAvoidanceTest.cpp @@ -109,12 +109,17 @@ TEST_F(ObstacleAvoidanceTest, oa_enabled_healthy_bezier) vehicle_trajectory_bezier_s message {}; message.timestamp = hrt_absolute_time(); - message.bezier_order = 1; + message.bezier_order = 2; message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[0] = 2.6f; message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[1] = 2.4f; message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[2] = 2.7f; message.control_points[vehicle_trajectory_bezier_s::POINT_0].yaw = 0.23f; - message.control_points[vehicle_trajectory_bezier_s::POINT_0].delta = 0.5f; + message.control_points[vehicle_trajectory_bezier_s::POINT_0].delta = NAN; + message.control_points[vehicle_trajectory_bezier_s::POINT_1].position[0] = 2.6f; + message.control_points[vehicle_trajectory_bezier_s::POINT_1].position[1] = 2.4f; + message.control_points[vehicle_trajectory_bezier_s::POINT_1].position[2] = 3.7f; + message.control_points[vehicle_trajectory_bezier_s::POINT_1].yaw = 0.23f; + message.control_points[vehicle_trajectory_bezier_s::POINT_1].delta = 0.5f; // GIVEN: and we publish the vehicle_trajectory_waypoint message and vehicle status message uORB::Publication vehicle_trajectory_bezier_pub{ORB_ID(vehicle_trajectory_bezier)}; @@ -129,11 +134,13 @@ TEST_F(ObstacleAvoidanceTest, oa_enabled_healthy_bezier) oa.injectAvoidanceSetpoints(pos_sp, vel_sp, yaw_sp, yaw_speed_sp); // THEN: the setpoints should be injected - EXPECT_FLOAT_EQ(message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[0], pos_sp(0)); - EXPECT_FLOAT_EQ(message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[1], pos_sp(1)); - EXPECT_FLOAT_EQ(message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[2], pos_sp(2)); - EXPECT_FLOAT_EQ(vel_sp.norm(), 0); - EXPECT_FLOAT_EQ(message.control_points[vehicle_trajectory_bezier_s::POINT_0].yaw, yaw_sp); + EXPECT_FLOAT_EQ(2.6f, pos_sp(0)); + EXPECT_FLOAT_EQ(2.4f, pos_sp(1)); + EXPECT_LT(2.7f, pos_sp(2)); + EXPECT_GT(2.8f, pos_sp(2)); // probably only a tiny bit above 2.7, but let's not have flakey tests + EXPECT_FLOAT_EQ(vel_sp.xy().norm(), 0); + EXPECT_FLOAT_EQ(vel_sp(2), (3.7f - 2.7f) / 0.5f); + EXPECT_FLOAT_EQ(0.23, yaw_sp); EXPECT_FLOAT_EQ(yaw_speed_sp, 0); }