diff --git a/src/lib/flight_tasks/tasks/Utility/CMakeLists.txt b/src/lib/flight_tasks/tasks/Utility/CMakeLists.txt index 287f24f22b..93ff3062d9 100644 --- a/src/lib/flight_tasks/tasks/Utility/CMakeLists.txt +++ b/src/lib/flight_tasks/tasks/Utility/CMakeLists.txt @@ -41,7 +41,7 @@ px4_add_library(FlightTaskUtility ManualVelocitySmoothingZ.cpp ) -target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis) +target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis bezier) target_include_directories(FlightTaskUtility PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) px4_add_unit_gtest(SRC VelocitySmoothingTest.cpp LINKLIBS FlightTaskUtility) diff --git a/src/lib/flight_tasks/tasks/Utility/ObstacleAvoidance.cpp b/src/lib/flight_tasks/tasks/Utility/ObstacleAvoidance.cpp index bdd0b14d0b..d18a54a246 100644 --- a/src/lib/flight_tasks/tasks/Utility/ObstacleAvoidance.cpp +++ b/src/lib/flight_tasks/tasks/Utility/ObstacleAvoidance.cpp @@ -36,6 +36,7 @@ */ #include "ObstacleAvoidance.hpp" +#include "bezier/BezierN.hpp" using namespace matrix; using namespace time_literals; @@ -61,13 +62,21 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel { _sub_vehicle_status.update(); _sub_vehicle_trajectory_waypoint.update(); + _sub_vehicle_trajectory_bezier.update(); - const bool avoidance_data_timeout = hrt_elapsed_time((hrt_abstime *)&_sub_vehicle_trajectory_waypoint.get().timestamp) - > TRAJECTORY_STREAM_TIMEOUT_US; - const bool avoidance_point_valid = - _sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid; + const auto &wp_msg = _sub_vehicle_trajectory_waypoint.get(); + const auto &bezier_msg = _sub_vehicle_trajectory_bezier.get(); - _avoidance_point_not_valid_hysteresis.set_state_and_update(!avoidance_point_valid, hrt_absolute_time()); + const bool avoidance_data_timeout = + hrt_elapsed_time((hrt_abstime *)&wp_msg.timestamp) > TRAJECTORY_STREAM_TIMEOUT_US && + hrt_elapsed_time((hrt_abstime *)&bezier_msg.timestamp) > hrt_abstime(bezier_msg.control_points[bezier_msg.bezier_order - + 1].delta * 1e6f); + + const bool avoidance_point_valid = wp_msg.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid; + const bool avoidance_bezier_valid = bezier_msg.bezier_order > 0; + + _avoidance_point_not_valid_hysteresis.set_state_and_update(!avoidance_point_valid + && !avoidance_bezier_valid, hrt_absolute_time()); const bool avoidance_invalid = (avoidance_data_timeout || _avoidance_point_not_valid_hysteresis.get_state()); @@ -97,18 +106,63 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel } if (avoidance_point_valid) { - pos_sp = Vector3f(_sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position); - vel_sp = Vector3f(_sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity); + const auto &point0 = wp_msg.waypoints[vehicle_trajectory_waypoint_s::POINT_0]; + pos_sp = Vector3f(point0.position); + vel_sp = Vector3f(point0.velocity); if (!_ext_yaw_active) { // inject yaw setpoints only if weathervane isn't active - yaw_sp = _sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw; - yaw_speed_sp = _sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw_speed; + yaw_sp = point0.yaw; + yaw_speed_sp = point0.yaw_speed; + } + + } else if (avoidance_bezier_valid) { + + float yaw = NAN, yaw_speed = NAN; + _generateBezierSetpoints(pos_sp, vel_sp, yaw, yaw_speed); + + if (!_ext_yaw_active) { + // inject yaw setpoints only if weathervane isn't active + yaw_sp = yaw; + yaw_speed_sp = yaw_speed; } } - } +void ObstacleAvoidance::_generateBezierSetpoints(matrix::Vector3f &position, matrix::Vector3f &velocity, + float &yaw, float &yaw_velocity) +{ + const auto &msg = _sub_vehicle_trajectory_bezier.get(); + int bezier_order = msg.bezier_order; + matrix::Vector3f bezier_points[bezier_order]; + float bezier_yaws[bezier_order]; + + for (int i = 0; i < bezier_order; i++) { + bezier_points[i] = Vector3f(msg.control_points[i].position); + bezier_yaws[i] = msg.control_points[i].yaw; + } + + const float duration_s = msg.control_points[bezier_order - 1].delta; + const hrt_abstime now = hrt_absolute_time(); + const hrt_abstime start = msg.timestamp; + const hrt_abstime end = start + hrt_abstime(duration_s * 1e6f); + + float T = NAN; + + if (bezier::calculateT(start, end, now, T) && + 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; + + } else { + PX4_WARN("Obstacle Avoidance system failed, bad trajectory"); + } +} + + void ObstacleAvoidance::updateAvoidanceDesiredWaypoints(const Vector3f &curr_wp, const float curr_yaw, const float curr_yawspeed, const Vector3f &next_wp, const float next_yaw, const float next_yawspeed, const bool ext_yaw_active, const int wp_type) diff --git a/src/lib/flight_tasks/tasks/Utility/ObstacleAvoidance.hpp b/src/lib/flight_tasks/tasks/Utility/ObstacleAvoidance.hpp index bacc203b8e..9ed6491e74 100644 --- a/src/lib/flight_tasks/tasks/Utility/ObstacleAvoidance.hpp +++ b/src/lib/flight_tasks/tasks/Utility/ObstacleAvoidance.hpp @@ -52,6 +52,7 @@ #include #include #include +#include #include #include @@ -116,6 +117,7 @@ public: protected: + uORB::SubscriptionData _sub_vehicle_trajectory_bezier{ORB_ID(vehicle_trajectory_bezier)}; /**< vehicle trajectory waypoint subscription */ uORB::SubscriptionData _sub_vehicle_trajectory_waypoint{ORB_ID(vehicle_trajectory_waypoint)}; /**< vehicle trajectory waypoint subscription */ uORB::SubscriptionData _sub_vehicle_status{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ @@ -140,6 +142,7 @@ protected: * Publishes vehicle command. */ void _publishVehicleCmdDoLoiter(); + void _generateBezierSetpoints(matrix::Vector3f &position, matrix::Vector3f &velocity, float &yaw, float &yaw_velocity); DEFINE_PARAMETERS( (ParamFloat) _param_nav_mc_alt_rad /**< Acceptance radius for multicopter altitude */ diff --git a/src/lib/flight_tasks/tasks/Utility/ObstacleAvoidanceTest.cpp b/src/lib/flight_tasks/tasks/Utility/ObstacleAvoidanceTest.cpp index 67cfbaf9ac..2c8506e545 100644 --- a/src/lib/flight_tasks/tasks/Utility/ObstacleAvoidanceTest.cpp +++ b/src/lib/flight_tasks/tasks/Utility/ObstacleAvoidanceTest.cpp @@ -101,6 +101,42 @@ TEST_F(ObstacleAvoidanceTest, oa_enabled_healthy) EXPECT_FALSE(PX4_ISFINITE(yaw_speed_sp)); } +TEST_F(ObstacleAvoidanceTest, oa_enabled_healthy_bezier) +{ + // GIVEN: the flight controller setpoints from FlightTaskAutoMapper and a vehicle_trajectory_waypoint message coming + // from offboard + TestObstacleAvoidance oa; + + vehicle_trajectory_bezier_s message {}; + message.timestamp = hrt_absolute_time(); + message.bezier_order = 1; + 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; + + // GIVEN: and we publish the vehicle_trajectory_waypoint message and vehicle status message + uORB::Publication vehicle_trajectory_bezier_pub{ORB_ID(vehicle_trajectory_bezier)}; + vehicle_trajectory_bezier_pub.publish(message); + + vehicle_status_s vehicle_status{}; + vehicle_status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; + uORB::Publication vehicle_status_pub{ORB_ID(vehicle_status)}; + vehicle_status_pub.publish(vehicle_status); + + // WHEN: we inject the vehicle_trajectory_waypoint in the interface + 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(yaw_speed_sp, 0); +} + TEST_F(ObstacleAvoidanceTest, oa_enabled_not_healthy) { // GIVEN: the flight controller setpoints from FlightTaskAutoMapper and a vehicle_trajectory_waypoint message