36 initState(Eigen::Vector2d(0, 0), 0);
38 std::vector<Eigen::Vector3d> poses;
39 for (
double x = 0.0;
x < 0.5;
x += 0.01)
40 poses.push_back(Eigen::Vector3d(
x, 0.0, 0.0));
41 poses.push_back(Eigen::Vector3d(0.5, 0.0, 0.0));
53 if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL)
56 const double frame_rate = getCmdVelFrameRate();
57 for (
int i = 0; i < 25; ++i)
63 ASSERT_NEAR(yaw_, 0.0, 1e-2);
64 ASSERT_NEAR(pos_[0], 0.5, 1e-2);
65 ASSERT_NEAR(pos_[1], 0.0, 1e-2);
66 ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp);
68 ASSERT_NEAR(50.0, frame_rate, 10.0);
73 initState(Eigen::Vector2d(0, 0), 0);
75 std::vector<Eigen::Vector3d> poses;
76 for (
double x = 0.0;
x < 2.0;
x += 0.01)
77 poses.push_back(Eigen::Vector3d(
x, 0.0, 0.0));
78 poses.push_back(Eigen::Vector3d(2.0, 0.0, 0.0));
82 for (
int i = 0; i < 50; ++i)
92 ASSERT_FLOAT_EQ(cmd_vel_->linear.x, 0.0);
93 ASSERT_FLOAT_EQ(cmd_vel_->angular.z, 0.0);
94 ASSERT_GT(pos_[0], 0.0);
95 ASSERT_LT(pos_[0], 2.0);
96 ASSERT_EQ(status_->status, trajectory_tracker_msgs::TrajectoryTrackerStatus::NO_PATH);
103 if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL)
106 for (
int i = 0; i < 25; ++i)
112 ASSERT_NEAR(yaw_, 0.0, 1e-2);
113 ASSERT_NEAR(pos_[0], 2.0, 1e-2);
114 ASSERT_NEAR(pos_[1], 0.0, 1e-2);
115 ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp);
118 int main(
int argc,
char** argv)
120 testing::InitGoogleTest(&argc, argv);
121 ros::init(argc, argv,
"test_trajectory_tracker_frame_rate");
123 return RUN_ALL_TESTS();
void publishPath(const std::vector< Eigen::Vector3d > &poses)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
TFSIMD_FORCE_INLINE const tfScalar & x() const
int main(int argc, char **argv)
ROSCPP_DECL void spinOnce()
TEST_F(TrajectoryTrackerTest, FrameRate)