Skip to content

Commit

Permalink
trajectory_tracker: make trajectory_tracker library
Browse files Browse the repository at this point in the history
  • Loading branch information
@nhatao
nhatao committed Aug 23, 2023
1 parent 0435013 commit e4a3a1c
Show file tree
Hide file tree
Showing 3 changed files with 185 additions and 4 deletions.
5 changes: 5 additions & 0 deletions trajectory_tracker/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@ generate_dynamic_reconfigure_options(
)

catkin_package(
INCLUDE_DIRS include
CATKIN_DEPENDS ${CATKIN_DEPENDS}
)

Expand Down Expand Up @@ -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}
)
72 changes: 68 additions & 4 deletions trajectory_tracker/include/trajectory_tracker/path2d.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,10 +37,11 @@
#include <Eigen/Geometry>

#include <geometry_msgs/Pose.h>
#include <nav_msgs/Path.h>
#include <tf2/utils.h>

#include <trajectory_tracker/eigen_line.h>
#include <trajectory_tracker/average.h>
#include <trajectory_tracker/eigen_line.h>

namespace trajectory_tracker
{
Expand Down Expand Up @@ -132,10 +133,24 @@ class Path2D : public std::vector<Pose2D>
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<ConstIterator, double> 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<double>::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;
Expand Down Expand Up @@ -165,7 +180,7 @@ class Path2D : public std::vector<Pose2D>
}
it_prev = it;
}
return it_nearest;
return std::make_pair(it_nearest, min_dist);
}
inline double remainedDistance(
const ConstIterator& begin,
Expand Down Expand Up @@ -245,6 +260,55 @@ class Path2D : public std::vector<Pose2D>
}
return curv;
}
inline void fromMsg(const nav_msgs::Path& 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.pose, 0.0);
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);
}
}
inline void toMsg(nav_msgs::Path& path) const
{
path.poses.clear();
for (const auto& pose : *this)
{
geometry_msgs::PoseStamped pose_msg;
pose_msg.header = path.header;
pose_msg.pose.position.x = pose.pos_.x();
pose_msg.pose.position.y = pose.pos_.y();
pose_msg.pose.orientation.z = std::sin(pose.yaw_ / 2);
pose_msg.pose.orientation.w = std::cos(pose.yaw_ / 2);
path.poses.push_back(pose_msg);
}
}
};
} // namespace trajectory_tracker

Expand Down
112 changes: 112 additions & 0 deletions trajectory_tracker/test/src/test_path2d.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,13 +29,16 @@

#include <algorithm>
#include <cstddef>
#include <limits>
#include <string>

#include <gtest/gtest.h>

#include <trajectory_tracker/eigen_line.h>
#include <trajectory_tracker/path2d.h>

#include <ros/ros.h>

namespace
{
double getRemainedDistance(const trajectory_tracker::Path2D& path, const Eigen::Vector2d& p)
Expand Down Expand Up @@ -144,6 +147,115 @@ 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<double>::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);
ASSERT_EQ(path.size(), 6);

for (size_t i = 0; i < path.size(); ++i)
{
size_t org_idx = i;
if (i == 5)
{
org_idx += 2;
}
else if (i >= 2)
{
org_idx += 1;
}
EXPECT_EQ(path[i].pos_.x(), path_msg_org.poses[org_idx].pose.position.x) << "idx: " << i << " org_idx: " << org_idx;
EXPECT_EQ(path[i].pos_.y(), path_msg_org.poses[org_idx].pose.position.y) << "idx: " << i << " org_idx: " << org_idx;
EXPECT_NEAR(path[i].yaw_, tf2::getYaw(path_msg_org.poses[org_idx].pose.orientation), 1.0e-6)
<< "idx: " << i << " org_idx: " << org_idx;
}

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) << "idx: " << i;
EXPECT_EQ(path[i].pos_.y(), path_msg.poses[i].pose.position.y) << "idx: " << i;
EXPECT_NEAR(path[i].yaw_, tf2::getYaw(path_msg.poses[i].pose.orientation), 1.0e-6) << "idx: " << 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);
}
}

int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
Expand Down

0 comments on commit e4a3a1c

Please sign in to comment.