34 #include <gtest/gtest.h> 47 float yaw_sp = 0.123f;
48 float yaw_speed_sp = NAN;
76 message.
type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS;
80 message.
waypoints[vehicle_trajectory_waypoint_s::POINT_0].
yaw = 0.23f;
85 vehicle_trajectory_waypoint_pub.publish(message);
88 vehicle_status.
nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
90 vehicle_status_pub.publish(vehicle_status);
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));
115 vehicle_trajectory_waypoint_pub.publish(message);
118 vehicle_status.
nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
120 vehicle_status_pub.publish(vehicle_status);
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));
143 float curr_yaw = 1.02f;
144 float curr_yawspeed = NAN;
146 float next_yaw = 0.82f;
147 float next_yawspeed = NAN;
148 bool ext_yaw_active =
false;
157 _sub_traj_wp_avoidance_desired.update();
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);
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);
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);
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.
const vehicle_trajectory_waypoint_s empty_trajectory_waypoint
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Vector3< float > Vector3f
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.
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...