38 initState(Eigen::Vector2d(0, 0), 0);
40 std::vector<Eigen::Vector3d> poses;
41 for (
double x = 0.0;
x < 0.5;
x += 0.01)
42 poses.push_back(Eigen::Vector3d(
x, 0.0, 0.0));
43 poses.push_back(Eigen::Vector3d(0.5, 0.0, 0.0));
55 if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL)
58 for (
int j = 0; j < 5; ++j)
60 for (
int i = 0; i < 5; ++i)
68 ASSERT_NEAR(yaw_, 0.0, error_ang_)
69 <<
"[overshoot after goal (" << j <<
")] ";
70 ASSERT_NEAR(pos_[0], 0.5, error_lin_)
71 <<
"[overshoot after goal (" << j <<
")] ";
72 ASSERT_NEAR(pos_[1], 0.0, error_lin_)
73 <<
"[overshoot after goal (" << j <<
")] ";
75 ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp);
80 const double resolutions[] =
86 for (
const double resolution : resolutions)
88 const std::string info_message =
"resolution: " + std::to_string(resolution);
90 initState(Eigen::Vector2d(1, 0), 0);
92 std::vector<Eigen::Vector3d> poses;
93 for (
double x = 0.0;
x < 0.5 - resolution;
x += 0.1)
94 poses.push_back(Eigen::Vector3d(
x, 0, 0));
95 poses.push_back(Eigen::Vector3d(0.5 - resolution, 0, 0));
96 poses.push_back(Eigen::Vector3d(0.5, 0, 0));
108 if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL)
111 for (
int j = 0; j < 5; ++j)
113 for (
int i = 0; i < 5; ++i)
121 ASSERT_NEAR(yaw_, 0.0, error_ang_)
122 <<
"[overshoot after goal (" << j <<
")] " 124 EXPECT_NEAR(pos_[0], 0.5, error_lin_)
125 <<
"[overshoot after goal (" << j <<
")] " 127 ASSERT_NEAR(pos_[1], 0.0, error_lin_)
128 <<
"[overshoot after goal (" << j <<
")] " 131 ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp) << info_message;
137 const double vels[] =
139 0.02, 0.05, 0.1, 0.2, 0.5, 1.0
141 const double path_length = 2.0;
142 for (
const double vel : vels)
144 const std::string info_message =
"linear vel: " + std::to_string(vel);
146 initState(Eigen::Vector2d(0, 0.01), 0);
148 std::vector<Eigen::Vector4d> poses;
149 for (
double x = 0.0;
x < path_length;
x += 0.01)
150 poses.push_back(Eigen::Vector4d(
x, 0.0, 0.0, vel));
151 poses.push_back(Eigen::Vector4d(path_length, 0.0, 0.0, vel));
163 if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL)
166 for (
int j = 0; j < 5; ++j)
168 for (
int i = 0; i < 5; ++i)
176 EXPECT_NEAR(yaw_, 0.0, error_ang_)
177 <<
"[overshoot after goal (" << j <<
")] " 179 EXPECT_NEAR(pos_[0], path_length, error_lin_)
180 <<
"[overshoot after goal (" << j <<
")] " 182 EXPECT_NEAR(pos_[1], 0.0, error_lin_)
183 <<
"[overshoot after goal (" << j <<
")] " 186 ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp);
192 initState(Eigen::Vector2d(0, 0), 0);
194 std::vector<Eigen::Vector4d> poses;
195 for (
double x = 0.0;
x < 0.6;
x += 0.01)
196 poses.push_back(Eigen::Vector4d(
x, 0.0, 0.0, 0.3));
197 for (
double x = 0.6;
x < 1.5;
x += 0.01)
198 poses.push_back(Eigen::Vector4d(
x, 0.0, 0.0, 0.5));
199 poses.push_back(Eigen::Vector4d(1.5, 0.0, 0.0, 0.5));
212 if (0.3 < pos_[0] && pos_[0] < 0.35)
214 ASSERT_NEAR(cmd_vel_->linear.x, 0.3, error_lin_);
216 else if (0.95 < pos_[0] && pos_[0] < 1.0)
218 ASSERT_NEAR(cmd_vel_->linear.x, 0.5, error_lin_);
221 if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL)
224 for (
int j = 0; j < 5; ++j)
226 for (
int i = 0; i < 5; ++i)
234 ASSERT_NEAR(yaw_, 0.0, error_ang_)
235 <<
"[overshoot after goal (" << j <<
")] ";
236 ASSERT_NEAR(pos_[0], 1.5, error_lin_)
237 <<
"[overshoot after goal (" << j <<
")] ";
238 ASSERT_NEAR(pos_[1], 0.0, error_lin_)
239 <<
"[overshoot after goal (" << j <<
")] ";
241 ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp);
246 initState(Eigen::Vector2d(0, 0), 0);
248 std::vector<Eigen::Vector3d> poses;
249 Eigen::Vector3d p(0.0, 0.0, 0.0);
250 for (
double t = 0.0;
t < 1.0;
t += 0.01)
252 p += Eigen::Vector3d(std::cos(p[2]) * 0.05, std::sin(p[2]) * 0.05, 0.005);
255 for (
double t = 0.0;
t < 1.0;
t += 0.01)
257 p += Eigen::Vector3d(std::cos(p[2]) * 0.05, std::sin(p[2]) * 0.05, 0.0);
271 if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL)
274 for (
int j = 0; j < 5; ++j)
276 for (
int i = 0; i < 5; ++i)
284 ASSERT_NEAR(yaw_, p[2], error_ang_)
285 <<
"[overshoot after goal (" << j <<
")] ";
286 ASSERT_NEAR(pos_[0], p[0], error_large_lin_)
287 <<
"[overshoot after goal (" << j <<
")] ";
288 ASSERT_NEAR(pos_[1], p[1], error_large_lin_)
289 <<
"[overshoot after goal (" << j <<
")] ";
291 ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp);
296 const float init_yaw_array[] =
301 for (
const float init_yaw : init_yaw_array)
303 const std::vector<float> target_angle_array[] =
307 { 0.1, 0.2, 0.3, 0.4, 0.5 },
308 { -0.1, -0.2, -0.3, -0.4, -0.5 },
310 for (
const auto&
angles : target_angle_array)
312 for (
const bool& has_short_path : {
false,
true })
314 std::stringstream condition_name;
316 <<
"init_yaw: " << init_yaw
317 <<
", angles: " <<
angles.front() <<
"-" <<
angles.back()
318 <<
", has_short_path: " << has_short_path;
320 initState(Eigen::Vector2d(0, 0), init_yaw);
322 std::vector<Eigen::Vector3d> poses;
325 poses.push_back(Eigen::Vector3d(-std::cos(init_yaw) * 0.01, std::sin(init_yaw) * 0.01, init_yaw));
329 poses.push_back(Eigen::Vector3d(0.0, 0.0, init_yaw + ang));
335 for (
int i = 0;
ros::ok(); ++i)
343 if (cmd_vel_ && i > 5)
345 ASSERT_GT(cmd_vel_->angular.z * std::copysign(1.0, angles.back()), -error_ang_)
346 <<
"[overshoot detected] " 347 << condition_name.str();
349 if (status_ && i > 5)
351 ASSERT_LT(status_->angle_remains * std::copysign(1.0, angles.back()), error_ang_)
352 <<
"[overshoot detected] " 353 << condition_name.str();
356 if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL)
359 ASSERT_TRUE(static_cast<bool>(cmd_vel_)) << condition_name.str();
360 for (
int j = 0; j < 5; ++j)
362 for (
int i = 0; i < 5; ++i)
370 ASSERT_NEAR(yaw_, init_yaw + angles.back(), error_ang_)
371 <<
"[overshoot after goal (" << j <<
")] " 372 << condition_name.str();
374 ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp);
382 initState(Eigen::Vector2d(0, 0), 0);
384 std::vector<Eigen::Vector3d> poses;
385 Eigen::Vector3d p(0.0, 0.0, 0.0);
386 for (
double t = 0.0;
t < 0.5;
t += 0.01)
388 p -= Eigen::Vector3d(std::cos(p[2]) * 0.05, std::sin(p[2]) * 0.05, -0.01);
391 for (
double t = 0.0;
t < 0.5;
t += 0.01)
393 p += Eigen::Vector3d(std::cos(p[2]) * 0.05, std::sin(p[2]) * 0.05, 0.01);
407 if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL)
410 for (
int j = 0; j < 5; ++j)
412 for (
int i = 0; i < 5; ++i)
420 ASSERT_NEAR(yaw_, p[2], error_ang_)
421 <<
"[overshoot after goal (" << j <<
")] ";
422 ASSERT_NEAR(pos_[0], p[0], error_large_lin_)
423 <<
"[overshoot after goal (" << j <<
")] ";
424 ASSERT_NEAR(pos_[1], p[1], error_large_lin_)
425 <<
"[overshoot after goal (" << j <<
")] ";
427 ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp);
432 initState(Eigen::Vector2d(0, 0), 0);
434 std::vector<Eigen::Vector3d> poses;
435 std::vector<Eigen::Vector3d> poses_second_half;
436 Eigen::Vector3d p(0.0, 0.0, 0.0);
437 for (
double t = 0.0;
t < 0.5;
t += 0.01)
439 p -= Eigen::Vector3d(std::cos(p[2]) * 0.05, std::sin(p[2]) * 0.05, -0.01);
442 const Eigen::Vector2d pos_local_goal = p.head<2>();
443 for (
double t = 0.0;
t < 1.0;
t += 0.01)
445 p += Eigen::Vector3d(std::cos(p[2]) * 0.05, std::sin(p[2]) * 0.05, 0.01);
447 poses_second_half.push_back(p);
451 int cnt_arrive_local_goal(0);
461 if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL)
464 if ((pos_local_goal - pos_).norm() < 0.1)
465 cnt_arrive_local_goal++;
467 if (cnt_arrive_local_goal > 25)
468 publishPath(poses_second_half);
472 for (
int j = 0; j < 5; ++j)
474 for (
int i = 0; i < 5; ++i)
482 ASSERT_NEAR(yaw_, p[2], error_ang_)
483 <<
"[overshoot after goal (" << j <<
")] ";
484 ASSERT_NEAR(pos_[0], p[0], error_large_lin_)
485 <<
"[overshoot after goal (" << j <<
")] ";
486 ASSERT_NEAR(pos_[1], p[1], error_large_lin_)
487 <<
"[overshoot after goal (" << j <<
")] ";
489 ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp);
496 nh.
param(
"/use_sim_time", use_sim_time,
false);
506 rosgraph_msgs::Clock clock;
507 clock.clock.fromNSec(time.
toNSec());
514 int main(
int argc,
char** argv)
516 testing::InitGoogleTest(&argc, argv);
517 ros::init(argc, argv,
"test_trajectory_tracker");
521 return RUN_ALL_TESTS();
void publish(const boost::shared_ptr< M > &message) const
void publishPath(const std::vector< Eigen::Vector3d > &poses)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
geometry_msgs::TransformStamped t
int main(int argc, char **argv)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
TEST_F(TrajectoryTrackerTest, StraightStop)
TFSIMD_FORCE_INLINE const tfScalar & x() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void publishPathVelocity(const std::vector< Eigen::Vector4d > &poses)
ROSCPP_DECL void spinOnce()