From 847ed88fd3acb0bfb12c8a2f8d0b1f3dccd12412 Mon Sep 17 00:00:00 2001 From: Naotaka Hatao Date: Mon, 30 Aug 2021 19:21:07 +0900 Subject: [PATCH] trajectory_tracker: add velocity tolerance parameters (#607) --- trajectory_tracker/cfg/TrajectoryTracker.cfg | 2 + trajectory_tracker/src/trajectory_tracker.cpp | 25 +++- trajectory_tracker/test/CMakeLists.txt | 10 ++ .../test/include/trajectory_tracker_test.h | 40 +++++- .../src/test_trajectory_tracker_overshoot.cpp | 134 ++++++++++++++++++ .../trajectory_tracker_overshoot_rostest.test | 29 ++++ 6 files changed, 231 insertions(+), 9 deletions(-) create mode 100644 trajectory_tracker/test/src/test_trajectory_tracker_overshoot.cpp create mode 100644 trajectory_tracker/test/test/trajectory_tracker_overshoot_rostest.test diff --git a/trajectory_tracker/cfg/TrajectoryTracker.cfg b/trajectory_tracker/cfg/TrajectoryTracker.cfg index 047082562..3b51347c4 100755 --- a/trajectory_tracker/cfg/TrajectoryTracker.cfg +++ b/trajectory_tracker/cfg/TrajectoryTracker.cfg @@ -35,5 +35,7 @@ gen.add("use_time_optimal_control", bool_t, 0, "", True) gen.add("time_optimal_control_future_gain", double_t, 0, "", 1.5, 0.0, 100.0) gen.add("k_ang_rotation", double_t, 0, "", 1.0, 0.0, 100.0) gen.add("k_avel_rotation", double_t, 0, "", 1.0, 0.0, 100.0) +gen.add("goal_tolerance_lin_vel", double_t, 0, "", 0.0, 0.0, 100.0) +gen.add("goal_tolerance_ang_vel", double_t, 0, "", 0.0, 0.0, 100.0) exit(gen.generate(PACKAGE, "trajectory_tracker", "TrajectoryTracker")) diff --git a/trajectory_tracker/src/trajectory_tracker.cpp b/trajectory_tracker/src/trajectory_tracker.cpp index 526199660..715a6e7a1 100644 --- a/trajectory_tracker/src/trajectory_tracker.cpp +++ b/trajectory_tracker/src/trajectory_tracker.cpp @@ -116,6 +116,8 @@ class TrackerNode double time_optimal_control_future_gain_; double k_ang_rotation_; double k_avel_rotation_; + double goal_tolerance_lin_vel_; + double goal_tolerance_ang_vel_; ros::Subscriber sub_path_; ros::Subscriber sub_path_velocity_; @@ -182,9 +184,9 @@ class TrackerNode void cbOdometry(const nav_msgs::Odometry::ConstPtr&); void cbTimer(const ros::TimerEvent&); void cbOdomTimeout(const ros::TimerEvent&); - void control(const tf2::Stamped&, const Eigen::Vector3d&, const double); + void control(const tf2::Stamped&, const Eigen::Vector3d&, const double, const double, const double); TrackingResult getTrackingResult( - const tf2::Stamped&, const Eigen::Vector3d&) const; + const tf2::Stamped&, const Eigen::Vector3d&, const double, const double) const; void cbParameter(const TrajectoryTrackerConfig& config, const uint32_t /* level */); }; @@ -263,6 +265,8 @@ void TrackerNode::cbParameter(const TrajectoryTrackerConfig& config, const uint3 time_optimal_control_future_gain_ = config.time_optimal_control_future_gain; k_ang_rotation_ = config.k_ang_rotation; k_avel_rotation_ = config.k_avel_rotation; + goal_tolerance_lin_vel_ = config.goal_tolerance_lin_vel; + goal_tolerance_ang_vel_ = config.goal_tolerance_ang_vel; } TrackerNode::~TrackerNode() @@ -389,7 +393,7 @@ void TrackerNode::cbOdometry(const nav_msgs::Odometry::ConstPtr& odom) odom_to_robot.inverse(), odom->header.stamp, odom->header.frame_id); - control(robot_to_odom, prediction_offset, dt); + control(robot_to_odom, prediction_offset, odom->twist.twist.linear.x, odom->twist.twist.angular.z, dt); } prev_odom_stamp_ = odom->header.stamp; } @@ -401,7 +405,7 @@ void TrackerNode::cbTimer(const ros::TimerEvent& event) tf2::Stamped transform; tf2::fromMsg( tfbuf_.lookupTransform(frame_robot_, frame_odom_, ros::Time(0)), transform); - control(transform, Eigen::Vector3d(0, 0, 0), 1.0 / hz_); + control(transform, Eigen::Vector3d(0, 0, 0), 0, 0, 1.0 / hz_); } catch (tf2::TransformException& e) { @@ -449,6 +453,8 @@ void TrackerNode::spin() void TrackerNode::control( const tf2::Stamped& robot_to_odom, const Eigen::Vector3d& prediction_offset, + const double odom_linear_vel, + const double odom_angular_vel, const double dt) { trajectory_tracker_msgs::TrajectoryTrackerStatus status; @@ -457,11 +463,13 @@ void TrackerNode::control( if (is_path_updated_) { // Call getTrackingResult to update path_step_done_. - const TrackingResult initial_tracking_result = getTrackingResult(robot_to_odom, prediction_offset); + const TrackingResult initial_tracking_result = + getTrackingResult(robot_to_odom, prediction_offset, odom_linear_vel, odom_angular_vel); path_step_done_ = initial_tracking_result.path_step_done; is_path_updated_ = false; } - const TrackingResult tracking_result = getTrackingResult(robot_to_odom, prediction_offset); + const TrackingResult tracking_result = + getTrackingResult(robot_to_odom, prediction_offset, odom_linear_vel, odom_angular_vel); switch (tracking_result.status) { case trajectory_tracker_msgs::TrajectoryTrackerStatus::NO_PATH: @@ -548,7 +556,8 @@ void TrackerNode::control( } TrackerNode::TrackingResult TrackerNode::getTrackingResult( - const tf2::Stamped& robot_to_odom, const Eigen::Vector3d& prediction_offset) const + const tf2::Stamped& robot_to_odom, const Eigen::Vector3d& prediction_offset, + const double odom_linear_vel, const double odom_angular_vel) const { if (path_header_.frame_id.size() == 0 || path_.size() == 0) { @@ -725,6 +734,8 @@ TrackerNode::TrackingResult TrackerNode::getTrackingResult( std::abs(result.angle_remains) < goal_tolerance_ang_ && std::abs(result.distance_remains_raw) < goal_tolerance_dist_ && std::abs(result.angle_remains_raw) < goal_tolerance_ang_ && + (goal_tolerance_lin_vel_ == 0.0 || std::abs(odom_linear_vel) < goal_tolerance_lin_vel_) && + (goal_tolerance_ang_vel_ == 0.0 || std::abs(odom_angular_vel) < goal_tolerance_ang_vel_) && it_local_goal == lpath.end()) { result.status = trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL; diff --git a/trajectory_tracker/test/CMakeLists.txt b/trajectory_tracker/test/CMakeLists.txt index 8b76463bf..e4fa8253e 100644 --- a/trajectory_tracker/test/CMakeLists.txt +++ b/trajectory_tracker/test/CMakeLists.txt @@ -1,5 +1,10 @@ include_directories(include) +if(NOT "$ENV{ROS_DISTRO}" STREQUAL "noetic") + # This flag is needed to compile dynamic_reconfigure::Client on Melodic + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-reorder") +endif() + catkin_add_gtest(test_${PROJECT_NAME}_filter src/test_filter.cpp) target_link_libraries(test_${PROJECT_NAME}_filter ${catkin_LIBRARIES}) @@ -30,3 +35,8 @@ add_rostest_gtest(test_trajectory_tracker_with_odom test/trajectory_tracker_with src/test_trajectory_tracker_with_odom.cpp) target_link_libraries(test_trajectory_tracker_with_odom ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_dependencies(test_trajectory_tracker_with_odom trajectory_tracker) + +add_rostest_gtest(test_trajectory_tracker_overshoot test/trajectory_tracker_overshoot_rostest.test + src/test_trajectory_tracker_overshoot.cpp) +target_link_libraries(test_trajectory_tracker_overshoot ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +add_dependencies(test_trajectory_tracker_overshoot trajectory_tracker) diff --git a/trajectory_tracker/test/include/trajectory_tracker_test.h b/trajectory_tracker/test/include/trajectory_tracker_test.h index d993410ae..bfee60c23 100644 --- a/trajectory_tracker/test/include/trajectory_tracker_test.h +++ b/trajectory_tracker/test/include/trajectory_tracker_test.h @@ -31,6 +31,7 @@ #define TRAJECTORY_TRACKER_TEST_H #include +#include #include #include #include @@ -42,6 +43,7 @@ #include +#include #include #include #include @@ -49,6 +51,7 @@ #include #include +#include #include #include @@ -78,6 +81,8 @@ class TrajectoryTrackerTest : public ::testing::Test double error_lin_; double error_large_lin_; double error_ang_; + using ParamType = trajectory_tracker::TrajectoryTrackerConfig; + std::unique_ptr> dynamic_reconfigure_client_; private: void cbStatus(const trajectory_tracker_msgs::TrajectoryTrackerStatus::ConstPtr& msg) @@ -124,6 +129,8 @@ class TrajectoryTrackerTest : public ::testing::Test pnh_.param("error_large_lin", error_large_lin_, 0.1); pnh_.param("error_ang", error_ang_, 0.01); + dynamic_reconfigure_client_.reset(new dynamic_reconfigure::Client("/trajectory_tracker")); + ros::Rate wait(10); for (size_t i = 0; i < 100; ++i) { @@ -235,7 +242,6 @@ class TrajectoryTrackerTest : public ::testing::Test void publishTransform() { const ros::Time now = ros::Time::now(); - const Eigen::Quaterniond q(Eigen::AngleAxisd(yaw_, Eigen::Vector3d(0, 0, 1))); nav_msgs::Odometry odom; @@ -254,10 +260,14 @@ class TrajectoryTrackerTest : public ::testing::Test odom.twist.twist.linear = cmd_vel_->linear; odom.twist.twist.angular = cmd_vel_->angular; } + publishTransform(odom); + } + void publishTransform(const nav_msgs::Odometry& odom) + { odom_buffer_.push_back(odom); - const ros::Time pub_time = now - delay_; + const ros::Time pub_time = odom.header.stamp - delay_; while (odom_buffer_.size() > 0) { @@ -286,10 +296,36 @@ class TrajectoryTrackerTest : public ::testing::Test trans_stamp_last_ = odom.header.stamp; } } + double getCmdVelFrameRate() const { return cmd_vel_count_ / (cmd_vel_time_ - initial_cmd_vel_time_).toSec(); } + + bool getConfig(ParamType& config) const + { + const ros::WallTime time_limit = ros::WallTime::now() + ros::WallDuration(10.0); + while (time_limit > ros::WallTime::now()) + { + if (dynamic_reconfigure_client_->getCurrentConfiguration(config, ros::Duration(0.1))) + { + return true; + } + ros::spinOnce(); + } + return false; + } + + bool setConfig(const ParamType& config) + { + // Wait until parameter server becomes ready + ParamType dummy; + if (!getConfig(dummy)) + { + return false; + } + return dynamic_reconfigure_client_->setConfiguration(config); + } }; #endif // TRAJECTORY_TRACKER_TEST_H diff --git a/trajectory_tracker/test/src/test_trajectory_tracker_overshoot.cpp b/trajectory_tracker/test/src/test_trajectory_tracker_overshoot.cpp new file mode 100644 index 000000000..89ffb135a --- /dev/null +++ b/trajectory_tracker/test/src/test_trajectory_tracker_overshoot.cpp @@ -0,0 +1,134 @@ +/* + * Copyright (c) 2021, the neonavigation authors + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +#include +#include + +#include + +class TrajectoryTrackerOvershootTest : public TrajectoryTrackerTest +{ +protected: + void runTest(const double goal_tolerance_lin_vel, const double goal_tolerance_ang_vel, + const double linear_vel, const double rotation_vel, const int32_t expected_status) + { + initState(Eigen::Vector2d(0, 0), 0); + + std::vector poses; + for (double x = 0.0; x < 0.5; x += 0.01) + poses.push_back(Eigen::Vector3d(x, 0.0, 0.0)); + poses.push_back(Eigen::Vector3d(0.5, 0.0, 0.0)); + waitUntilStart(std::bind(&TrajectoryTrackerTest::publishPath, this, poses)); + + ParamType param; + ASSERT_TRUE(getConfig(param)); + param.goal_tolerance_lin_vel = goal_tolerance_lin_vel; + param.goal_tolerance_ang_vel = goal_tolerance_ang_vel; + ASSERT_TRUE(setConfig(param)); + + nav_msgs::Odometry odom; + odom.header.frame_id = "odom"; + odom.child_frame_id = "base_link"; + odom.pose.pose.position.x = 0.5; + odom.pose.pose.position.y = 0.0; + odom.pose.pose.position.z = 0.0; + odom.pose.pose.orientation.x = 0.0; + odom.pose.pose.orientation.y = 0.0; + odom.pose.pose.orientation.z = 0.0; + odom.pose.pose.orientation.w = 1.0; + odom.twist.twist.linear.x = linear_vel; + odom.twist.twist.linear.y = 0.0; + odom.twist.twist.linear.z = 0.0; + odom.twist.twist.angular.x = 0.0; + odom.twist.twist.angular.y = 0.0; + odom.twist.twist.angular.z = rotation_vel; + + ros::Rate rate(50); + const ros::Time initial_time = ros::Time::now(); + const ros::Time time_limit = initial_time + ros::Duration(5.0); + while (ros::ok() && time_limit > ros::Time::now()) + { + odom.header.stamp = ros::Time::now(); + publishTransform(odom); + rate.sleep(); + ros::spinOnce(); + if ((status_->header.stamp > initial_time + ros::Duration(0.5)) && (status_->status == expected_status)) + { + return; + } + } + FAIL() << "Expected status: " << expected_status << " Actual status: " << status_->status; + } +}; + +TEST_F(TrajectoryTrackerOvershootTest, NoVelocityToleranceWithRemainingLinearVel) +{ + SCOPED_TRACE("NoVelocityToleranceWithRemainingLinearVel"); + runTest(0.0, 0.0, 0.1, 0.0, trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL); +} + +TEST_F(TrajectoryTrackerOvershootTest, NoVelocityToleranceWithRemainingAngularVel) +{ + SCOPED_TRACE("NoVelocityToleranceWithRemainingAngularVel"); + runTest(0.0, 0.0, 0.0, 0.1, trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL); +} + +TEST_F(TrajectoryTrackerOvershootTest, LinearVelocityToleranceWithRemainingLinearVel) +{ + SCOPED_TRACE("LinearVelocityToleranceWithRemainingLinearVel"); + runTest(0.05, 0.0, 0.1, 0.0, trajectory_tracker_msgs::TrajectoryTrackerStatus::FOLLOWING); +} + +TEST_F(TrajectoryTrackerOvershootTest, LinearVelocityToleranceWithRemainingAngularVel) +{ + SCOPED_TRACE("LinearVelocityToleranceWithRemainingAngularVel"); + runTest(0.05, 0.0, 0.0, 0.1, trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL); +} + +TEST_F(TrajectoryTrackerOvershootTest, AngularrVelocityToleranceWithRemainingLinearVel) +{ + SCOPED_TRACE("AngularrVelocityToleranceWithRemainingLinearVel"); + runTest(0.0, 0.05, 0.1, 0.0, trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL); +} + +TEST_F(TrajectoryTrackerOvershootTest, AngularrVelocityToleranceWithRemainingAngularVel) +{ + SCOPED_TRACE("AngularrVelocityToleranceWithRemainingAngularVel"); + runTest(0.0, 0.05, 0.0, 0.1, trajectory_tracker_msgs::TrajectoryTrackerStatus::FOLLOWING); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "test_trajectory_tracker_overshoot"); + + return RUN_ALL_TESTS(); +} diff --git a/trajectory_tracker/test/test/trajectory_tracker_overshoot_rostest.test b/trajectory_tracker/test/test/trajectory_tracker_overshoot_rostest.test new file mode 100644 index 000000000..7192c0ea0 --- /dev/null +++ b/trajectory_tracker/test/test/trajectory_tracker_overshoot_rostest.test @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + +