From 3c622ab89b5264fe334971911bf0676d10cd7474 Mon Sep 17 00:00:00 2001 From: Naotaka Hatao Date: Wed, 23 Aug 2023 15:50:07 +0900 Subject: [PATCH] trajectory_tracker: make trajectory_tracker library (#713) --- trajectory_tracker/CMakeLists.txt | 5 + .../include/trajectory_tracker/path2d.h | 102 +++++++++++- trajectory_tracker/src/trajectory_tracker.cpp | 46 +----- trajectory_tracker/test/src/test_path2d.cpp | 147 ++++++++++++++++++ 4 files changed, 251 insertions(+), 49 deletions(-) diff --git a/trajectory_tracker/CMakeLists.txt b/trajectory_tracker/CMakeLists.txt index ab621dfde..0a99a8a22 100644 --- a/trajectory_tracker/CMakeLists.txt +++ b/trajectory_tracker/CMakeLists.txt @@ -26,6 +26,7 @@ generate_dynamic_reconfigure_options( ) catkin_package( + INCLUDE_DIRS include CATKIN_DEPENDS ${CATKIN_DEPENDS} ) @@ -72,3 +73,7 @@ install(TARGETS LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) diff --git a/trajectory_tracker/include/trajectory_tracker/path2d.h b/trajectory_tracker/include/trajectory_tracker/path2d.h index 6c53def26..9e90a34ba 100644 --- a/trajectory_tracker/include/trajectory_tracker/path2d.h +++ b/trajectory_tracker/include/trajectory_tracker/path2d.h @@ -30,17 +30,20 @@ #ifndef TRAJECTORY_TRACKER_PATH2D_H #define TRAJECTORY_TRACKER_PATH2D_H -#include #include +#include +#include #include #include #include +#include #include +#include -#include #include +#include namespace trajectory_tracker { @@ -63,12 +66,24 @@ class Pose2D , velocity_(velocity) { } - inline explicit Pose2D(const geometry_msgs::Pose& pose, float velocity) + inline Pose2D(const geometry_msgs::Pose& pose, float velocity) : pos_(Eigen::Vector2d(pose.position.x, pose.position.y)) , yaw_(tf2::getYaw(pose.orientation)) , velocity_(velocity) { } + inline explicit Pose2D(const geometry_msgs::PoseStamped& pose) + : pos_(Eigen::Vector2d(pose.pose.position.x, pose.pose.position.y)) + , yaw_(tf2::getYaw(pose.pose.orientation)) + , velocity_(std::numeric_limits::quiet_NaN()) + { + } + inline explicit Pose2D(const trajectory_tracker_msgs::PoseStampedWithVelocity& pose) + : pos_(Eigen::Vector2d(pose.pose.position.x, pose.pose.position.y)) + , yaw_(tf2::getYaw(pose.pose.orientation)) + , velocity_(pose.linear_velocity.x) + { + } inline void rotate(const float ang) { const float org_x = pos_.x(); @@ -84,6 +99,19 @@ class Pose2D while (yaw_ > 2 * M_PI) yaw_ -= 2 * M_PI; } + void toMsg(geometry_msgs::PoseStamped& pose) const + { + pose.pose.position.x = pos_.x(); + pose.pose.position.y = pos_.y(); + pose.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), yaw_)); + } + void toMsg(trajectory_tracker_msgs::PoseStampedWithVelocity& pose) const + { + pose.pose.position.x = pos_.x(); + pose.pose.position.y = pos_.y(); + pose.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), yaw_)); + pose.linear_velocity.x = velocity_; + } }; class Path2D : public std::vector { @@ -132,10 +160,24 @@ class Path2D : public std::vector const Eigen::Vector2d& target, const float max_search_range = 0, const float epsilon = 1e-6) const + { + return findNearestWithDistance(begin, end, target, max_search_range, epsilon).first; + } + inline std::pair findNearestWithDistance( + const ConstIterator& begin, + const ConstIterator& end, + const Eigen::Vector2d& target, + const float max_search_range = 0, + const float epsilon = 1e-6) const { if (begin == end) - return end; - + { + if (end == this->end()) + { + return std::make_pair(end, std::numeric_limits::max()); + } + return std::make_pair(end, (end->pos_ - target).norm()); + } float distance_path_search = 0; ConstIterator it_nearest = begin; float min_dist = (begin->pos_ - target).norm() + epsilon; @@ -165,7 +207,7 @@ class Path2D : public std::vector } it_prev = it; } - return it_nearest; + return std::make_pair(it_nearest, min_dist); } inline double remainedDistance( const ConstIterator& begin, @@ -245,6 +287,54 @@ class Path2D : public std::vector } return curv; } + // PATH_TYPE should be trajectory_tracker_msgs::PathWithVelocity or nav_msgs::Path + template + inline void fromMsg(const PATH_TYPE& path, const double in_place_turn_eps = 1.0e-6) + { + clear(); + bool in_place_turning = false; + trajectory_tracker::Pose2D in_place_turn_end; + for (const auto& pose : path.poses) + { + const trajectory_tracker::Pose2D next(pose); + if (empty()) + { + push_back(next); + continue; + } + if ((back().pos_ - next.pos_).squaredNorm() >= std::pow(in_place_turn_eps, 2)) + { + if (in_place_turning) + { + push_back(in_place_turn_end); + in_place_turning = false; + } + push_back(next); + } + else + { + in_place_turn_end = trajectory_tracker::Pose2D( + back().pos_, next.yaw_, next.velocity_); + in_place_turning = true; + } + } + if (in_place_turning) + { + push_back(in_place_turn_end); + } + } + // PATH_TYPE should be trajectory_tracker_msgs::PathWithVelocity or nav_msgs::Path + template + inline void toMsg(PATH_TYPE& path) const + { + path.poses.clear(); + path.poses.resize(size()); + for (size_t i = 0; i < size(); ++i) + { + path.poses[i].header = path.header; + at(i).toMsg(path.poses[i]); + } + } }; } // namespace trajectory_tracker diff --git a/trajectory_tracker/src/trajectory_tracker.cpp b/trajectory_tracker/src/trajectory_tracker.cpp index bb6124737..b7df6d834 100644 --- a/trajectory_tracker/src/trajectory_tracker.cpp +++ b/trajectory_tracker/src/trajectory_tracker.cpp @@ -282,62 +282,22 @@ void TrackerNode::cbSpeed(const std_msgs::Float32::ConstPtr& msg) vel_[0] = msg->data; } -namespace -{ -float getVelocity(const geometry_msgs::PoseStamped& msg) -{ - return std::numeric_limits::quiet_NaN(); -} -float getVelocity(const trajectory_tracker_msgs::PoseStampedWithVelocity& msg) -{ - return msg.linear_velocity.x; -} -} // namespace - template void TrackerNode::cbPath(const typename MSG_TYPE::ConstPtr& msg) { path_header_ = msg->header; - path_.clear(); is_path_updated_ = true; path_step_done_ = 0; - if (msg->poses.size() == 0) - return; - - trajectory_tracker::Pose2D in_place_turn_end; - bool in_place_turning = false; - - auto j = msg->poses.begin(); - path_.push_back(trajectory_tracker::Pose2D(j->pose, getVelocity(*j))); - for (++j; j < msg->poses.end(); ++j) + path_.fromMsg(*msg, epsilon_); + for (const auto& path_pose : path_) { - const float velocity = getVelocity(*j); - if (std::isfinite(velocity) && velocity < -0.0) + if (std::isfinite(path_pose.velocity_) && path_pose.velocity_ < -0.0) { ROS_ERROR_THROTTLE(1.0, "path_velocity.velocity.x must be positive"); path_.clear(); return; } - const trajectory_tracker::Pose2D next(j->pose, velocity); - - if ((path_.back().pos_ - next.pos_).squaredNorm() >= std::pow(epsilon_, 2)) - { - if (in_place_turning) - { - path_.push_back(in_place_turn_end); - in_place_turning = false; - } - path_.push_back(next); - } - else - { - in_place_turn_end = trajectory_tracker::Pose2D( - path_.back().pos_, next.yaw_, next.velocity_); - in_place_turning = true; - } } - if (in_place_turning) - path_.push_back(in_place_turn_end); } void TrackerNode::cbOdometry(const nav_msgs::Odometry::ConstPtr& odom) diff --git a/trajectory_tracker/test/src/test_path2d.cpp b/trajectory_tracker/test/src/test_path2d.cpp index 22fe1789e..411f463c6 100644 --- a/trajectory_tracker/test/src/test_path2d.cpp +++ b/trajectory_tracker/test/src/test_path2d.cpp @@ -28,13 +28,18 @@ */ #include +#include #include +#include #include +#include #include +#include #include #include +#include namespace { @@ -144,6 +149,148 @@ TEST(Path2D, LocalGoalWithSwitchBack) } } +TEST(Path2D, FindNearestWithDistance) +{ + trajectory_tracker::Path2D path; + path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(0.5, 0.5), 0, 1)); + path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(0.6, 0.5), 0, 1)); + path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(0.7, 0.5), 0, 1)); + path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(0.8, 0.6), M_PI / 4, 1)); + path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(0.9, 0.7), M_PI / 4, 1)); + path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(0.9, 0.8), M_PI / 2, 1)); + path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(0.9, 0.9), M_PI / 2, 1)); + + { + // The nearest line is (0.8, 0.6) - (0.9, 0.7), and the nearest point on the line is (0.85, 0.65). + const auto nearest_with_dist = path.findNearestWithDistance(path.begin(), path.end(), Eigen::Vector2d(1.0, 0.5)); + EXPECT_EQ(nearest_with_dist.first, path.begin() + 4); + EXPECT_NEAR(nearest_with_dist.second, std::sqrt(std::pow(1.0 - 0.85, 2) + std::pow(0.5 - 0.65, 2)), 1.0e-6); + } + { + // The nearest point is (0.7, 0.5). + const auto nearest_with_dist = + path.findNearestWithDistance(path.begin(), path.begin() + 3, Eigen::Vector2d(1.0, 0.5)); + EXPECT_EQ(nearest_with_dist.first, path.begin() + 2); + EXPECT_NEAR(nearest_with_dist.second, std::sqrt(std::pow(1.0 - 0.7, 2) + std::pow(0.5 - 0.5, 2)), 1.0e-6); + } + { + // Test edge cases + const auto nearest_with_dist = + path.findNearestWithDistance(path.begin() + 5, path.begin() + 5, Eigen::Vector2d(1.0, 0.5)); + EXPECT_EQ(nearest_with_dist.first, path.begin() + 5); + EXPECT_NEAR(nearest_with_dist.second, std::sqrt(std::pow(1.0 - 0.9, 2) + std::pow(0.5 - 0.8, 2)), 1.0e-6); + + const auto invalid_result = + path.findNearestWithDistance(path.end(), path.end(), Eigen::Vector2d(1.0, 0.5)); + EXPECT_EQ(invalid_result.first, path.end()); + EXPECT_EQ(invalid_result.second, std::numeric_limits::max()); + } +} + +TEST(Path2D, Conversions) +{ + nav_msgs::Path path_msg_org; + path_msg_org.poses.resize(8); + path_msg_org.poses[0].pose.position.x = 0.0; + path_msg_org.poses[0].pose.position.y = 0.0; + path_msg_org.poses[0].pose.orientation.w = 1.0; + path_msg_org.poses[1].pose.position.x = 1.0; // Start of in-place turning + path_msg_org.poses[1].pose.position.y = 0.0; + path_msg_org.poses[1].pose.orientation.w = 1.0; + path_msg_org.poses[2].pose.position.x = 1.0; + path_msg_org.poses[2].pose.position.y = 0.0; + path_msg_org.poses[2].pose.orientation.z = std::sin(M_PI / 8); + path_msg_org.poses[2].pose.orientation.w = std::cos(M_PI / 8); + path_msg_org.poses[3].pose.position.x = 1.0; // End of in-place turning + path_msg_org.poses[3].pose.position.y = 0.0; + path_msg_org.poses[3].pose.orientation.z = std::sin(M_PI / 4); + path_msg_org.poses[3].pose.orientation.w = std::cos(M_PI / 4); + path_msg_org.poses[4].pose.position.x = 1.0; + path_msg_org.poses[4].pose.position.y = 1.0; + path_msg_org.poses[4].pose.orientation.z = std::sin(M_PI / 4); + path_msg_org.poses[4].pose.orientation.w = std::cos(M_PI / 4); + path_msg_org.poses[5].pose.position.x = 1.0; // Start of in-place turning + path_msg_org.poses[5].pose.position.y = 2.0; + path_msg_org.poses[5].pose.orientation.z = std::sin(M_PI / 4); + path_msg_org.poses[5].pose.orientation.w = std::cos(M_PI / 4); + path_msg_org.poses[6].pose.position.x = 1.0; + path_msg_org.poses[6].pose.position.y = 2.0; + path_msg_org.poses[6].pose.orientation.z = std::sin(M_PI / 4 + M_PI / 6); + path_msg_org.poses[6].pose.orientation.w = std::cos(M_PI / 4 + M_PI / 6); + path_msg_org.poses[7].pose.position.x = 1.0; // End of in-place turning + path_msg_org.poses[7].pose.position.y = 2.0; + path_msg_org.poses[7].pose.orientation.z = std::sin(M_PI / 4 + M_PI / 3); + path_msg_org.poses[7].pose.orientation.w = std::cos(M_PI / 4 + M_PI / 3); + + trajectory_tracker::Path2D path; + path.fromMsg(path_msg_org); + + const std::vector expected_org_indexes = {0, 1, 3, 4, 5, 7}; + ASSERT_EQ(path.size(), 6); + for (size_t i = 0; i < path.size(); ++i) + { + const size_t org_index = expected_org_indexes[i]; + EXPECT_EQ(path[i].pos_.x(), path_msg_org.poses[org_index].pose.position.x) << "i: " << i << " org: " << org_index; + EXPECT_EQ(path[i].pos_.y(), path_msg_org.poses[org_index].pose.position.y) << "i: " << i << " org: " << org_index; + EXPECT_NEAR(path[i].yaw_, tf2::getYaw(path_msg_org.poses[org_index].pose.orientation), 1.0e-6) + << "i: " << i << " org: " << org_index; + EXPECT_TRUE(std::isnan(path[i].velocity_)); + } + + nav_msgs::Path path_msg; + path_msg.header.frame_id = "map"; + path_msg.header.stamp = ros::Time(123.456); + path.toMsg(path_msg); + ASSERT_EQ(path_msg.poses.size(), 6); + for (size_t i = 0; i < path.size(); ++i) + { + EXPECT_EQ(path[i].pos_.x(), path_msg.poses[i].pose.position.x) << "i: " << i; + EXPECT_EQ(path[i].pos_.y(), path_msg.poses[i].pose.position.y) << "i: " << i; + EXPECT_NEAR(path[i].yaw_, tf2::getYaw(path_msg.poses[i].pose.orientation), 1.0e-6) << "i: " << i; + EXPECT_EQ(path_msg.poses[i].header.frame_id, path_msg.header.frame_id); + EXPECT_EQ(path_msg.poses[i].header.stamp, path_msg.header.stamp); + } + + trajectory_tracker_msgs::PathWithVelocity path_with_vel_msg_org; + path_with_vel_msg_org.poses.resize(path_msg_org.poses.size()); + for (size_t i = 0; i < path_msg_org.poses.size(); ++i) + { + path_with_vel_msg_org.poses[i].pose = path_msg_org.poses[i].pose; + path_with_vel_msg_org.poses[i].linear_velocity.x = i * 0.1; + } + + trajectory_tracker::Path2D path_with_vel; + path_with_vel.fromMsg(path_with_vel_msg_org); + + ASSERT_EQ(path_with_vel.size(), 6); + for (size_t i = 0; i < path_with_vel.size(); ++i) + { + const size_t org_index = expected_org_indexes[i]; + EXPECT_EQ(path_with_vel[i].pos_.x(), path_with_vel_msg_org.poses[org_index].pose.position.x) + << "i: " << i << " org: " << org_index; + EXPECT_EQ(path_with_vel[i].pos_.y(), path_with_vel_msg_org.poses[org_index].pose.position.y) + << "i: " << i << " org: " << org_index; + EXPECT_NEAR(path_with_vel[i].yaw_, tf2::getYaw(path_with_vel_msg_org.poses[org_index].pose.orientation), 1.0e-6) + << "i: " << i << " org: " << org_index; + EXPECT_NEAR(path_with_vel[i].velocity_, org_index * 0.1, 1.0e-6) << "i: " << i << " org: " << org_index; + } + + trajectory_tracker_msgs::PathWithVelocity path_with_vel_msg; + path_with_vel_msg.header.frame_id = "map"; + path_with_vel_msg.header.stamp = ros::Time(123.456); + path_with_vel.toMsg(path_with_vel_msg); + ASSERT_EQ(path_with_vel_msg.poses.size(), 6); + for (size_t i = 0; i < path_with_vel.size(); ++i) + { + EXPECT_EQ(path_with_vel[i].pos_.x(), path_with_vel_msg.poses[i].pose.position.x) << "i: " << i; + EXPECT_EQ(path_with_vel[i].pos_.y(), path_with_vel_msg.poses[i].pose.position.y) << "i: " << i; + EXPECT_NEAR(path_with_vel[i].yaw_, tf2::getYaw(path_with_vel_msg.poses[i].pose.orientation), 1.0e-6) + << "i: " << i; + EXPECT_EQ(path_with_vel_msg.poses[i].header.frame_id, path_with_vel_msg.header.frame_id); + EXPECT_EQ(path_with_vel_msg.poses[i].header.stamp, path_with_vel_msg.header.stamp); + } +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv);