From 5d3a6d8213bfe49ddaa87ba9b6ca5f8719e97e5c Mon Sep 17 00:00:00 2001 From: Tanja Baumann Date: Thu, 21 Nov 2019 17:13:28 +0100 Subject: [PATCH] collision_prevention: improve behavior in case of range data loss * add test for no incoming range data --- .../CollisionPrevention.cpp | 41 +++++++++-- .../CollisionPrevention.hpp | 7 +- .../CollisionPreventionTest.cpp | 70 ++++++++++++++++++- 3 files changed, 109 insertions(+), 9 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index d17c080ece..0c2b47bf54 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -105,6 +105,18 @@ hrt_abstime CollisionPrevention::getElapsedTime(const hrt_abstime *ptr) return hrt_absolute_time() - *ptr; } +bool CollisionPrevention::is_active() +{ + bool activated = _param_cp_dist.get() > 0; + + if (activated && !_was_active) { + _time_activated = getTime(); + } + + _was_active = activated; + return activated; +} + void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &vehicle_attitude) { @@ -467,9 +479,24 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec } } else { + //allow no movement + float vel_max = 0.f; + setpoint = setpoint * vel_max; + // if distance data is stale, switch to Loiter - _publishVehicleCmdDoLoiter(); - mavlink_log_critical(&_mavlink_log_pub, "No range data received, loitering."); + if (getElapsedTime(&_last_timeout_warning) > 1_s && getElapsedTime(&_time_activated) > 1_s) { + mavlink_log_critical(&_mavlink_log_pub, "No range data received, no movement allowed."); + + if ((constrain_time - _obstacle_map_body_frame.timestamp) > TIMEOUT_HOLD_US + && getElapsedTime(&_time_activated) > TIMEOUT_HOLD_US) { + _publishVehicleCmdDoLoiter(); + mavlink_log_critical(&_mavlink_log_pub, "No range data received, switch to HOLD."); + } + + _last_timeout_warning = getTime(); + } + + } } @@ -488,9 +515,9 @@ CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max || new_setpoint(1) > original_setpoint(1) + 0.05f * max_speed); if (currently_interfering && !_interfering) { - if (hrt_elapsed_time(&_last_collision_warning) > 3_s) { - mavlink_log_critical(&_mavlink_log_pub, "Collision Warning"); - _last_collision_warning = hrt_absolute_time(); + if (getElapsedTime(&_last_collision_warning) > 3_s) { + mavlink_log_info(&_mavlink_log_pub, "Collision Prevention limiting velocity"); + _last_collision_warning = getTime(); } } @@ -498,7 +525,7 @@ CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max // publish constraints collision_constraints_s constraints{}; - constraints.timestamp = hrt_absolute_time(); + constraints.timestamp = getTime(); original_setpoint.copyTo(constraints.original_setpoint); new_setpoint.copyTo(constraints.adapted_setpoint); _constraints_pub.publish(constraints); @@ -509,7 +536,7 @@ CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max void CollisionPrevention::_publishVehicleCmdDoLoiter() { vehicle_command_s command{}; - command.timestamp = hrt_absolute_time(); + command.timestamp = getTime(); command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE; command.param1 = (float)1; // base mode command.param3 = (float)0; // sub mode diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index b111f00578..44713a622e 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -67,10 +67,11 @@ public: CollisionPrevention(ModuleParams *parent); virtual ~CollisionPrevention(); + /** * Returs true if Collision Prevention is running */ - bool is_active() { return _param_cp_dist.get() > 0; } + bool is_active(); /** * Computes collision free setpoints @@ -122,6 +123,7 @@ protected: private: bool _interfering{false}; /**< states if the collision prevention interferes with the user input */ + bool _was_active{false}; /**< states if the collision prevention interferes with the user input */ orb_advert_t _mavlink_log_pub{nullptr}; /**< Mavlink log uORB handle */ @@ -134,8 +136,11 @@ private: uORB::SubscriptionData _sub_vehicle_attitude{ORB_ID(vehicle_attitude)}; static constexpr uint64_t RANGE_STREAM_TIMEOUT_US{500_ms}; + static constexpr uint64_t TIMEOUT_HOLD_US{5_s}; hrt_abstime _last_collision_warning{0}; + hrt_abstime _last_timeout_warning{0}; + hrt_abstime _time_activated{0}; DEFINE_PARAMETERS( (ParamFloat) _param_cp_dist, /**< collision prevention keep minimum distance */ diff --git a/src/lib/collision_prevention/CollisionPreventionTest.cpp b/src/lib/collision_prevention/CollisionPreventionTest.cpp index 2b1209a9dc..e92f3fdc38 100644 --- a/src/lib/collision_prevention/CollisionPreventionTest.cpp +++ b/src/lib/collision_prevention/CollisionPreventionTest.cpp @@ -251,9 +251,9 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithDistanceMessage) TEST_F(CollisionPreventionTest, testPurgeOldData) { // GIVEN: a simple setup condition + TestTimingCollisionPrevention cp; hrt_abstime start_time = hrt_absolute_time(); mocked_time = start_time; - TestTimingCollisionPrevention cp; matrix::Vector2f original_setpoint(10, 0); float max_speed = 3; matrix::Vector2f curr_pos(0, 0); @@ -325,6 +325,74 @@ TEST_F(CollisionPreventionTest, testPurgeOldData) orb_unadvertise(vehicle_attitude_pub); } +TEST_F(CollisionPreventionTest, testNoRangeData) +{ + // GIVEN: a simple setup condition + TestTimingCollisionPrevention cp; + hrt_abstime start_time = hrt_absolute_time(); + mocked_time = start_time; + matrix::Vector2f original_setpoint(10, 0); + float max_speed = 3; + matrix::Vector2f curr_pos(0, 0); + matrix::Vector2f curr_vel(2, 0); + vehicle_attitude_s attitude; + attitude.timestamp = start_time; + attitude.q[0] = 1.0f; + attitude.q[1] = 0.0f; + attitude.q[2] = 0.0f; + attitude.q[3] = 0.0f; + + // AND: a parameter handle + param_t param = param_handle(px4::params::CP_DIST); + float value = 10; // try to keep 10m distance + param_set(param, &value); + cp.paramsChanged(); + + // AND: an obstacle message without any obstacle + obstacle_distance_s message; + memset(&message, 0xDEAD, sizeof(message)); + message.frame = message.MAV_FRAME_GLOBAL; //north aligned + message.min_distance = 100; + message.max_distance = 10000; + message.angle_offset = 0; + message.timestamp = start_time; + int distances_array_size = sizeof(message.distances) / sizeof(message.distances[0]); + message.increment = 360 / distances_array_size; + + for (int i = 0; i < distances_array_size; i++) { + message.distances[i] = 9000; + } + + + // WHEN: we publish the message and set the parameter and then run the setpoint modification + orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); + orb_advert_t vehicle_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &attitude); + orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); + orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); + + for (int i = 0; i < 10; i++) { + + matrix::Vector2f modified_setpoint = original_setpoint; + cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + + //advance time by 0.1 seconds but no new message comes in + mocked_time = mocked_time + 100000; + + if (i < 5) { + // THEN: If the data is new enough, the velocity setpoint should stay the same as the input + // Note: direction will change slightly due to guidance + EXPECT_EQ(original_setpoint.norm(), modified_setpoint.norm()); + + } else { + // THEN: If the data is expired, the velocity setpoint should be cut down to zero because there is no data + EXPECT_FLOAT_EQ(0.f, modified_setpoint.norm()) << modified_setpoint(0) << "," << modified_setpoint(1); + } + } + + orb_unadvertise(obstacle_distance_pub); + orb_unadvertise(vehicle_attitude_pub); +} + TEST_F(CollisionPreventionTest, noBias) { // GIVEN: a simple setup condition