mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-03 10:28:52 +08:00
CollisionPrevention: only save quaternion and yaw on attitude update
This commit is contained in:
Matthias Grob
committed by
Claudio Chies
Claudio Chies
parent
001d722abd
commit
4c8c5fbb37
@@ -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);
|
||||
|
||||
@@ -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<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<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<vehicle_attitude_s> _sub_vehicle_attitude{ORB_ID(vehicle_attitude)};
|
||||
uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_subs{ORB_ID::distance_sensor};
|
||||
|
||||
static constexpr uint64_t RANGE_STREAM_TIMEOUT_US{500_ms};
|
||||
|
||||
@@ -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<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
|
||||
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<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
|
||||
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<float> 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<float> 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<float> 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);
|
||||
|
||||
Reference in New Issue
Block a user