PX4 Firmware
PX4 Autopilot Software http://px4.io
ObstacleAvoidanceTest.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (C) 2019 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 #include <gtest/gtest.h>
35 #include <ObstacleAvoidance.hpp>
36 #include <uORB/Subscription.hpp>
37 
38 
39 using namespace matrix;
40 // to run: make tests TESTFILTER=CollisionPrevention
41 
42 class ObstacleAvoidanceTest : public ::testing::Test
43 {
44 public:
47  float yaw_sp = 0.123f;
48  float yaw_speed_sp = NAN;
49  void SetUp() override
50 
51  {
53  pos_sp = Vector3f(1.f, 1.2f, 0.1f);
54  vel_sp = Vector3f(0.3f, 0.4f, 0.1f);
55  }
56 };
57 
59 {
60 public:
62  void paramsChanged() {ObstacleAvoidance::updateParamsImpl();}
63  void test_setPosition(Vector3f &pos) {_position = pos;}
64 };
65 
66 TEST_F(ObstacleAvoidanceTest, instantiation) { ObstacleAvoidance oa(nullptr); }
67 
68 TEST_F(ObstacleAvoidanceTest, oa_enabled_healthy)
69 {
70  // GIVEN: the flight controller setpoints from FlightTaskAutoMapper and a vehicle_trajectory_waypoint message coming
71  // from offboard
73 
75  message.timestamp = hrt_absolute_time();
76  message.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS;
77  message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[0] = 2.6f;
78  message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[1] = 2.4f;
79  message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[2] = 2.7f;
80  message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw = 0.23f;
81  message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid = true;
82 
83  // GIVEN: and we publish the vehicle_trajectory_waypoint message and vehicle status message
84  uORB::Publication<vehicle_trajectory_waypoint_s> vehicle_trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)};
85  vehicle_trajectory_waypoint_pub.publish(message);
86 
87  vehicle_status_s vehicle_status{};
88  vehicle_status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
89  uORB::Publication<vehicle_status_s> vehicle_status_pub{ORB_ID(vehicle_status)};
90  vehicle_status_pub.publish(vehicle_status);
91 
92  // WHEN: we inject the vehicle_trajectory_waypoint in the interface
93  oa.injectAvoidanceSetpoints(pos_sp, vel_sp, yaw_sp, yaw_speed_sp);
94 
95  // THEN: the setpoints should be injected
96  EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[0], pos_sp(0));
97  EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[1], pos_sp(1));
98  EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[2], pos_sp(2));
99  EXPECT_TRUE(vel_sp.isAllNan());
100  EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw, yaw_sp);
101  EXPECT_FALSE(PX4_ISFINITE(yaw_speed_sp));
102 }
103 
104 TEST_F(ObstacleAvoidanceTest, oa_enabled_not_healthy)
105 {
106  // GIVEN: the flight controller setpoints from FlightTaskAutoMapper and a vehicle_trajectory_waypoint message
108 
110  Vector3f pos(3.1f, 4.7f, 5.2f);
111  oa.test_setPosition(pos);
112 
113  // GIVEN: and we publish the vehicle_trajectory_waypoint message and vehicle_status
114  uORB::Publication<vehicle_trajectory_waypoint_s> vehicle_trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)};
115  vehicle_trajectory_waypoint_pub.publish(message);
116 
117  vehicle_status_s vehicle_status{};
118  vehicle_status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
119  uORB::Publication<vehicle_status_s> vehicle_status_pub{ORB_ID(vehicle_status)};
120  vehicle_status_pub.publish(vehicle_status);
121 
122  // WHEN: we inject the vehicle_trajectory_waypoint in the interface
123  oa.injectAvoidanceSetpoints(pos_sp, vel_sp, yaw_sp, yaw_speed_sp);
124 
125  // THEN: the setpoints shouldn't be injected
126  EXPECT_FLOAT_EQ(pos(0), pos_sp(0));
127  EXPECT_FLOAT_EQ(pos(1), pos_sp(1));
128  EXPECT_FLOAT_EQ(pos(2), pos_sp(2));
129  EXPECT_TRUE(vel_sp.isAllNan());
130  EXPECT_FALSE(PX4_ISFINITE(yaw_sp));
131  EXPECT_FALSE(PX4_ISFINITE(yaw_speed_sp));
132 }
133 
135 {
136  // GIVEN: the flight controller setpoints from FlightTaskAutoMapper and the waypoints from FLightTaskAuto
138 
139  pos_sp = Vector3f(1.f, 1.2f, NAN);
140  vel_sp = Vector3f(NAN, NAN, 0.7f);
141  int type = 4;
142  Vector3f curr_wp(1.f, 1.2f, 5.0f);
143  float curr_yaw = 1.02f;
144  float curr_yawspeed = NAN;
145  Vector3f next_wp(11.f, 1.2f, 5.0f);
146  float next_yaw = 0.82f;
147  float next_yawspeed = NAN;
148  bool ext_yaw_active = false;
149 
150  // WHEN: we inject the setpoints and waypoints in the interface
151  oa.updateAvoidanceDesiredWaypoints(curr_wp, curr_yaw, curr_yawspeed, next_wp, next_yaw, next_yawspeed, ext_yaw_active,
152  type);
153  oa.updateAvoidanceDesiredSetpoints(pos_sp, vel_sp, type);
154 
155  // WHEN: we subscribe to the uORB message out of the interface
156  uORB::SubscriptionData<vehicle_trajectory_waypoint_s> _sub_traj_wp_avoidance_desired{ORB_ID(vehicle_trajectory_waypoint_desired)};
157  _sub_traj_wp_avoidance_desired.update();
158 
159  // THEN: we expect the setpoints in POINT_0 and waypoints in POINT_1 and POINT_2
160  EXPECT_FLOAT_EQ(pos_sp(0),
161  _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[0]);
162  EXPECT_FLOAT_EQ(pos_sp(1),
163  _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[1]);
164  EXPECT_FALSE(PX4_ISFINITE(
165  _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[2]));
166  EXPECT_FALSE(PX4_ISFINITE(
167  _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[0]));
168  EXPECT_FALSE(PX4_ISFINITE(
169  _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[1]));
170  EXPECT_FLOAT_EQ(vel_sp(2),
171  _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[2]);
172  EXPECT_EQ(type, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].type);
173  EXPECT_TRUE(_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid);
174 
175  EXPECT_FLOAT_EQ(curr_wp(0),
176  _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].position[0]);
177  EXPECT_FLOAT_EQ(curr_wp(1),
178  _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].position[1]);
179  EXPECT_FLOAT_EQ(curr_wp(2),
180  _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].position[2]);
181  EXPECT_FLOAT_EQ(curr_yaw, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw);
182  EXPECT_FALSE(PX4_ISFINITE(
183  _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw_speed));
184  EXPECT_EQ(type, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].type);
185  EXPECT_TRUE(_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].point_valid);
186 
187  EXPECT_FLOAT_EQ(next_wp(0),
188  _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].position[0]);
189  EXPECT_FLOAT_EQ(next_wp(1),
190  _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].position[1]);
191  EXPECT_FLOAT_EQ(next_wp(2),
192  _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].position[2]);
193  EXPECT_FLOAT_EQ(next_yaw, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw);
194  EXPECT_FALSE(PX4_ISFINITE(
195  _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw_speed));
196  EXPECT_EQ(UINT8_MAX, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].type);
197  EXPECT_TRUE(_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].point_valid);
198 
199 }
void updateAvoidanceDesiredSetpoints(const matrix::Vector3f &pos_sp, const matrix::Vector3f &vel_sp, const int type)
Updates the desired setpoints to send to the obstacle avoidance system.
void injectAvoidanceSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, float &yaw_sp, float &yaw_speed_sp)
Inject setpoints from obstacle avoidance system into FlightTasks.
void test_setPosition(Vector3f &pos)
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
const vehicle_trajectory_waypoint_s empty_trajectory_waypoint
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
Vector3< float > Vector3f
Definition: Vector3.hpp:136
void updateAvoidanceDesiredWaypoints(const matrix::Vector3f &curr_wp, const float curr_yaw, const float curr_yawspeed, const matrix::Vector3f &next_wp, const float next_yaw, const float next_yawspeed, const bool ext_yaw_active, const int wp_type)
Updates the desired waypoints to send to the obstacle avoidance system.
__EXPORT void param_reset_all(void)
Reset all parameters to their default values.
Definition: parameters.cpp:910
TEST_F(ObstacleAvoidanceTest, instantiation)
struct trajectory_waypoint_s waypoints[5]
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
This class is used to inject the setpoints of an obstacle avoidance system into the FlightTasks...