Skip to content

Commit

Permalink
Enable conversion with velocity
Browse files Browse the repository at this point in the history
  • Loading branch information
@nhatao
nhatao committed Aug 23, 2023
1 parent e4a3a1c commit e7935d9
Show file tree
Hide file tree
Showing 3 changed files with 95 additions and 63 deletions.
56 changes: 42 additions & 14 deletions trajectory_tracker/include/trajectory_tracker/path2d.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,15 +30,17 @@
#ifndef TRAJECTORY_TRACKER_PATH2D_H
#define TRAJECTORY_TRACKER_PATH2D_H

#include <vector>
#include <limits>
#include <utility>
#include <vector>

#include <Eigen/Core>
#include <Eigen/Geometry>

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

#include <trajectory_tracker/average.h>
#include <trajectory_tracker/eigen_line.h>
Expand All @@ -64,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<float>::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();
Expand All @@ -85,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<Pose2D>
{
Expand Down Expand Up @@ -260,15 +287,18 @@ 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)
// PATH_TYPE should be trajectory_tracker_msgs::PathWithVelocity or nav_msgs::Path
template <typename PATH_TYPE>
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.pose, 0.0);
const trajectory_tracker::Pose2D next(pose);
if (empty())
{
push_back(next);
Expand All @@ -295,18 +325,16 @@ class Path2D : public std::vector<Pose2D>
push_back(in_place_turn_end);
}
}
inline void toMsg(nav_msgs::Path& path) const
// PATH_TYPE should be trajectory_tracker_msgs::PathWithVelocity or nav_msgs::Path
template <typename PATH_TYPE>
inline void toMsg(PATH_TYPE& path) const
{
path.poses.clear();
for (const auto& pose : *this)
path.poses.resize(size());
for (size_t i = 0; i < size(); ++i)
{
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);
path.poses[i].header = path.header;
at(i).toMsg(path.poses[i]);
}
}
};
Expand Down
49 changes: 1 addition & 48 deletions trajectory_tracker/src/trajectory_tracker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -282,62 +282,15 @@ 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<float>::quiet_NaN();
}
float getVelocity(const trajectory_tracker_msgs::PoseStampedWithVelocity& msg)
{
return msg.linear_velocity.x;
}
} // namespace

template <typename MSG_TYPE>
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)
{
const float velocity = getVelocity(*j);
if (std::isfinite(velocity) && 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);
path_.fromMsg(*msg);
}

void TrackerNode::cbOdometry(const nav_msgs::Odometry::ConstPtr& odom)
Expand Down
53 changes: 52 additions & 1 deletion trajectory_tracker/test/src/test_path2d.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,14 +28,17 @@
*/

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

#include <gtest/gtest.h>

#include <nav_msgs/Path.h>
#include <trajectory_tracker/eigen_line.h>
#include <trajectory_tracker/path2d.h>
#include <trajectory_tracker_msgs/PathWithVelocity.h>

#include <ros/ros.h>

Expand Down Expand Up @@ -222,8 +225,8 @@ TEST(Path2D, Conversions)

trajectory_tracker::Path2D path;
path.fromMsg(path_msg_org);
ASSERT_EQ(path.size(), 6);

ASSERT_EQ(path.size(), 6);
for (size_t i = 0; i < path.size(); ++i)
{
size_t org_idx = i;
Expand All @@ -239,6 +242,7 @@ TEST(Path2D, Conversions)
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;
EXPECT_TRUE(std::isnan(path[i].velocity_));
}

nav_msgs::Path path_msg;
Expand All @@ -254,6 +258,53 @@ TEST(Path2D, Conversions)
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)
{
size_t org_idx = i;
if (i == 5)
{
org_idx += 2;
}
else if (i >= 2)
{
org_idx += 1;
}
EXPECT_EQ(path_with_vel[i].pos_.x(), path_with_vel_msg_org.poses[org_idx].pose.position.x)
<< "idx: " << i << " org_idx: " << org_idx;
EXPECT_EQ(path_with_vel[i].pos_.y(), path_with_vel_msg_org.poses[org_idx].pose.position.y)
<< "idx: " << i << " org_idx: " << org_idx;
EXPECT_NEAR(path_with_vel[i].yaw_, tf2::getYaw(path_with_vel_msg_org.poses[org_idx].pose.orientation), 1.0e-6)
<< "idx: " << i << " org_idx: " << org_idx;
EXPECT_NEAR(path_with_vel[i].velocity_, org_idx * 0.1, 1.0e-6) << "idx: " << i << " org_idx: " << org_idx;
}

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

0 comments on commit e7935d9

Please sign in to comment.