fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 13:48:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

CollisionPrevention: only save quaternion and yaw on attitude update

Browse Source
This commit is contained in:
Matthias Grob
2024-11-21 11:36:03 +01:00
committed by Claudio Chies
parent 001d722abd
commit 4c8c5fbb37
3 changed files with 43 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/collision_prevention/CollisionPrevention.cpp
+18 -18
View File
@@ -87,6 +87,15 @@ bool CollisionPrevention::is_active()
void CollisionPrevention::modifySetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel) 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 //calculate movement constraints based on range data
const Vector2f original_setpoint = setpoint_accel; const Vector2f original_setpoint = setpoint_accel;
_updateObstacleMap(); _updateObstacleMap();
@@ -103,8 +112,6 @@ void CollisionPrevention::modifySetpoint(Vector2f &setpoint_accel, const Vector2
void CollisionPrevention::_updateObstacleMap() void CollisionPrevention::_updateObstacleMap()
{ {
_sub_vehicle_attitude.update();
// add distance sensor data // add distance sensor data
for (auto &dist_sens_sub : _distance_sensor_subs) { for (auto &dist_sens_sub : _distance_sensor_subs) {
distance_sensor_s distance_sensor; 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, _obstacle_map_body_frame.min_distance = math::min(_obstacle_map_body_frame.min_distance,
(uint16_t)(distance_sensor.min_distance * 100.0f)); (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_distance.max_distance);
_obstacle_map_body_frame.min_distance = math::min(_obstacle_map_body_frame.min_distance, _obstacle_map_body_frame.min_distance = math::min(_obstacle_map_body_frame.min_distance,
obstacle_distance.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; _obstacle_data_present = false;
_closest_dist = UINT16_MAX; _closest_dist = UINT16_MAX;
_closest_dist_dir.setZero(); _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++) { for (int i = 0; i < BIN_COUNT; i++) {
// if the data is stale, reset the bin // if the data is stale, reset the bin
@@ -160,7 +166,7 @@ void CollisionPrevention::_updateObstacleData()
_obstacle_map_body_frame.distances[i] = UINT16_MAX; _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)); _obstacle_map_body_frame.angle_offset));
const Vector2f bin_direction = {cosf(angle), sinf(angle)}; const Vector2f bin_direction = {cosf(angle), sinf(angle)};
uint bin_distance = _obstacle_map_body_frame.distances[i]; 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) 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(); 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()); _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); _transformSetpoint(setpoint_accel);
_getVelocityCompensationAcceleration(vehicle_yaw_angle_rad, setpoint_vel, now, _getVelocityCompensationAcceleration(_vehicle_yaw, setpoint_vel, now,
vel_comp_accel, vel_comp_accel_dir); vel_comp_accel, vel_comp_accel_dir);
if (_checkSetpointDirectionFeasability()) { if (_checkSetpointDirectionFeasability()) {
@@ -226,11 +229,10 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint_accel
} }
} }
void void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, const float vehicle_yaw)
CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, const Quatf &vehicle_attitude)
{ {
int msg_index = 0; 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; float increment_factor = 1.f / obstacle.increment;
if (obstacle.frame == obstacle.MAV_FRAME_GLOBAL || obstacle.frame == obstacle.MAV_FRAME_LOCAL_NED) { if (obstacle.frame == obstacle.MAV_FRAME_GLOBAL || obstacle.frame == obstacle.MAV_FRAME_LOCAL_NED) {
@@ -331,14 +333,13 @@ CollisionPrevention::_checkSetpointDirectionFeasability()
void void
CollisionPrevention::_transformSetpoint(const Vector2f &setpoint) CollisionPrevention::_transformSetpoint(const Vector2f &setpoint)
{ {
const float vehicle_yaw_angle_rad = Eulerf(Quatf(_sub_vehicle_attitude.get().q)).psi();
_setpoint_dir = setpoint / setpoint.norm();; _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) - const float sp_angle_with_offset_deg = _wrap_360(math::degrees(sp_angle_body_frame) -
_obstacle_map_body_frame.angle_offset); _obstacle_map_body_frame.angle_offset);
_setpoint_index = floor(sp_angle_with_offset_deg / BIN_SIZE); _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 // 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 void
@@ -488,8 +489,7 @@ float CollisionPrevention::_getObstacleDistance(const Vector2f &direction)
if (direction_norm > FLT_EPSILON) { if (direction_norm > FLT_EPSILON) {
Vector2f dir = direction / direction_norm; 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;
const float sp_angle_body_frame = atan2f(dir(1), dir(0)) - vehicle_yaw_angle_rad;
const float sp_angle_with_offset_deg = const float sp_angle_with_offset_deg =
_wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset); _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); int dir_index = floor(sp_angle_with_offset_deg / BIN_SIZE);
src/lib/collision_prevention/CollisionPrevention.hpp
+5 -3
View File
@@ -104,7 +104,7 @@ protected:
* Updates obstacle distance message with measurement from offboard * Updates obstacle distance message with measurement from offboard
* @param obstacle, obstacle_distance message to be updated * @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 * Computes an adaption to the setpoint direction to guide towards free space
@@ -173,14 +173,16 @@ private:
float _min_dist_to_keep{}; float _min_dist_to_keep{};
orb_advert_t _mavlink_log_pub{nullptr}; /**< Mavlink log uORB handle */ 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<collision_constraints_s> _constraints_pub{ORB_ID(collision_constraints)}; /**< constraints publication */ uORB::Publication<collision_constraints_s> _constraints_pub{ORB_ID(collision_constraints)}; /**< constraints publication */
uORB::Publication<obstacle_distance_s> _obstacle_distance_pub{ORB_ID(obstacle_distance_fused)}; /**< obstacle_distance publication */ uORB::Publication<obstacle_distance_s> _obstacle_distance_pub{ORB_ID(obstacle_distance_fused)}; /**< obstacle_distance publication */
uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)}; /**< vehicle command do publication */ uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)}; /**< vehicle command do publication */
uORB::SubscriptionData<obstacle_distance_s> _sub_obstacle_distance{ORB_ID(obstacle_distance)}; /**< obstacle distances received form a range sensor */ uORB::SubscriptionData<obstacle_distance_s> _sub_obstacle_distance{ORB_ID(obstacle_distance)}; /**< obstacle distances received form a range sensor */
uORB::SubscriptionData<vehicle_attitude_s> _sub_vehicle_attitude{ORB_ID(vehicle_attitude)};
uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_subs{ORB_ID::distance_sensor}; uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_subs{ORB_ID::distance_sensor};
static constexpr uint64_t RANGE_STREAM_TIMEOUT_US{500_ms}; static constexpr uint64_t RANGE_STREAM_TIMEOUT_US{500_ms};
src/lib/collision_prevention/CollisionPreventionTest.cpp
+20 -40
View File
@@ -59,9 +59,9 @@ public:
{ {
_addDistanceSensorData(distance_sensor, attitude); _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, void test_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoint_index,
float vehicle_yaw_angle_rad) float vehicle_yaw_angle_rad)
@@ -730,14 +730,6 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude)
obstacle_msg.max_distance = 2000; obstacle_msg.max_distance = 2000;
obstacle_msg.angle_offset = 0.f; obstacle_msg.angle_offset = 0.f;
Quaternion<float> vehicle_attitude1(1, 0, 0, 0); //unit transform
Euler<float> attitude2_euler(0, 0, M_PI / 2.0);
Quaternion<float> vehicle_attitude2(attitude2_euler); //90 deg yaw
Euler<float> attitude3_euler(0, 0, -M_PI / 4.0);
Quaternion<float> vehicle_attitude3(attitude3_euler); // -45 deg yaw
Euler<float> attitude4_euler(0, 0, M_PI);
Quaternion<float> vehicle_attitude4(attitude4_euler); // 180 deg yaw
//obstacle at 10-30 deg world frame, distance 5 meters //obstacle at 10-30 deg world frame, distance 5 meters
memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); 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 //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 //THEN: the correct bins in the map should be filled
for (int i = 0; i < distances_array_size; i++) { 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 //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 //THEN: the correct bins in the map should be filled
for (int i = 0; i < distances_array_size; i++) { 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 //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 //THEN: the correct bins in the map should be filled
for (int i = 0; i < distances_array_size; i++) { 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 //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 //THEN: the correct bins in the map should be filled
for (int i = 0; i < distances_array_size; i++) { 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.max_distance = 2000;
obstacle_msg.angle_offset = 0.f; obstacle_msg.angle_offset = 0.f;
Quaternion<float> vehicle_attitude1(1, 0, 0, 0); //unit transform
Euler<float> attitude2_euler(0, 0, M_PI / 2.0);
Quaternion<float> vehicle_attitude2(attitude2_euler); //90 deg yaw
Euler<float> attitude3_euler(0, 0, -M_PI / 4.0);
Quaternion<float> vehicle_attitude3(attitude3_euler); // -45 deg yaw
Euler<float> attitude4_euler(0, 0, M_PI);
Quaternion<float> vehicle_attitude4(attitude4_euler); // 180 deg yaw
//obstacle at 10-30 deg body frame, distance 5 meters //obstacle at 10-30 deg body frame, distance 5 meters
memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); 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 //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 //THEN: the correct bins in the map should be filled
for (int i = 0; i < distances_array_size; i++) { 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 //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 //THEN: the correct bins in the map should be filled
for (int i = 0; i < distances_array_size; i++) { 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 //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 //THEN: the correct bins in the map should be filled
for (int i = 0; i < distances_array_size; i++) { 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 //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 //THEN: the correct bins in the map should be filled
for (int i = 0; i < distances_array_size; i++) { 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.max_distance = 2000;
obstacle_msg.angle_offset = 0.f; obstacle_msg.angle_offset = 0.f;
Quaternion<float> vehicle_attitude(1, 0, 0, 0); //unit transform
//obstacle at 0-30 deg world frame, distance 5 meters //obstacle at 0-30 deg world frame, distance 5 meters
memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); 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 //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 //THEN: the correct bins in the map should be filled
int distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); 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 //WHEN: we add distance sensor data with an angle offset
obstacle_msg.angle_offset = 30.f; 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 //THEN: the correct bins in the map should be filled
for (int i = 0; i < distances_array_size; i++) { 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.max_distance = 2000;
obstacle_msg.angle_offset = 0.f; obstacle_msg.angle_offset = 0.f;
Quaternion<float> vehicle_attitude(1, 0, 0, 0); //unit transform const float vehicle_yaw = 0.f;
float vehicle_yaw_angle_rad = Eulerf(vehicle_attitude).psi();
//obstacle at 0-30 deg world frame, distance 5 meters //obstacle at 0-30 deg world frame, distance 5 meters
memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances));
@@ -1003,7 +984,7 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_distinct_minimum)
//define setpoint //define setpoint
Vector2f setpoint_dir(1, 0); 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, float sp_angle_with_offset_deg = wrap(math::degrees(sp_angle_body_frame) - cp.getObstacleMap().angle_offset,
0.f, 360.f); 0.f, 360.f);
int sp_index = floor(sp_angle_with_offset_deg / cp.getObstacleMap().increment); int sp_index = floor(sp_angle_with_offset_deg / cp.getObstacleMap().increment);
@@ -1015,8 +996,8 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_distinct_minimum)
cp.paramsChanged(); cp.paramsChanged();
//WHEN: we add distance sensor data //WHEN: we add distance sensor data
cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude); cp.test_addObstacleSensorData(obstacle_msg, vehicle_yaw);
cp.test_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw_angle_rad); cp.test_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw);
//THEN: the setpoint direction should be modified correctly //THEN: the setpoint direction should be modified correctly
EXPECT_EQ(sp_index, 2); EXPECT_EQ(sp_index, 2);
@@ -1036,8 +1017,7 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_flat_minimum)
obstacle_msg.max_distance = 2000; obstacle_msg.max_distance = 2000;
obstacle_msg.angle_offset = 0.f; obstacle_msg.angle_offset = 0.f;
Quaternion<float> vehicle_attitude(1, 0, 0, 0); //unit transform const float vehicle_yaw = 0.f;
float vehicle_yaw_angle_rad = Eulerf(vehicle_attitude).psi();
//obstacle at 0-30 deg world frame, distance 5 meters //obstacle at 0-30 deg world frame, distance 5 meters
memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances));
@@ -1052,7 +1032,7 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_flat_minimum)
//define setpoint //define setpoint
Vector2f setpoint_dir(1, 0); 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, float sp_angle_with_offset_deg = wrap(math::degrees(sp_angle_body_frame) - cp.getObstacleMap().angle_offset,
0.f, 360.f); 0.f, 360.f);
int sp_index = floor(sp_angle_with_offset_deg / cp.getObstacleMap().increment); int sp_index = floor(sp_angle_with_offset_deg / cp.getObstacleMap().increment);
@@ -1064,8 +1044,8 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_flat_minimum)
cp.paramsChanged(); cp.paramsChanged();
//WHEN: we add distance sensor data //WHEN: we add distance sensor data
cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude); cp.test_addObstacleSensorData(obstacle_msg, vehicle_yaw);
cp.test_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw_angle_rad); cp.test_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw);
//THEN: the setpoint direction should be modified correctly //THEN: the setpoint direction should be modified correctly
EXPECT_EQ(sp_index, 2); EXPECT_EQ(sp_index, 2);