30 #ifndef TRAJECTORY_TRACKER_TEST_H 31 #define TRAJECTORY_TRACKER_TEST_H 38 #include <boost/thread.hpp> 41 #include <Eigen/Geometry> 45 #include <geometry_msgs/Twist.h> 46 #include <nav_msgs/Odometry.h> 47 #include <nav_msgs/Path.h> 48 #include <rosgraph_msgs/Clock.h> 52 #include <trajectory_tracker_msgs/PathWithVelocity.h> 53 #include <trajectory_tracker_msgs/TrajectoryTrackerStatus.h> 55 #include <gtest/gtest.h> 83 void cbStatus(
const trajectory_tracker_msgs::TrajectoryTrackerStatus::ConstPtr& msg)
87 void cbCmdVel(
const geometry_msgs::Twist::ConstPtr& msg)
92 const float dt = std::min((now - cmd_vel_time_).toSec(), 0.1);
94 yaw_ += msg->angular.z * dt;
95 pos_ += Eigen::Vector2d(std::cos(
yaw_), std::sin(
yaw_)) * msg->linear.x * dt;
104 trajectory_tracker_msgs::TrajectoryTrackerStatus::ConstPtr
status_;
116 pub_path_ = nh_.
advertise<nav_msgs::Path>(
"path", 1,
true);
117 pub_path_vel_ = nh_.
advertise<trajectory_tracker_msgs::PathWithVelocity>(
"path_velocity", 1,
true);
118 pub_odom_ = nh_.
advertise<nav_msgs::Odometry>(
"odom", 10,
true);
121 pnh_.
param(
"odom_delay", delay, 0.0);
123 pnh_.
param(
"error_lin", error_lin_, 0.01);
124 pnh_.
param(
"error_large_lin", error_large_lin_, 0.1);
125 pnh_.
param(
"error_ang", error_ang_, 0.01);
128 for (
size_t i = 0; i < 100; ++i)
136 void initState(
const Eigen::Vector2d& pos,
const float yaw)
144 path.header.frame_id =
"odom";
155 status_->status != trajectory_tracker_msgs::TrajectoryTrackerStatus::FOLLOWING)
158 <<
"trajectory_tracker status timeout, status: " 159 << (status_ ? std::to_string(static_cast<int>(status_->status)) :
"none");
175 status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::FOLLOWING)
178 <<
"trajectory_tracker status timeout, status: " 179 << (status_ ? std::to_string(static_cast<int>(status_->status)) :
"none");
187 path.header.frame_id =
"odom";
190 for (
const Eigen::Vector3d& p : poses)
192 const Eigen::Quaterniond q(Eigen::AngleAxisd(p[2], Eigen::Vector3d(0, 0, 1)));
194 geometry_msgs::PoseStamped pose;
195 pose.header.frame_id = path.header.frame_id;
196 pose.pose.position.x = p[0];
197 pose.pose.position.y = p[1];
198 pose.pose.orientation.x = q.x();
199 pose.pose.orientation.y = q.y();
200 pose.pose.orientation.z = q.z();
201 pose.pose.orientation.w = q.w();
203 path.poses.push_back(pose);
206 last_path_header_ = path.header;
210 trajectory_tracker_msgs::PathWithVelocity path;
211 path.header.frame_id =
"odom";
214 for (
const Eigen::Vector4d& p : poses)
216 const Eigen::Quaterniond q(Eigen::AngleAxisd(p[2], Eigen::Vector3d(0, 0, 1)));
218 trajectory_tracker_msgs::PoseStampedWithVelocity pose;
219 pose.header.frame_id = path.header.frame_id;
220 pose.pose.position.x = p[0];
221 pose.pose.position.y = p[1];
222 pose.pose.orientation.x = q.x();
223 pose.pose.orientation.y = q.y();
224 pose.pose.orientation.z = q.z();
225 pose.pose.orientation.w = q.w();
226 pose.linear_velocity.x = p[3];
228 path.poses.push_back(pose);
233 last_path_header_ = path.header;
239 const Eigen::Quaterniond q(Eigen::AngleAxisd(yaw_, Eigen::Vector3d(0, 0, 1)));
241 nav_msgs::Odometry odom;
242 odom.header.frame_id =
"odom";
243 odom.header.stamp = now;
244 odom.child_frame_id =
"base_link";
245 odom.pose.pose.position.x = pos_[0];
246 odom.pose.pose.position.y = pos_[1];
247 odom.pose.pose.position.z = 0.0;
248 odom.pose.pose.orientation.x = q.x();
249 odom.pose.pose.orientation.y = q.y();
250 odom.pose.pose.orientation.z = q.z();
251 odom.pose.pose.orientation.w = q.w();
254 odom.twist.twist.linear = cmd_vel_->linear;
255 odom.twist.twist.angular = cmd_vel_->angular;
258 odom_buffer_.push_back(odom);
262 while (odom_buffer_.size() > 0)
264 nav_msgs::Odometry odom = odom_buffer_.front();
265 if (odom.header.stamp > pub_time)
268 odom_buffer_.pop_front();
270 if (odom.header.stamp != trans_stamp_last_)
272 geometry_msgs::TransformStamped trans;
273 trans.header = odom.header;
275 trans.child_frame_id = odom.child_frame_id;
276 trans.transform.translation.x = odom.pose.pose.position.x;
277 trans.transform.translation.y = odom.pose.pose.position.y;
278 trans.transform.rotation.x = odom.pose.pose.orientation.x;
279 trans.transform.rotation.y = odom.pose.pose.orientation.y;
280 trans.transform.rotation.z = odom.pose.pose.orientation.z;
281 trans.transform.rotation.w = odom.pose.pose.orientation.w;
286 trans_stamp_last_ = odom.header.stamp;
295 #endif // TRAJECTORY_TRACKER_TEST_H
void publish(const boost::shared_ptr< M > &message) const
void publishPath(const std::vector< Eigen::Vector3d > &poses)
geometry_msgs::Twist::ConstPtr cmd_vel_
tf2_ros::TransformBroadcaster tfb_
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
std_msgs::Header last_path_header_
void cbCmdVel(const geometry_msgs::Twist::ConstPtr &msg)
ros::Publisher pub_path_vel_
std::list< nav_msgs::Odometry > odom_buffer_
trajectory_tracker_msgs::TrajectoryTrackerStatus::ConstPtr status_
ros::Subscriber sub_cmd_vel_
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
ros::Subscriber sub_status_
void initState(const Eigen::Vector2d &pos, const float yaw)
ros::Time trans_stamp_last_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void publishPathVelocity(const std::vector< Eigen::Vector4d > &poses)
ros::Time initial_cmd_vel_time_
uint32_t getNumSubscribers() const
void cbStatus(const trajectory_tracker_msgs::TrajectoryTrackerStatus::ConstPtr &msg)
double getCmdVelFrameRate() const
void waitUntilStart(const std::function< void()> func=nullptr)
ROSCPP_DECL void spinOnce()