From 02593742cf2320fd5fb8d1abc483bd734dd8dc75 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 6 Oct 2026 18:10:15 +0200 Subject: [PATCH] Fix fail with short paths MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../SerestController.cpp | 10 +- .../tests/CMakeLists.txt | 7 + .../serest_controller_short_path_tests.cpp | 149 ++++++++++++++++++ 3 files changed, 165 insertions(+), 1 deletion(-) create mode 100644 controllers/easynav_serest_controller/tests/serest_controller_short_path_tests.cpp diff --git a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp index 6432ee1c..c6fbec6c 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp @@ -226,7 +226,7 @@ SerestController::ref_heading_and_curvature( // Determine if we are near the beginning or end of a segment double s_i = pd.s_acc[i]; - double s_ip1 = pd.s_acc[i + 1]; + double s_ip1 = (i + 1 < pd.s_acc.size()) ? pd.s_acc[i + 1] : s_i; // single-point path double s = prj.s_star; // Default: constant segment heading, kappa = 0 @@ -650,6 +650,14 @@ SerestController::update_rt(NavState & nav_state) Vec2 robot_xy; double yaw = 0.0; robot_state_from_odom(odom, robot_xy, yaw); + // A single pose (goal in or next to the robot's cell): the segment goes from the robot to it. + if (path.poses.size() == 1) { + geometry_msgs::msg::PoseStamped from = path.poses.front(); + from.pose.position.x = robot_xy.x; + from.pose.position.y = robot_xy.y; + path.poses.insert(path.poses.begin(), from); + } + // 3) Path primitives, closest-point projection, and local reference kinematics PathData pd = build_path_data(path); Projection prj = project_on_path(pd, robot_xy); diff --git a/controllers/easynav_serest_controller/tests/CMakeLists.txt b/controllers/easynav_serest_controller/tests/CMakeLists.txt index f5bfb752..756af7d1 100644 --- a/controllers/easynav_serest_controller/tests/CMakeLists.txt +++ b/controllers/easynav_serest_controller/tests/CMakeLists.txt @@ -21,3 +21,10 @@ ament_add_gtest(serest_controller_legacy_params_file_tests serest_controller_leg target_link_libraries(serest_controller_legacy_params_file_tests easynav_controller::easynav_controller) target_compile_definitions(serest_controller_legacy_params_file_tests PRIVATE TEST_DATA_DIR="${CMAKE_CURRENT_SOURCE_DIR}/data") + +ament_add_gtest(serest_controller_short_path_tests serest_controller_short_path_tests.cpp) +target_link_libraries(serest_controller_short_path_tests + ${PROJECT_NAME} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle +) diff --git a/controllers/easynav_serest_controller/tests/serest_controller_short_path_tests.cpp b/controllers/easynav_serest_controller/tests/serest_controller_short_path_tests.cpp new file mode 100644 index 00000000..bc11098f --- /dev/null +++ b/controllers/easynav_serest_controller/tests/serest_controller_short_path_tests.cpp @@ -0,0 +1,149 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \file +/// \brief Serest with very short paths: a planner returns a single pose when the goal is in (or +/// next to) the robot's cell, which the controller indexed past its end. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "nav_msgs/msg/path.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "tf2/LinearMath/Quaternion.hpp" +#include "easynav_common/types/NavState.hpp" +#include "easynav_serest_controller/SerestController.hpp" + +class SerestShortPathTest : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + static int count = 0; + node_ = std::make_shared( + "serest_short_path_" + std::to_string(count++)); + serest_ = std::make_shared(); + serest_->initialize(node_, "serest"); + nav_state_.set("map", 0); // only its presence is checked + } + + void set_robot(double x, double y, double yaw) + { + nav_msgs::msg::Odometry odom; + odom.header.frame_id = "map"; + odom.header.stamp = node_->now(); + odom.pose.pose.position.x = x; + odom.pose.pose.position.y = y; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, yaw); + odom.pose.pose.orientation.z = q.z(); + odom.pose.pose.orientation.w = q.w(); + nav_state_.set("robot_pose", odom); + } + + void set_path(const std::vector> & xy_yaw) + { + nav_msgs::msg::Path path; + path.header.frame_id = "map"; + path.header.stamp = node_->now(); + for (const auto & p : xy_yaw) { + geometry_msgs::msg::PoseStamped ps; + ps.header = path.header; + ps.pose.position.x = p[0]; + ps.pose.position.y = p[1]; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, p[2]); + ps.pose.orientation.z = q.z(); + ps.pose.orientation.w = q.w(); + path.poses.push_back(ps); + } + nav_state_.set("path", path); + } + + geometry_msgs::msg::Twist run(int cycles = 5) + { + for (int i = 0; i < cycles; ++i) { + rclcpp::sleep_for(std::chrono::milliseconds(20)); + serest_->update_rt(nav_state_); + } + return nav_state_.get("cmd_vel").twist; + } + + static bool finite(const geometry_msgs::msg::Twist & t) + { + return std::isfinite(t.linear.x) && std::isfinite(t.angular.z); + } + + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + std::shared_ptr serest_; + easynav::NavState nav_state_; +}; + +TEST_F(SerestShortPathTest, ASinglePoseAheadDrivesTowardsIt) +{ + set_robot(0.0, 0.0, 0.0); + set_path({{0.6, 0.0, 0.0}}); + const auto cmd = run(); + ASSERT_TRUE(finite(cmd)); + EXPECT_GE(cmd.linear.x, 0.0); + EXPECT_NEAR(cmd.angular.z, 0.0, 0.3); +} + +TEST_F(SerestShortPathTest, ASinglePoseToTheLeftTurnsLeft) +{ + set_robot(0.0, 0.0, 0.0); + set_path({{0.0, 0.6, M_PI / 2.0}}); + const auto cmd = run(); + ASSERT_TRUE(finite(cmd)); + EXPECT_GT(cmd.angular.z, 0.0); +} + +TEST_F(SerestShortPathTest, ASinglePoseOnTheRobotStaysFinite) +{ + // The user's case: the goal (0.9, 4.9, yaw 1.49) in the robot's cell. + set_robot(0.9, 4.9, 0.0); + set_path({{0.9, 4.9, 1.49}}); + const auto cmd = run(); + ASSERT_TRUE(finite(cmd)); + EXPECT_NEAR(cmd.linear.x, 0.0, 0.05); +} + +TEST_F(SerestShortPathTest, TwoIdenticalPosesStayFinite) +{ + set_robot(0.0, 0.0, 0.0); + set_path({{0.5, 0.5, 0.0}, {0.5, 0.5, 0.0}}); + EXPECT_TRUE(finite(run())); +} + +TEST_F(SerestShortPathTest, APathShrinkingToOnePoseAsTheRobotArrives) +{ + set_robot(0.0, 0.0, 0.0); + set_path({{0.2, 0.0, 0.0}, {0.4, 0.0, 0.0}, {0.6, 0.0, 0.0}}); + EXPECT_TRUE(finite(run(3))); + set_robot(0.3, 0.0, 0.0); + set_path({{0.4, 0.0, 0.0}, {0.6, 0.0, 0.0}}); + EXPECT_TRUE(finite(run(3))); + set_robot(0.5, 0.0, 0.0); + set_path({{0.6, 0.0, 0.0}}); + EXPECT_TRUE(finite(run(3))); +}