diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 5fdd9c8010..d5c19b3d09 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -87,6 +87,15 @@ bool CollisionPrevention::is_active() void CollisionPrevention::modifySetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel) { + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude; + + if (_vehicle_attitude_sub.copy(&vehicle_attitude)) { + _vehicle_attitude = Quatf(vehicle_attitude.q); + _vehicle_yaw = Eulerf(_vehicle_attitude).psi(); + } + } + //calculate movement constraints based on range data const Vector2f original_setpoint = setpoint_accel; _updateObstacleMap(); @@ -103,8 +112,6 @@ void CollisionPrevention::modifySetpoint(Vector2f &setpoint_accel, const Vector2 void CollisionPrevention::_updateObstacleMap() { - _sub_vehicle_attitude.update(); - // add distance sensor data for (auto &dist_sens_sub : _distance_sensor_subs) { distance_sensor_s distance_sensor; @@ -122,7 +129,7 @@ void CollisionPrevention::_updateObstacleMap() _obstacle_map_body_frame.min_distance = math::min(_obstacle_map_body_frame.min_distance, (uint16_t)(distance_sensor.min_distance * 100.0f)); - _addDistanceSensorData(distance_sensor, Quatf(_sub_vehicle_attitude.get().q)); + _addDistanceSensorData(distance_sensor, _vehicle_attitude); } } } @@ -139,7 +146,7 @@ void CollisionPrevention::_updateObstacleMap() obstacle_distance.max_distance); _obstacle_map_body_frame.min_distance = math::min(_obstacle_map_body_frame.min_distance, obstacle_distance.min_distance); - _addObstacleSensorData(obstacle_distance, Quatf(_sub_vehicle_attitude.get().q)); + _addObstacleSensorData(obstacle_distance, _vehicle_yaw); } } @@ -152,7 +159,6 @@ void CollisionPrevention::_updateObstacleData() _obstacle_data_present = false; _closest_dist = UINT16_MAX; _closest_dist_dir.setZero(); - const float vehicle_yaw_angle_rad = Eulerf(Quatf(_sub_vehicle_attitude.get().q)).psi(); for (int i = 0; i < BIN_COUNT; i++) { // if the data is stale, reset the bin @@ -160,7 +166,7 @@ void CollisionPrevention::_updateObstacleData() _obstacle_map_body_frame.distances[i] = UINT16_MAX; } - float angle = wrap_2pi(vehicle_yaw_angle_rad + math::radians((float)i * BIN_SIZE + + float angle = wrap_2pi(_vehicle_yaw + math::radians((float)i * BIN_SIZE + _obstacle_map_body_frame.angle_offset)); const Vector2f bin_direction = {cosf(angle), sinf(angle)}; uint bin_distance = _obstacle_map_body_frame.distances[i]; @@ -180,9 +186,6 @@ void CollisionPrevention::_updateObstacleData() void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel) { - const Quatf attitude = Quatf(_sub_vehicle_attitude.get().q); - const float vehicle_yaw_angle_rad = Eulerf(attitude).psi(); - const float setpoint_length = setpoint_accel.norm(); _min_dist_to_keep = math::max(_obstacle_map_body_frame.min_distance / 100.0f, _param_cp_dist.get()); @@ -198,7 +201,7 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint_accel _transformSetpoint(setpoint_accel); - _getVelocityCompensationAcceleration(vehicle_yaw_angle_rad, setpoint_vel, now, + _getVelocityCompensationAcceleration(_vehicle_yaw, setpoint_vel, now, vel_comp_accel, vel_comp_accel_dir); if (_checkSetpointDirectionFeasability()) { @@ -226,11 +229,10 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint_accel } } -void -CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, const Quatf &vehicle_attitude) +void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, const float vehicle_yaw) { int msg_index = 0; - float vehicle_orientation_deg = math::degrees(Eulerf(vehicle_attitude).psi()); + float vehicle_orientation_deg = math::degrees(vehicle_yaw); float increment_factor = 1.f / obstacle.increment; if (obstacle.frame == obstacle.MAV_FRAME_GLOBAL || obstacle.frame == obstacle.MAV_FRAME_LOCAL_NED) { @@ -331,14 +333,13 @@ CollisionPrevention::_checkSetpointDirectionFeasability() void CollisionPrevention::_transformSetpoint(const Vector2f &setpoint) { - const float vehicle_yaw_angle_rad = Eulerf(Quatf(_sub_vehicle_attitude.get().q)).psi(); _setpoint_dir = setpoint / setpoint.norm();; - const float sp_angle_body_frame = atan2f(_setpoint_dir(1), _setpoint_dir(0)) - vehicle_yaw_angle_rad; + const float sp_angle_body_frame = atan2f(_setpoint_dir(1), _setpoint_dir(0)) - _vehicle_yaw; const float sp_angle_with_offset_deg = _wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset); _setpoint_index = floor(sp_angle_with_offset_deg / BIN_SIZE); // change setpoint direction slightly (max by _param_cp_guide_ang degrees) to help guide through narrow gaps - _adaptSetpointDirection(_setpoint_dir, _setpoint_index, vehicle_yaw_angle_rad); + _adaptSetpointDirection(_setpoint_dir, _setpoint_index, _vehicle_yaw); } void @@ -488,8 +489,7 @@ float CollisionPrevention::_getObstacleDistance(const Vector2f &direction) if (direction_norm > FLT_EPSILON) { Vector2f dir = direction / direction_norm; - const float vehicle_yaw_angle_rad = Eulerf(Quatf(_sub_vehicle_attitude.get().q)).psi(); - const float sp_angle_body_frame = atan2f(dir(1), dir(0)) - vehicle_yaw_angle_rad; + const float sp_angle_body_frame = atan2f(dir(1), dir(0)) - _vehicle_yaw; const float sp_angle_with_offset_deg = _wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset); int dir_index = floor(sp_angle_with_offset_deg / BIN_SIZE); diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index 1c52724d9b..0023f7716c 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -104,7 +104,7 @@ protected: * Updates obstacle distance message with measurement from offboard * @param obstacle, obstacle_distance message to be updated */ - void _addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &vehicle_attitude); + void _addObstacleSensorData(const obstacle_distance_s &obstacle, const float vehicle_yaw); /** * Computes an adaption to the setpoint direction to guide towards free space @@ -173,14 +173,16 @@ private: float _min_dist_to_keep{}; orb_advert_t _mavlink_log_pub{nullptr}; /**< Mavlink log uORB handle */ - matrix::Vector2f _DEBUG; + + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + matrix::Quatf _vehicle_attitude{}; + float _vehicle_yaw{0.f}; uORB::Publication _constraints_pub{ORB_ID(collision_constraints)}; /**< constraints publication */ uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance_fused)}; /**< obstacle_distance publication */ uORB::Publication _vehicle_command_pub{ORB_ID(vehicle_command)}; /**< vehicle command do publication */ uORB::SubscriptionData _sub_obstacle_distance{ORB_ID(obstacle_distance)}; /**< obstacle distances received form a range sensor */ - uORB::SubscriptionData _sub_vehicle_attitude{ORB_ID(vehicle_attitude)}; uORB::SubscriptionMultiArray _distance_sensor_subs{ORB_ID::distance_sensor}; static constexpr uint64_t RANGE_STREAM_TIMEOUT_US{500_ms}; diff --git a/src/lib/collision_prevention/CollisionPreventionTest.cpp b/src/lib/collision_prevention/CollisionPreventionTest.cpp index a75cd57662..955293dad5 100644 --- a/src/lib/collision_prevention/CollisionPreventionTest.cpp +++ b/src/lib/collision_prevention/CollisionPreventionTest.cpp @@ -59,9 +59,9 @@ public: { _addDistanceSensorData(distance_sensor, attitude); } - void test_addObstacleSensorData(const obstacle_distance_s &obstacle, const Quatf &attitude) + void test_addObstacleSensorData(const obstacle_distance_s &obstacle, const float vehicle_yaw) { - _addObstacleSensorData(obstacle, attitude); + _addObstacleSensorData(obstacle, vehicle_yaw); } void test_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad) @@ -730,14 +730,6 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - Quaternion vehicle_attitude1(1, 0, 0, 0); //unit transform - Euler attitude2_euler(0, 0, M_PI / 2.0); - Quaternion vehicle_attitude2(attitude2_euler); //90 deg yaw - Euler attitude3_euler(0, 0, -M_PI / 4.0); - Quaternion vehicle_attitude3(attitude3_euler); // -45 deg yaw - Euler attitude4_euler(0, 0, M_PI); - Quaternion vehicle_attitude4(attitude4_euler); // 180 deg yaw - //obstacle at 10-30 deg world frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); @@ -754,7 +746,7 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) } //WHEN: we add distance sensor data while vehicle has zero yaw - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude1); + cp.test_addObstacleSensorData(obstacle_msg, 0.f); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { @@ -771,7 +763,7 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) //WHEN: we add distance sensor data while vehicle yaw 90deg to the right - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude2); + cp.test_addObstacleSensorData(obstacle_msg, M_PI_2); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { @@ -787,7 +779,7 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) } //WHEN: we add distance sensor data while vehicle yaw 45deg to the left - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude3); + cp.test_addObstacleSensorData(obstacle_msg, -M_PI_4); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { @@ -803,7 +795,7 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) } //WHEN: we add distance sensor data while vehicle yaw 180deg - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude4); + cp.test_addObstacleSensorData(obstacle_msg, M_PI); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { @@ -831,14 +823,6 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - Quaternion vehicle_attitude1(1, 0, 0, 0); //unit transform - Euler attitude2_euler(0, 0, M_PI / 2.0); - Quaternion vehicle_attitude2(attitude2_euler); //90 deg yaw - Euler attitude3_euler(0, 0, -M_PI / 4.0); - Quaternion vehicle_attitude3(attitude3_euler); // -45 deg yaw - Euler attitude4_euler(0, 0, M_PI); - Quaternion vehicle_attitude4(attitude4_euler); // 180 deg yaw - //obstacle at 10-30 deg body frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); @@ -855,7 +839,7 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) } //WHEN: we add obstacle data while vehicle has zero yaw - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude1); + cp.test_addObstacleSensorData(obstacle_msg, 0.f); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { @@ -871,7 +855,7 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) } //WHEN: we add obstacle data while vehicle yaw 90deg to the right - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude2); + cp.test_addObstacleSensorData(obstacle_msg, M_PI_2); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { @@ -887,7 +871,7 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) } //WHEN: we add obstacle data while vehicle yaw 45deg to the left - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude3); + cp.test_addObstacleSensorData(obstacle_msg, -M_PI_4); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { @@ -903,7 +887,7 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) } //WHEN: we add obstacle data while vehicle yaw 180deg - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude4); + cp.test_addObstacleSensorData(obstacle_msg, M_PI); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { @@ -932,8 +916,6 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_resolution_offset) obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform - //obstacle at 0-30 deg world frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); @@ -942,7 +924,7 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_resolution_offset) } //WHEN: we add distance sensor data - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude); + cp.test_addObstacleSensorData(obstacle_msg, 0.f); //THEN: the correct bins in the map should be filled int distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); @@ -961,7 +943,7 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_resolution_offset) //WHEN: we add distance sensor data with an angle offset obstacle_msg.angle_offset = 30.f; - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude); + cp.test_addObstacleSensorData(obstacle_msg, 0.f); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { @@ -989,8 +971,7 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_distinct_minimum) obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform - float vehicle_yaw_angle_rad = Eulerf(vehicle_attitude).psi(); + const float vehicle_yaw = 0.f; //obstacle at 0-30 deg world frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); @@ -1003,7 +984,7 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_distinct_minimum) //define setpoint Vector2f setpoint_dir(1, 0); - float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw_angle_rad; + float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw; float sp_angle_with_offset_deg = wrap(math::degrees(sp_angle_body_frame) - cp.getObstacleMap().angle_offset, 0.f, 360.f); int sp_index = floor(sp_angle_with_offset_deg / cp.getObstacleMap().increment); @@ -1015,8 +996,8 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_distinct_minimum) cp.paramsChanged(); //WHEN: we add distance sensor data - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude); - cp.test_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw_angle_rad); + cp.test_addObstacleSensorData(obstacle_msg, vehicle_yaw); + cp.test_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw); //THEN: the setpoint direction should be modified correctly EXPECT_EQ(sp_index, 2); @@ -1036,8 +1017,7 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_flat_minimum) obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform - float vehicle_yaw_angle_rad = Eulerf(vehicle_attitude).psi(); + const float vehicle_yaw = 0.f; //obstacle at 0-30 deg world frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); @@ -1052,7 +1032,7 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_flat_minimum) //define setpoint Vector2f setpoint_dir(1, 0); - float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw_angle_rad; + float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw; float sp_angle_with_offset_deg = wrap(math::degrees(sp_angle_body_frame) - cp.getObstacleMap().angle_offset, 0.f, 360.f); int sp_index = floor(sp_angle_with_offset_deg / cp.getObstacleMap().increment); @@ -1064,8 +1044,8 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_flat_minimum) cp.paramsChanged(); //WHEN: we add distance sensor data - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude); - cp.test_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw_angle_rad); + cp.test_addObstacleSensorData(obstacle_msg, vehicle_yaw); + cp.test_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw); //THEN: the setpoint direction should be modified correctly EXPECT_EQ(sp_index, 2);