diff --git a/costmap_cspace/COLCON_IGNORE b/costmap_cspace/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/joystick_interrupt/COLCON_IGNORE b/joystick_interrupt/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/map_organizer/COLCON_IGNORE b/map_organizer/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/neonavigation/COLCON_IGNORE b/neonavigation/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/neonavigation_common/CMakeLists.txt b/neonavigation_common/CMakeLists.txt index edeece194..9c4b74176 100644 --- a/neonavigation_common/CMakeLists.txt +++ b/neonavigation_common/CMakeLists.txt @@ -1,35 +1,21 @@ -cmake_minimum_required(VERSION 3.1.3) +cmake_minimum_required(VERSION 3.5) project(neonavigation_common) +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() -set(CATKIN_DEPENDS - roscpp -) - -find_package(catkin REQUIRED COMPONENTS ${CATKIN_DEPENDS}) -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS ${CATKIN_DEPENDS} -) - - -set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) -include_directories(include ${catkin_INCLUDE_DIRS}) +include_directories(include) +ament_auto_add_library(neonavigation_common SHARED + src/neonavigation_node.cpp) - -if(CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - find_package(std_msgs REQUIRED) - find_package(std_srvs REQUIRED) - add_subdirectory(test) - - find_package(roslint REQUIRED) - roslint_cpp() - roslint_add_test() +if(BUILD_TESTING) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_uncrustify_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_auto_find_test_dependencies() + ament_lint_auto_find_test_dependencies() endif() - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) +ament_auto_package() diff --git a/neonavigation_common/include/neonavigation_common/compatibility.h b/neonavigation_common/include/neonavigation_common/compatibility.h index a77f1b159..d53a51c42 100644 --- a/neonavigation_common/include/neonavigation_common/compatibility.h +++ b/neonavigation_common/include/neonavigation_common/compatibility.h @@ -30,7 +30,7 @@ #ifndef NEONAVIGATION_COMMON_COMPATIBILITY_H #define NEONAVIGATION_COMMON_COMPATIBILITY_H -#include +#include "rclcpp/rclcpp.hpp" #include @@ -57,7 +57,7 @@ STATIC_ASSERT(supported_level <= default_level && default_level <= current_level int getCompat() { int compat(default_level); - ros::NodeHandle("/").param("neonavigation_compatible", compat, compat); + rclcpp::Node("/").param("neonavigation_compatible", compat, compat); return compat; } @@ -90,7 +90,7 @@ void checkCompatMode() ros::this_node::getName().c_str(), current_level); } } -std::string getSimplifiedNamespace(ros::NodeHandle& nh) +std::string getSimplifiedNamespace(rclcpp::Node& nh) { if (nh.getUnresolvedNamespace() == ros::this_node::getName()) return std::string("~/"); @@ -100,9 +100,9 @@ std::string getSimplifiedNamespace(ros::NodeHandle& nh) } template ros::Subscriber subscribe( - ros::NodeHandle& nh_new, + rclcpp::Node& nh_new, const std::string& topic_new, - ros::NodeHandle& nh_old, + rclcpp::Node& nh_old, const std::string& topic_old, uint32_t queue_size, void (*fp)(M), @@ -110,7 +110,7 @@ ros::Subscriber subscribe( { if (getCompat() != current_level) { - ROS_ERROR( + RCLCPP_ERROR(rclcpp::get_logger("NeonavigationCommon"), "Use %s (%s%s) topic instead of %s (%s%s)", nh_new.resolveName(topic_new, false).c_str(), getSimplifiedNamespace(nh_new).c_str(), topic_new.c_str(), @@ -125,9 +125,9 @@ ros::Subscriber subscribe( } template ros::Subscriber subscribe( - ros::NodeHandle& nh_new, + rclcpp::Node& nh_new, const std::string& topic_new, - ros::NodeHandle& nh_old, + rclcpp::Node& nh_old, const std::string& topic_old, uint32_t queue_size, void (T::*fp)(M) const, @@ -136,7 +136,7 @@ ros::Subscriber subscribe( { if (getCompat() != current_level) { - ROS_ERROR( + RCLCPP_ERROR(rclcpp::get_logger("NeonavigationCommon"), "Use %s (%s%s) topic instead of %s (%s%s)", nh_new.resolveName(topic_new, false).c_str(), getSimplifiedNamespace(nh_new).c_str(), topic_new.c_str(), @@ -151,9 +151,9 @@ ros::Subscriber subscribe( } template ros::Subscriber subscribe( - ros::NodeHandle& nh_new, + rclcpp::Node& nh_new, const std::string& topic_new, - ros::NodeHandle& nh_old, + rclcpp::Node& nh_old, const std::string& topic_old, uint32_t queue_size, void (T::*fp)(M), @@ -162,7 +162,7 @@ ros::Subscriber subscribe( { if (getCompat() != current_level) { - ROS_ERROR( + RCLCPP_ERROR(rclcpp::get_logger("NeonavigationCommon"), "Use %s (%s%s) topic instead of %s (%s%s)", nh_new.resolveName(topic_new, false).c_str(), getSimplifiedNamespace(nh_new).c_str(), topic_new.c_str(), @@ -177,18 +177,18 @@ ros::Subscriber subscribe( } template ros::Subscriber subscribe( - ros::NodeHandle& nh_new, + rclcpp::Node& nh_new, const std::string& topic_new, - ros::NodeHandle& nh_old, + rclcpp::Node& nh_old, const std::string& topic_old, uint32_t queue_size, - const boost::function&)>& callback, + const std::function&)>& callback, const ros::VoidConstPtr& tracked_object = ros::VoidConstPtr(), const ros::TransportHints& transport_hints = ros::TransportHints()) { if (getCompat() != current_level) { - ROS_ERROR( + RCLCPP_ERROR(rclcpp::get_logger("NeonavigationCommon"), "Use %s (%s%s) topic instead of %s (%s%s)", nh_new.resolveName(topic_new, false).c_str(), getSimplifiedNamespace(nh_new).c_str(), topic_new.c_str(), @@ -203,16 +203,16 @@ ros::Subscriber subscribe( } template ros::Publisher advertise( - ros::NodeHandle& nh_new, + rclcpp::Node& nh_new, const std::string& topic_new, - ros::NodeHandle& nh_old, + rclcpp::Node& nh_old, const std::string& topic_old, uint32_t queue_size, bool latch = false) { if (getCompat() != current_level) { - ROS_ERROR( + RCLCPP_ERROR(rclcpp::get_logger("NeonavigationCommon"), "Use %s (%s%s) topic instead of %s (%s%s)", nh_new.resolveName(topic_new, false).c_str(), getSimplifiedNamespace(nh_new).c_str(), topic_new.c_str(), @@ -227,16 +227,16 @@ ros::Publisher advertise( } template ros::ServiceServer advertiseService( - ros::NodeHandle& nh_new, + rclcpp::Node& nh_new, const std::string& service_new, - ros::NodeHandle& nh_old, + rclcpp::Node& nh_old, const std::string& service_old, bool (T::*srv_func)(MReq&, MRes&), T* obj) { if (getCompat() != current_level) { - ROS_ERROR( + RCLCPP_ERROR(rclcpp::get_logger("NeonavigationCommon"), "Use %s (%s%s) service instead of %s (%s%s)", nh_new.resolveName(service_new, false).c_str(), getSimplifiedNamespace(nh_new).c_str(), service_new.c_str(), @@ -252,14 +252,14 @@ ros::ServiceServer advertiseService( template void deprecatedParam( - const ros::NodeHandle& nh, + const rclcpp::Node& nh, const std::string& key, T& param, const T& default_value) { if (nh.hasParam(key)) { - ROS_ERROR( + RCLCPP_ERROR(rclcpp::get_logger("NeonavigationCommon"), "Use of the parameter %s is deprecated. Don't use this.", nh.resolveName(key, false).c_str()); } diff --git a/neonavigation_common/include/neonavigation_common/neonavigation_node.hpp b/neonavigation_common/include/neonavigation_common/neonavigation_node.hpp new file mode 100644 index 000000000..0d5a080c2 --- /dev/null +++ b/neonavigation_common/include/neonavigation_common/neonavigation_node.hpp @@ -0,0 +1,84 @@ +/* + * Copyright (c) 2024, 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. + */ + +#ifndef NEONAVIGATION_COMMON__NEONAVIGATION_NODE_HPP_ +#define NEONAVIGATION_COMMON__NEONAVIGATION_NODE_HPP_ + +#include +#include +#include +#include + +#include +#include + +#include +#include + +namespace neonavigation_common +{ +class NeonavigationNode : public rclcpp::Node +{ +public: + explicit NeonavigationNode(const std::string& node_name, const rclcpp::NodeOptions& options); + + // Disable copy and move. declare_dynamic_parameter() will not work correctly if copied. + NeonavigationNode& operator=(const NeonavigationNode&) = delete; + NeonavigationNode(const NeonavigationNode&) = delete; + +protected: + OnSetParametersCallbackHandle::SharedPtr parameter_handler_; + bool parameter_handler_disabled_; + using ParamType = std::variant; + std::unordered_map params_; + rclcpp::Service::SharedPtr get_loggers_service_; + rclcpp::Service::SharedPtr set_loggers_service_; + + template + void declare_dynamic_parameter(const std::string& name, T* const param, const T& default_value) + { + // Disable cbParameter while declaring parameters + parameter_handler_disabled_ = true; + *param = declare_parameter(name, default_value); + params_[name] = param; + parameter_handler_disabled_ = false; + } + + void create_logger_services(); + rcl_interfaces::msg::SetParametersResult cbParameter(const std::vector& parameters); + + // This function is called after a parameter is updated. + virtual void onDynamicParameterUpdated(const std::vector&) + { + } +}; + +} // namespace neonavigation_common + +#endif // NEONAVIGATION_COMMON__NEONAVIGATION_NODE_HPP_ diff --git a/neonavigation_common/package.xml b/neonavigation_common/package.xml index d0c20258e..845e35740 100644 --- a/neonavigation_common/package.xml +++ b/neonavigation_common/package.xml @@ -1,5 +1,8 @@ - + + neonavigation_common 0.17.1 Common headers for neonavigation meta-package @@ -8,11 +11,15 @@ BSD Atsushi Watanabe - catkin + ament_cmake + ament_cmake_auto + rclcpp + neonavigation_common_msgs - roscpp - roslint - rostest - std_srvs - std_msgs + ament_lint_auto + ament_lint_common + + + ament_cmake + diff --git a/neonavigation_common/src/neonavigation_node.cpp b/neonavigation_common/src/neonavigation_node.cpp new file mode 100644 index 000000000..33a6b89a4 --- /dev/null +++ b/neonavigation_common/src/neonavigation_node.cpp @@ -0,0 +1,139 @@ +/* + * Copyright (c) 2024, 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 +#include +#include + +#include + +namespace neonavigation_common +{ +NeonavigationNode::NeonavigationNode(const std::string& node_name, const rclcpp::NodeOptions& options) + : Node(node_name, options) + , parameter_handler_disabled_(false) +{ + create_logger_services(); + parameter_handler_ = + add_on_set_parameters_callback(std::bind(&NeonavigationNode::cbParameter, this, std::placeholders::_1)); +} + +rcl_interfaces::msg::SetParametersResult +NeonavigationNode::cbParameter(const std::vector& parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + if (parameter_handler_disabled_) + { + return result; + } + bool parameter_updated = false; + for (const auto& param : parameters) + { + if (params_.find(param.get_name()) != params_.end()) + { + std::visit( + [&](auto&& arg) + { + using T = std::decay_t; + if constexpr (std::is_same_v) + *std::get(params_[param.get_name()]) = param.as_int(); + else if constexpr (std::is_same_v) + *std::get(params_[param.get_name()]) = param.as_bool(); + else if constexpr (std::is_same_v) + *std::get(params_[param.get_name()]) = param.as_double(); + else if constexpr (std::is_same_v) + *std::get(params_[param.get_name()]) = param.as_string(); + }, + params_[param.get_name()]); + parameter_updated = true; + } + } + if (parameter_updated) + { + onDynamicParameterUpdated(parameters); + } + return result; +} + +// This function is copied from the "jazzy" branch of rclcpp/src/rclcpp/node_interfaces/node_logging.cpp +void NeonavigationNode::create_logger_services() +{ + const std::string node_name = get_name(); + get_loggers_service_ = create_service( + node_name + "/get_logger_levels", + [](const std::shared_ptr, + const std::shared_ptr request, + std::shared_ptr response) + { + for (auto& name : request->names) + { + neonavigation_common_msgs::msg::LoggerLevel logger_level; + logger_level.name = name; + auto ret = rcutils_logging_get_logger_level(name.c_str()); + if (ret < 0) + { + logger_level.level = 0; + } + else + { + logger_level.level = static_cast(ret); + } + response->levels.push_back(std::move(logger_level)); + } + }); + + set_loggers_service_ = create_service( + node_name + "/set_logger_levels", + [](const std::shared_ptr, + const std::shared_ptr request, + std::shared_ptr response) + { + neonavigation_common_msgs::msg::SetLoggerLevelsResult result; + for (auto& level : request->levels) + { + auto ret = rcutils_logging_set_logger_level(level.name.c_str(), level.level); + if (ret != RCUTILS_RET_OK) + { + result.successful = false; + result.reason = rcutils_get_error_string().str; + } + else + { + result.successful = true; + } + response->results.push_back(std::move(result)); + } + }); +} + +} // namespace neonavigation_common diff --git a/neonavigation_launch/COLCON_IGNORE b/neonavigation_launch/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/obj_to_pointcloud/COLCON_IGNORE b/obj_to_pointcloud/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/planner_cspace/COLCON_IGNORE b/planner_cspace/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/safety_limiter/CMakeLists.txt b/safety_limiter/CMakeLists.txt index 0f50936df..688e9b958 100644 --- a/safety_limiter/CMakeLists.txt +++ b/safety_limiter/CMakeLists.txt @@ -1,43 +1,19 @@ -cmake_minimum_required(VERSION 3.1.3) +cmake_minimum_required(VERSION 3.5) project(safety_limiter) -set(CATKIN_DEPENDS - roscpp - - diagnostic_updater - dynamic_reconfigure - geometry_msgs - pcl_conversions - pcl_ros - sensor_msgs - std_msgs - tf2_ros - tf2_sensor_msgs - safety_limiter_msgs - - neonavigation_common -) - -find_package(catkin REQUIRED COMPONENTS ${CATKIN_DEPENDS}) +find_package(ament_cmake_auto REQUIRED) find_package(PCL REQUIRED) find_package(Eigen3 REQUIRED) -find_package(xmlrpcpp REQUIRED) - -generate_dynamic_reconfigure_options( - cfg/SafetyLimiter.cfg -) +ament_auto_find_build_dependencies() -catkin_package( - CATKIN_DEPENDS ${CATKIN_DEPENDS} -) - - -set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) - -include_directories(${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ${Eigen_INCLUDE_DIRS}) add_definitions(${PCL_DEFINITIONS}) +# Binary installed pcl provided by Linux distro is built with -march=native +# which causes a lot of compatibility problems. +# Define PCL_NO_PRECOMPILE to disable using the binary version. +add_definitions(-DPCL_NO_PRECOMPILE) # Workaround for debian stretch build (https://bugs.debian.org/cgi-bin/bugreport.cgi?bug=894656) list(REMOVE_ITEM PCL_LIBRARIES @@ -52,31 +28,35 @@ list(REMOVE_ITEM PCL_LIBRARIES # Workaround for the bug in PCL(<1.8.1) https://github.com/PointCloudLibrary/pcl/issues/1406 remove_definitions(-DDISABLE_LIBUSB-1.0) -# Binary installed pcl provided by Linux distro is built with -march=native -# which causes a lot of compatibility problems. -# Define PCL_NO_PRECOMPILE to disable using the binary version. -add_definitions(-DPCL_NO_PRECOMPILE) - - -add_executable(safety_limiter src/safety_limiter.cpp) -target_link_libraries(safety_limiter ${catkin_LIBRARIES} ${PCL_LIBRARIES}) -add_dependencies(safety_limiter ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) - -if(CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - find_package(nav_msgs REQUIRED) - find_package(tf2_geometry_msgs REQUIRED) - add_subdirectory(test) - - find_package(roslint REQUIRED) - roslint_cpp() - roslint_add_test() +include_directories(${PCL_INCLUDE_DIRS} ${Eigen_INCLUDE_DIRS}) +ament_auto_add_executable(safety_limiter src/safety_limiter.cpp) + +if(BUILD_TESTING) + find_package(GTest REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_uncrustify_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_auto_find_test_dependencies() + ament_lint_auto_find_test_dependencies() + + include_directories(test/include) + ament_auto_add_executable(test_safety_limiter test/src/test_safety_limiter.cpp) + target_link_libraries(test_safety_limiter ${GTEST_LIBRARIES}) + ament_auto_add_executable(test_safety_limiter2 test/src/test_safety_limiter2.cpp) + target_link_libraries(test_safety_limiter2 ${GTEST_LIBRARIES}) + + install(DIRECTORY + test/configs + DESTINATION share/${PROJECT_NAME}/test/ + ) + add_launch_test(test/test/test_safety_limiter_launch.py + TARGET test_safety_limiter + TIMEOUT 120 + ) + add_launch_test(test/test/test_safety_limiter2_launch.py + TARGET test_safety_limiter2 + TIMEOUT 30 + ) endif() - -install(TARGETS - safety_limiter - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) +ament_auto_package() diff --git a/safety_limiter/package.xml b/safety_limiter/package.xml index 93d44b04e..478289622 100644 --- a/safety_limiter/package.xml +++ b/safety_limiter/package.xml @@ -8,16 +8,16 @@ BSD Atsushi Watanabe - catkin - - roscpp + ament_cmake + ament_cmake_auto nav_msgs tf2_geometry_msgs - roslint - rostest + ament_cmake_gtest + ament_lint_auto + ament_lint_common + launch_testing_ament_cmake diagnostic_updater - dynamic_reconfigure geometry_msgs pcl_conversions pcl_ros @@ -25,13 +25,16 @@ std_msgs tf2_ros tf2_sensor_msgs - neonavigation_common safety_limiter_msgs eigen - xmlrpcpp + nav2_costmap_2d + rclcpp libpcl-all-dev libpcl-all + + ament_cmake + diff --git a/safety_limiter/src/safety_limiter.cpp b/safety_limiter/src/safety_limiter.cpp index 8b74c58ac..55a50037f 100644 --- a/safety_limiter/src/safety_limiter.cpp +++ b/safety_limiter/src/safety_limiter.cpp @@ -37,22 +37,25 @@ #include #include #include +#include +#include #include #include -#include +#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include -#include +#include +#include +#include #include #include @@ -60,11 +63,7 @@ #include #include #include -#include - -#include - -#include +#include namespace safety_limiter { @@ -92,32 +91,126 @@ pcl::PointXYZ operator*(const pcl::PointXYZ& a, const float& b) c.z *= b; return c; } -bool XmlRpc_isNumber(XmlRpc::XmlRpcValue& value) +class SafetyLimiterNode : public neonavigation_common::NeonavigationNode { - return value.getType() == XmlRpc::XmlRpcValue::TypeInt || - value.getType() == XmlRpc::XmlRpcValue::TypeDouble; -} +public: + class vec + { + public: + float c[2]; + vec(const float x, const float y) + { + c[0] = x; + c[1] = y; + } + vec() + { + c[0] = c[1] = 0.0; + } + float& operator[](const int& i) + { + assert(i < 2); + return c[i]; + } + const float& operator[](const int& i) const + { + assert(i < 2); + return c[i]; + } + vec operator-(const vec& a) const + { + vec out = *this; + out[0] -= a[0]; + out[1] -= a[1]; + return out; + } + float cross(const vec& a) const + { + return (*this)[0] * a[1] - (*this)[1] * a[0]; + } + float dot(const vec& a) const + { + return (*this)[0] * a[0] + (*this)[1] * a[1]; + } + float dist(const vec& a) const + { + return std::hypot((*this)[0] - a[0], (*this)[1] - a[1]); + } + float dist_line(const vec& a, const vec& b) const + { + return (b - a).cross((*this) - a) / b.dist(a); + } + float dist_linestrip(const vec& a, const vec& b) const + { + if ((b - a).dot((*this) - a) <= 0) + return this->dist(a); + if ((a - b).dot((*this) - b) <= 0) + return this->dist(b); + return std::abs(this->dist_line(a, b)); + } + }; + class polygon + { + public: + std::vector v; + void move(const float& x, const float& y, const float& yaw) + { + const float cos_v = cosf(yaw); + const float sin_v = sinf(yaw); + for (auto& p : v) + { + const auto tmp = p; + p[0] = cos_v * tmp[0] - sin_v * tmp[1] + x; + p[1] = sin_v * tmp[0] + cos_v * tmp[1] + y; + } + } + bool inside(const vec& a) const + { + int cn = 0; + for (size_t i = 0; i < v.size() - 1; i++) + { + auto& v1 = v[i]; + auto& v2 = v[i + 1]; + if ((v1[1] <= a[1] && a[1] < v2[1]) || (v2[1] <= a[1] && a[1] < v1[1])) + { + float lx; + lx = v1[0] + (v2[0] - v1[0]) * (a[1] - v1[1]) / (v2[1] - v1[1]); + if (a[0] < lx) + cn++; + } + } + return ((cn & 1) == 1); + } + float dist(const vec& a) const + { + float dist = std::numeric_limits::max(); + for (size_t i = 0; i < v.size() - 1; i++) + { + auto& v1 = v[i]; + auto& v2 = v[i + 1]; + auto d = a.dist_linestrip(v1, v2); + if (d < dist) + dist = d; + } + return dist; + } + }; -class SafetyLimiterNode -{ protected: - ros::NodeHandle nh_; - ros::NodeHandle pnh_; - ros::Publisher pub_twist_; - ros::Publisher pub_cloud_; - ros::Publisher pub_status_; - ros::Subscriber sub_twist_; - std::vector sub_clouds_; - ros::Subscriber sub_disable_; - ros::Subscriber sub_watchdog_; - ros::Timer watchdog_timer_; + rclcpp::Publisher::SharedPtr pub_twist_; + rclcpp::Publisher::SharedPtr pub_cloud_; + rclcpp::Publisher::SharedPtr pub_status_; + rclcpp::SubscriptionBase::SharedPtr sub_twist_; + std::vector sub_clouds_; + rclcpp::SubscriptionBase::SharedPtr sub_disable_; + rclcpp::SubscriptionBase::SharedPtr sub_watchdog_; + rclcpp::TimerBase::SharedPtr watchdog_timer_; + rclcpp::TimerBase::SharedPtr predict_timer_; tf2_ros::Buffer tfbuf_; tf2_ros::TransformListener tfl_; - boost::recursive_mutex parameter_server_mutex_; - std::unique_ptr> parameter_server_; - geometry_msgs::Twist twist_; - ros::Time last_cloud_stamp_; + geometry_msgs::msg::Twist twist_; + rclcpp::Time last_cloud_stamp_; pcl::PointCloud::Ptr cloud_accum_; bool cloud_clear_; double hz_; @@ -139,108 +232,116 @@ class SafetyLimiterNode std::string fixed_frame_id_; std::string base_frame_id_; - ros::Time last_disable_cmd_; - ros::Duration hold_; - ros::Time hold_off_; - ros::Duration watchdog_interval_; + rclcpp::Time last_disable_cmd_; + double hold_d_; + rclcpp::Duration hold_; + rclcpp::Time hold_off_; + double watchdog_interval_d_; + std::string footprint_str_; bool allow_empty_cloud_; bool watchdog_stop_; bool has_cloud_; bool has_twist_; bool has_collision_at_now_; - ros::Time stuck_started_since_; + rclcpp::Time stuck_started_since_; constexpr static float EPSILON = 1e-6; - diagnostic_updater::Updater diag_updater_; + std::shared_ptr diag_updater_; + + polygon footprint_p; public: - SafetyLimiterNode() - : nh_() - , pnh_("~") + explicit SafetyLimiterNode(const std::string& node_name = "safety_limiter") + : neonavigation_common::NeonavigationNode(node_name, rclcpp::NodeOptions()) + , tfbuf_(get_clock()) , tfl_(tfbuf_) + , last_cloud_stamp_(0, 0, get_clock()->get_clock_type()) , cloud_accum_(new pcl::PointCloud) , cloud_clear_(false) - , last_disable_cmd_(0) + , last_disable_cmd_(0, 0, get_clock()->get_clock_type()) + , hold_(0, 0) + , hold_off_(0, 0, get_clock()->get_clock_type()) , watchdog_stop_(false) , has_cloud_(false) , has_twist_(true) , has_collision_at_now_(false) - , stuck_started_since_(ros::Time(0)) + , stuck_started_since_(0, 0, get_clock()->get_clock_type()) { - neonavigation_common::compat::checkCompatMode(); - pub_twist_ = neonavigation_common::compat::advertise( - nh_, "cmd_vel", - pnh_, "cmd_vel_out", 1, true); - pub_cloud_ = nh_.advertise("collision", 1, true); - pub_status_ = pnh_.advertise("status", 1, true); - sub_twist_ = neonavigation_common::compat::subscribe( - nh_, "cmd_vel_in", - pnh_, "cmd_vel_in", 1, &SafetyLimiterNode::cbTwist, this); - sub_disable_ = neonavigation_common::compat::subscribe( - nh_, "disable_safety", - pnh_, "disable", 1, &SafetyLimiterNode::cbDisable, this); - sub_watchdog_ = neonavigation_common::compat::subscribe( - nh_, "watchdog_reset", - pnh_, "watchdog_reset", 1, &SafetyLimiterNode::cbWatchdogReset, this); - - int num_input_clouds; - pnh_.param("num_input_clouds", num_input_clouds, 1); + declare_dynamic_parameter("freq", &hz_, 6.0); + declare_dynamic_parameter("cloud_timeout", &timeout_, 0.8); + declare_dynamic_parameter("disable_timeout", &disable_timeout_, 0.1); + declare_dynamic_parameter("lin_vel", &vel_[0], 0.5); + declare_dynamic_parameter("lin_acc", &acc_[0], 1.0); + declare_dynamic_parameter("ang_vel", &vel_[1], 0.8); + declare_dynamic_parameter("ang_acc", &acc_[1], 1.6); + declare_dynamic_parameter("max_linear_vel", &max_values_[0], std::numeric_limits::infinity()); + declare_dynamic_parameter("max_angular_vel", &max_values_[1], std::numeric_limits::infinity()); + declare_dynamic_parameter("z_range_min", &z_range_[0], 0.0); + declare_dynamic_parameter("z_range_max", &z_range_[1], 0.5); + declare_dynamic_parameter("dt", &dt_, 0.1); + declare_dynamic_parameter("d_margin", &d_margin_, 0.2); + declare_dynamic_parameter("d_escape", &d_escape_, 0.05); + declare_dynamic_parameter("yaw_margin", &yaw_margin_, 0.2); + declare_dynamic_parameter("yaw_escape", &yaw_escape_, 0.05); + declare_dynamic_parameter("downsample_grid", &downsample_grid_, 0.05); + declare_dynamic_parameter("hold", &hold_d_, 0.0); + declare_dynamic_parameter("allow_empty_cloud", &allow_empty_cloud_, false); + declare_dynamic_parameter("base_frame", &base_frame_id_, std::string("base_link")); + declare_dynamic_parameter("fixed_frame", &fixed_frame_id_, std::string("odom")); + declare_dynamic_parameter("watchdog_interval", &watchdog_interval_d_, 0.0); + declare_dynamic_parameter("footprint", &footprint_str_, std::string()); + if (footprint_str_.empty()) + { + RCLCPP_FATAL(get_logger(), "Footprint doesn't specified"); + throw std::runtime_error("Footprint doesn't specified"); + } + + pub_twist_ = create_publisher("cmd_vel", rclcpp::QoS(1).transient_local()); + pub_cloud_ = create_publisher("collision", rclcpp::QoS(1).transient_local()); + pub_status_ = + create_publisher("~/status", rclcpp::QoS(1).transient_local()); + sub_twist_ = create_subscription( + "cmd_vel_in", 1, std::bind(&SafetyLimiterNode::cbTwist, this, std::placeholders::_1)); + sub_disable_ = create_subscription( + "disable_safety", 1, std::bind(&SafetyLimiterNode::cbDisable, this, std::placeholders::_1)); + sub_watchdog_ = create_subscription( + "watchdog_reset", 1, std::bind(&SafetyLimiterNode::cbWatchdogReset, this, std::placeholders::_1)); + + const int num_input_clouds = declare_parameter("num_input_clouds", 1); if (num_input_clouds == 1) { - sub_clouds_.push_back(neonavigation_common::compat::subscribe( - nh_, "cloud", - pnh_, "cloud", 1, &SafetyLimiterNode::cbCloud, this)); + sub_clouds_.push_back(create_subscription( + "cloud", 1, std::bind(&SafetyLimiterNode::cbCloud, this, std::placeholders::_1))); } else { for (int i = 0; i < num_input_clouds; ++i) { - sub_clouds_.push_back(nh_.subscribe( - "cloud" + std::to_string(i), 1, &SafetyLimiterNode::cbCloud, this)); + sub_clouds_.push_back(create_subscription( + "cloud" + std::to_string(i), 1, std::bind(&SafetyLimiterNode::cbCloud, this, std::placeholders::_1))); } } + diag_updater_ = std::make_shared(this); + diag_updater_->setHardwareID("none"); + diag_updater_->add("Collision", this, &SafetyLimiterNode::diagnoseCollision); - if (pnh_.hasParam("t_margin")) - ROS_WARN("safety_limiter: t_margin parameter is obsolated. Use d_margin and yaw_margin instead."); - pnh_.param("base_frame", base_frame_id_, std::string("base_link")); - pnh_.param("fixed_frame", fixed_frame_id_, std::string("odom")); - double watchdog_interval_d; - pnh_.param("watchdog_interval", watchdog_interval_d, 0.0); - watchdog_interval_ = ros::Duration(watchdog_interval_d); - pnh_.param("max_linear_vel", max_values_[0], std::numeric_limits::infinity()); - pnh_.param("max_angular_vel", max_values_[1], std::numeric_limits::infinity()); - - parameter_server_.reset( - new dynamic_reconfigure::Server(parameter_server_mutex_, pnh_)); - parameter_server_->setCallback(boost::bind(&SafetyLimiterNode::cbParameter, this, _1, _2)); + onDynamicParameterUpdated({}); + } - XmlRpc::XmlRpcValue footprint_xml; - if (!pnh_.hasParam("footprint")) - { - ROS_FATAL("Footprint doesn't specified"); - throw std::runtime_error("Footprint doesn't specified"); - } - pnh_.getParam("footprint", footprint_xml); - if (footprint_xml.getType() != XmlRpc::XmlRpcValue::TypeArray || footprint_xml.size() < 3) - { - ROS_FATAL("Invalid footprint"); - throw std::runtime_error("Invalid footprint"); - } +protected: + void onDynamicParameterUpdated(const std::vector&) final + { + std::vector footprint; + nav2_costmap_2d::makeFootprintFromString(footprint_str_, footprint); + footprint_p.v.clear(); footprint_radius_ = 0; - for (int i = 0; i < footprint_xml.size(); i++) + for (const auto& p : footprint) { - if (!XmlRpc_isNumber(footprint_xml[i][0]) || - !XmlRpc_isNumber(footprint_xml[i][1])) - { - ROS_FATAL("Invalid footprint value"); - throw std::runtime_error("Invalid footprint value"); - } - vec v; - v[0] = static_cast(footprint_xml[i][0]); - v[1] = static_cast(footprint_xml[i][1]); + v[0] = p.x; + v[1] = p.y; footprint_p.v.push_back(v); const float dist = std::hypot(v[0], v[1]); @@ -248,110 +349,74 @@ class SafetyLimiterNode footprint_radius_ = dist; } footprint_p.v.push_back(footprint_p.v.front()); - ROS_INFO("footprint radius: %0.3f", footprint_radius_); - - diag_updater_.setHardwareID("none"); - diag_updater_.add("Collision", this, &SafetyLimiterNode::diagnoseCollision); - } - void spin() - { - ros::Timer predict_timer = - nh_.createTimer(ros::Duration(1.0 / hz_), &SafetyLimiterNode::cbPredictTimer, this); - - if (watchdog_interval_ != ros::Duration(0.0)) + hold_ = rclcpp::Duration::from_seconds(std::max(hold_d_, 1.0 / hz_)); + for (int i = 0; i < 2; i++) { - watchdog_timer_ = - nh_.createTimer(watchdog_interval_, &SafetyLimiterNode::cbWatchdogTimer, this); + auto t = vel_[i] / acc_[i]; + if (tmax_ < t) + tmax_ = t; } + tmax_ *= 1.5; + tmax_ += std::max(d_margin_ / vel_[0], yaw_margin_ / vel_[1]); + r_lim_ = 1.0; - ros::spin(); + watchdog_timer_ = rclcpp::create_timer(this, get_clock(), std::chrono::duration(watchdog_interval_d_), + std::bind(&SafetyLimiterNode::cbWatchdogTimer, this)); + predict_timer_ = rclcpp::create_timer(this, get_clock(), std::chrono::duration(1.0 / hz_), + std::bind(&SafetyLimiterNode::cbPredictTimer, this)); } -protected: - void cbWatchdogReset(const std_msgs::Empty::ConstPtr& msg) + void cbWatchdogReset(const std_msgs::msg::Empty& msg) { - watchdog_timer_.setPeriod(watchdog_interval_, true); + watchdog_timer_->reset(); watchdog_stop_ = false; } - void cbWatchdogTimer(const ros::TimerEvent& event) + void cbWatchdogTimer() { - ROS_WARN_THROTTLE(1.0, "safety_limiter: Watchdog timed-out"); + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "safety_limiter: Watchdog timed-out"); watchdog_stop_ = true; r_lim_ = 0; - geometry_msgs::Twist cmd_vel; - pub_twist_.publish(cmd_vel); + geometry_msgs::msg::Twist cmd_vel; + pub_twist_->publish(cmd_vel); - diag_updater_.force_update(); + diag_updater_->force_update(); } - void cbPredictTimer(const ros::TimerEvent& event) + void cbPredictTimer() { if (!has_twist_) return; if (!has_cloud_) return; - if (ros::Time::now() - last_cloud_stamp_ > ros::Duration(timeout_)) + if (now() - last_cloud_stamp_ > rclcpp::Duration::from_seconds(timeout_)) { - ROS_WARN_THROTTLE(1.0, "safety_limiter: PointCloud timed-out"); - geometry_msgs::Twist cmd_vel; - pub_twist_.publish(cmd_vel); + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "safety_limiter: PointCloud timed-out"); + geometry_msgs::msg::Twist cmd_vel; + pub_twist_->publish(cmd_vel); cloud_accum_.reset(new pcl::PointCloud); has_cloud_ = false; r_lim_ = 0; - diag_updater_.force_update(); + diag_updater_->force_update(); return; } - ros::Time now = ros::Time::now(); + auto now = this->now(); const double r_lim_current = predict(twist_); if (r_lim_current < r_lim_) r_lim_ = r_lim_current; if (r_lim_current < 1.0) - hold_off_ = now + hold_; - - cloud_clear_ = true; - - diag_updater_.force_update(); - } - void cbParameter(const SafetyLimiterConfig& config, const uint32_t /* level */) - { - boost::recursive_mutex::scoped_lock lock(parameter_server_mutex_); - hz_ = config.freq; - timeout_ = config.cloud_timeout; - disable_timeout_ = config.disable_timeout; - vel_[0] = config.lin_vel; - acc_[0] = config.lin_acc; - vel_[1] = config.ang_vel; - acc_[1] = config.ang_acc; - max_values_[0] = config.max_linear_vel; - max_values_[1] = config.max_angular_vel; - z_range_[0] = config.z_range_min; - z_range_[1] = config.z_range_max; - dt_ = config.dt; - d_margin_ = config.d_margin; - d_escape_ = config.d_escape; - yaw_margin_ = config.yaw_margin; - yaw_escape_ = config.yaw_escape; - downsample_grid_ = config.downsample_grid; - hold_ = ros::Duration(std::max(config.hold, 1.0 / hz_)); - allow_empty_cloud_ = config.allow_empty_cloud; - - tmax_ = 0.0; - for (int i = 0; i < 2; i++) { - auto t = vel_[i] / acc_[i]; - if (tmax_ < t) - tmax_ = t; + hold_off_ = now + hold_; } - tmax_ *= 1.5; - tmax_ += std::max(d_margin_ / vel_[0], yaw_margin_ / vel_[1]); - r_lim_ = 1.0; + RCLCPP_DEBUG(get_logger(), "safety_limiter: r_lim = %0.3f, hold_off = %0.3f", r_lim_, hold_off_.seconds()); + cloud_clear_ = true; + diag_updater_->force_update(); } - double predict(const geometry_msgs::Twist& in) + double predict(const geometry_msgs::msg::Twist& in) { if (cloud_accum_->size() == 0) { @@ -359,38 +424,29 @@ class SafetyLimiterNode { return 1.0; } - ROS_WARN_THROTTLE(1.0, "safety_limiter: Empty pointcloud passed."); + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "safety_limiter: Empty pointcloud passed."); return 0.0; } - const bool can_transform = tfbuf_.canTransform( - base_frame_id_, cloud_accum_->header.frame_id, - pcl_conversions::fromPCL(cloud_accum_->header.stamp)); - const ros::Time stamp = - can_transform ? pcl_conversions::fromPCL(cloud_accum_->header.stamp) : ros::Time(0); - - geometry_msgs::TransformStamped fixed_to_base; + const rclcpp::Time pcl_stamp = pcl_conversions::fromPCL(cloud_accum_->header.stamp); + const bool can_transform = tfbuf_.canTransform(base_frame_id_, cloud_accum_->header.frame_id, pcl_stamp); + const rclcpp::Time stamp = can_transform ? pcl_stamp : rclcpp::Time(0); + geometry_msgs::msg::TransformStamped fixed_to_base; try { - fixed_to_base = tfbuf_.lookupTransform( - base_frame_id_, cloud_accum_->header.frame_id, stamp); + fixed_to_base = tfbuf_.lookupTransform(base_frame_id_, cloud_accum_->header.frame_id, stamp); } catch (tf2::TransformException& e) { - ROS_WARN_THROTTLE(1.0, "safety_limiter: Transform failed: %s", e.what()); + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "safety_limiter: Transform failed: %s", e.what()); return 0.0; } const Eigen::Affine3f fixed_to_base_eigen = - Eigen::Translation3f( - fixed_to_base.transform.translation.x, - fixed_to_base.transform.translation.y, - fixed_to_base.transform.translation.z) * - Eigen::Quaternionf( - fixed_to_base.transform.rotation.w, - fixed_to_base.transform.rotation.x, - fixed_to_base.transform.rotation.y, - fixed_to_base.transform.rotation.z); + Eigen::Translation3f(fixed_to_base.transform.translation.x, fixed_to_base.transform.translation.y, + fixed_to_base.transform.translation.z) * + Eigen::Quaternionf(fixed_to_base.transform.rotation.w, fixed_to_base.transform.rotation.x, + fixed_to_base.transform.rotation.y, fixed_to_base.transform.rotation.z); pcl::transformPointCloud(*cloud_accum_, *cloud_accum_, fixed_to_base_eigen); pcl::PointCloud::Ptr pc(new pcl::PointCloud); @@ -406,8 +462,7 @@ class SafetyLimiterNode p.z = 0.0; return false; }; - pc->erase(std::remove_if(pc->points.begin(), pc->points.end(), filter_z), - pc->points.end()); + pc->erase(std::remove_if(pc->points.begin(), pc->points.end(), filter_z), pc->points.end()); if (pc->size() == 0) { @@ -415,26 +470,24 @@ class SafetyLimiterNode { return 1.0; } - ROS_WARN_THROTTLE(1.0, "safety_limiter: Empty pointcloud passed."); + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "safety_limiter: Empty pointcloud passed."); return 0.0; } - pcl::KdTreeFLANN kdtree; kdtree.setInputCloud(pc); Eigen::Affine3f move; Eigen::Affine3f move_inv; - Eigen::Affine3f motion = - Eigen::AngleAxisf(-twist_.angular.z * dt_, Eigen::Vector3f::UnitZ()) * - Eigen::Translation3f(Eigen::Vector3f(-twist_.linear.x * dt_, -twist_.linear.y * dt_, 0.0)); + Eigen::Affine3f motion = Eigen::AngleAxisf(-twist_.angular.z * dt_, Eigen::Vector3f::UnitZ()) * + Eigen::Translation3f(Eigen::Vector3f(-twist_.linear.x * dt_, -twist_.linear.y * dt_, 0.0)); Eigen::Affine3f motion_inv = Eigen::Translation3f(Eigen::Vector3f(twist_.linear.x * dt_, twist_.linear.y * dt_, 0.0)) * Eigen::AngleAxisf(twist_.angular.z * dt_, Eigen::Vector3f::UnitZ()); move.setIdentity(); move_inv.setIdentity(); - sensor_msgs::PointCloud col_points; + sensor_msgs::msg::PointCloud col_points; col_points.header.frame_id = base_frame_id_; - col_points.header.stamp = ros::Time::now(); + col_points.header.stamp = now(); float d_col = 0; float yaw_col = 0; @@ -472,7 +525,7 @@ class SafetyLimiterNode vec v(point.x, point.y); if (footprint_p.inside(v)) { - geometry_msgs::Point32 pos; + geometry_msgs::msg::Point32 pos; pos.x = p.x; pos.y = p.y; pos.z = p.z; @@ -506,17 +559,17 @@ class SafetyLimiterNode } } } - pub_cloud_.publish(col_points); + pub_cloud_->publish(col_points); if (has_collision_at_now_) { - if (stuck_started_since_ == ros::Time(0)) - stuck_started_since_ = ros::Time::now(); + if (stuck_started_since_.nanoseconds() == 0L) + stuck_started_since_ = now(); } else { - if (stuck_started_since_ != ros::Time(0)) - stuck_started_since_ = ros::Time(0); + if (stuck_started_since_.nanoseconds() != 0L) + stuck_started_since_ = rclcpp::Time(0, 0, get_clock()->get_clock_type()); } if (!has_collision) @@ -527,25 +580,18 @@ class SafetyLimiterNode // d_compensated = d + acc * delay^2 - sqrt((acc * delay^2)^2 + 2 * d * acc * delay^2) const float delay = 1.0 * (1.0 / hz_) + dt_; - const float acc_dtsq[2] = - { - static_cast(acc_[0] * std::pow(delay, 2)), - static_cast(acc_[1] * std::pow(delay, 2)), - }; - - d_col = std::max( - 0.0, - std::abs(d_col) - d_margin_ + acc_dtsq[0] - - std::sqrt(std::pow(acc_dtsq[0], 2) + 2 * acc_dtsq[0] * std::abs(d_col))); - yaw_col = std::max( - 0.0, - std::abs(yaw_col) - yaw_margin_ + acc_dtsq[1] - - std::sqrt(std::pow(acc_dtsq[1], 2) + 2 * acc_dtsq[1] * std::abs(yaw_col))); - - float d_r = - std::sqrt(std::abs(2 * acc_[0] * d_col)) / linear_vel; - float yaw_r = - std::sqrt(std::abs(2 * acc_[1] * yaw_col)) / std::abs(twist_.angular.z); + const float acc_dtsq[2] = { + static_cast(acc_[0] * std::pow(delay, 2)), + static_cast(acc_[1] * std::pow(delay, 2)), + }; + + d_col = std::max(0.0, std::abs(d_col) - d_margin_ + acc_dtsq[0] - + std::sqrt(std::pow(acc_dtsq[0], 2) + 2 * acc_dtsq[0] * std::abs(d_col))); + yaw_col = std::max(0.0, std::abs(yaw_col) - yaw_margin_ + acc_dtsq[1] - + std::sqrt(std::pow(acc_dtsq[1], 2) + 2 * acc_dtsq[1] * std::abs(yaw_col))); + + float d_r = std::sqrt(std::abs(2 * acc_[0] * d_col)) / linear_vel; + float yaw_r = std::sqrt(std::abs(2 * acc_[1] * yaw_col)) / std::abs(twist_.angular.z); if (!std::isfinite(d_r)) d_r = 1.0; if (!std::isfinite(yaw_r)) @@ -554,8 +600,7 @@ class SafetyLimiterNode return std::min(d_r, yaw_r); } - geometry_msgs::Twist - limit(const geometry_msgs::Twist& in) + geometry_msgs::msg::Twist limit(const geometry_msgs::msg::Twist& in) { auto out = in; if (r_lim_ < 1.0 - EPSILON) @@ -563,21 +608,19 @@ class SafetyLimiterNode out.linear.x *= r_lim_; out.linear.y *= r_lim_; out.angular.z *= r_lim_; - if (std::abs(in.linear.x - out.linear.x) > EPSILON || - std::abs(in.linear.y - out.linear.y) > EPSILON || + + if (std::abs(in.linear.x - out.linear.x) > EPSILON || std::abs(in.linear.y - out.linear.y) > EPSILON || std::abs(in.angular.z - out.angular.z) > EPSILON) { - ROS_WARN_THROTTLE( - 1.0, "safety_limiter: (%0.2f, %0.2f, %0.2f)->(%0.2f, %0.2f, %0.2f)", - in.linear.x, in.linear.y, in.angular.z, - out.linear.x, out.linear.y, out.angular.z); + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, + "safety_limiter: (%0.2f, %0.2f, %0.2f)->(%0.2f, %0.2f, %0.2f)", in.linear.x, in.linear.y, + in.angular.z, out.linear.x, out.linear.y, out.angular.z); } } return out; } - geometry_msgs::Twist - limitMaxVelocities(const geometry_msgs::Twist& in) + geometry_msgs::msg::Twist limitMaxVelocities(const geometry_msgs::msg::Twist& in) { auto out = in; if (max_values_[0] <= 0.0) @@ -595,161 +638,53 @@ class SafetyLimiterNode out.linear.y *= vel_ratio; } } - out.angular.z = (out.angular.z > 0) ? - std::min(out.angular.z, max_values_[1]) : - std::max(out.angular.z, -max_values_[1]); + out.angular.z = + (out.angular.z > 0) ? std::min(out.angular.z, max_values_[1]) : std::max(out.angular.z, -max_values_[1]); return out; } - class vec + void cbTwist(const geometry_msgs::msg::Twist& msg) { - public: - float c[2]; - vec(const float x, const float y) - { - c[0] = x; - c[1] = y; - } - vec() - { - c[0] = c[1] = 0.0; - } - float& operator[](const int& i) - { - ROS_ASSERT(i < 2); - return c[i]; - } - const float& operator[](const int& i) const - { - ROS_ASSERT(i < 2); - return c[i]; - } - vec operator-(const vec& a) const - { - vec out = *this; - out[0] -= a[0]; - out[1] -= a[1]; - return out; - } - float cross(const vec& a) const - { - return (*this)[0] * a[1] - (*this)[1] * a[0]; - } - float dot(const vec& a) const - { - return (*this)[0] * a[0] + (*this)[1] * a[1]; - } - float dist(const vec& a) const - { - return std::hypot((*this)[0] - a[0], (*this)[1] - a[1]); - } - float dist_line(const vec& a, const vec& b) const - { - return (b - a).cross((*this) - a) / b.dist(a); - } - float dist_linestrip(const vec& a, const vec& b) const - { - if ((b - a).dot((*this) - a) <= 0) - return this->dist(a); - if ((a - b).dot((*this) - b) <= 0) - return this->dist(b); - return std::abs(this->dist_line(a, b)); - } - }; - class polygon - { - public: - std::vector v; - void move(const float& x, const float& y, const float& yaw) - { - const float cos_v = cosf(yaw); - const float sin_v = sinf(yaw); - for (auto& p : v) - { - const auto tmp = p; - p[0] = cos_v * tmp[0] - sin_v * tmp[1] + x; - p[1] = sin_v * tmp[0] + cos_v * tmp[1] + y; - } - } - bool inside(const vec& a) const - { - int cn = 0; - for (size_t i = 0; i < v.size() - 1; i++) - { - auto& v1 = v[i]; - auto& v2 = v[i + 1]; - if ((v1[1] <= a[1] && a[1] < v2[1]) || - (v2[1] <= a[1] && a[1] < v1[1])) - { - float lx; - lx = v1[0] + (v2[0] - v1[0]) * (a[1] - v1[1]) / (v2[1] - v1[1]); - if (a[0] < lx) - cn++; - } - } - return ((cn & 1) == 1); - } - float dist(const vec& a) const - { - float dist = std::numeric_limits::max(); - for (size_t i = 0; i < v.size() - 1; i++) - { - auto& v1 = v[i]; - auto& v2 = v[i + 1]; - auto d = a.dist_linestrip(v1, v2); - if (d < dist) - dist = d; - } - return dist; - } - }; - - polygon footprint_p; + const rclcpp::Time current_stamp = now(); - void cbTwist(const geometry_msgs::Twist::ConstPtr& msg) - { - ros::Time now = ros::Time::now(); - - twist_ = *msg; + twist_ = msg; has_twist_ = true; - if (now - last_disable_cmd_ < ros::Duration(disable_timeout_)) + if (current_stamp - last_disable_cmd_ < rclcpp::Duration::from_seconds(disable_timeout_)) { - pub_twist_.publish(limitMaxVelocities(twist_)); + pub_twist_->publish(limitMaxVelocities(twist_)); } else if (!has_cloud_ || watchdog_stop_) { - geometry_msgs::Twist cmd_vel; - pub_twist_.publish(cmd_vel); + geometry_msgs::msg::Twist cmd_vel; + pub_twist_->publish(cmd_vel); } else { - geometry_msgs::Twist cmd_vel = limitMaxVelocities(limit(twist_)); - pub_twist_.publish(cmd_vel); - - if (now > hold_off_) + geometry_msgs::msg::Twist cmd_vel = limitMaxVelocities(limit(twist_)); + pub_twist_->publish(cmd_vel); + if (current_stamp > hold_off_) + { r_lim_ = 1.0; + } } } - void cbCloud(const sensor_msgs::PointCloud2::ConstPtr& msg) + void cbCloud(const sensor_msgs::msg::PointCloud2& msg) { - const bool can_transform = tfbuf_.canTransform( - fixed_frame_id_, msg->header.frame_id, msg->header.stamp); - const ros::Time stamp = - can_transform ? msg->header.stamp : ros::Time(0); - - sensor_msgs::PointCloud2 cloud_msg_fixed; + const bool can_transform = tfbuf_.canTransform(fixed_frame_id_, msg.header.frame_id, msg.header.stamp); + const rclcpp::Time stamp = can_transform ? rclcpp::Time(msg.header.stamp) : rclcpp::Time(0); + sensor_msgs::msg::PointCloud2 cloud_msg_fixed; try { - const geometry_msgs::TransformStamped cloud_to_fixed = - tfbuf_.lookupTransform(fixed_frame_id_, msg->header.frame_id, stamp); - tf2::doTransform(*msg, cloud_msg_fixed, cloud_to_fixed); + const geometry_msgs::msg::TransformStamped cloud_to_fixed = + tfbuf_.lookupTransform(fixed_frame_id_, msg.header.frame_id, stamp); + tf2::doTransform(msg, cloud_msg_fixed, cloud_to_fixed); } catch (tf2::TransformException& e) { - ROS_WARN_THROTTLE(1.0, "safety_limiter: Transform failed: %s", e.what()); + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "safety_limiter: Transform failed: %s", e.what()); return; } @@ -764,42 +699,39 @@ class SafetyLimiterNode } *cloud_accum_ += *cloud_fixed; cloud_accum_->header.frame_id = fixed_frame_id_; - last_cloud_stamp_ = msg->header.stamp; + last_cloud_stamp_ = msg.header.stamp; has_cloud_ = true; } - void cbDisable(const std_msgs::Bool::ConstPtr& msg) + void cbDisable(const std_msgs::msg::Bool& msg) { - if (msg->data) + if (msg.data) { - last_disable_cmd_ = ros::Time::now(); + last_disable_cmd_ = now(); } } void diagnoseCollision(diagnostic_updater::DiagnosticStatusWrapper& stat) { - safety_limiter_msgs::SafetyLimiterStatus status_msg; + safety_limiter_msgs::msg::SafetyLimiterStatus status_msg; if (!has_cloud_ || watchdog_stop_) { - stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Stopped due to data timeout."); + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Stopped due to data timeout."); } else if (r_lim_ == 1.0) { - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "OK"); + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "OK"); } else if (r_lim_ < EPSILON) { - stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, - (has_collision_at_now_) ? - "Cannot escape from collision." : - "Trying to avoid collision, but cannot move anymore."); + const auto msg = has_collision_at_now_ ? "Cannot escape from collision." : + "Trying to avoid collision, but cannot move anymore.."; + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg); } else { - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, - (has_collision_at_now_) ? - "Escaping from collision." : - "Reducing velocity to avoid collision."); + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, + (has_collision_at_now_) ? "Escaping from collision." : "Reducing velocity to avoid collision."); } stat.addf("Velocity Limit Ratio", "%.2f", r_lim_); stat.add("Pointcloud Availability", has_cloud_ ? "true" : "false"); @@ -810,7 +742,7 @@ class SafetyLimiterNode status_msg.has_watchdog_timed_out = watchdog_stop_; status_msg.stuck_started_since = stuck_started_since_; - pub_status_.publish(status_msg); + pub_status_->publish(status_msg); } }; @@ -818,10 +750,8 @@ class SafetyLimiterNode int main(int argc, char** argv) { - ros::init(argc, argv, "safety_limiter"); - - safety_limiter::SafetyLimiterNode limiter; - limiter.spin(); - + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared("trajectory_tracker")); + rclcpp::shutdown(); return 0; } diff --git a/safety_limiter/test/configs/test_params.yaml b/safety_limiter/test/configs/test_params.yaml new file mode 100644 index 000000000..6c2211d0e --- /dev/null +++ b/safety_limiter/test/configs/test_params.yaml @@ -0,0 +1,18 @@ +safety_limiter: + ros__parameters: + footprint: "[[0.0, -0.1], [0.0, 0.1], [-2.0, 0.1], [-2.0, -0.1]]" + allow_empty_cloud: false + watchdog_interval: 0.2 + max_linear_vel: 1.5 + max_angular_vel: 2.5 + cloud_timeout: 0.2 + freq: 15.0 + dt: 0.01 + lin_acc: 1.0 + lin_vel: 2.0 + ang_acc: 1.57 + ang_vel: 3.14 + d_margin: 0.0 + yaw_margin: 0.0 + z_range_min: -100.0 + z_range_max: 100.0 diff --git a/safety_limiter/test/configs/test_params2.yaml b/safety_limiter/test/configs/test_params2.yaml new file mode 100644 index 000000000..397c3bdec --- /dev/null +++ b/safety_limiter/test/configs/test_params2.yaml @@ -0,0 +1,16 @@ +safety_limiter: + ros__parameters: + footprint: "[[0.0, -0.1], [0.0, 0.1], [-2.0, 0.1], [-2.0, -0.1]]" + allow_empty_cloud: false + watchdog_interval: 0.2 + cloud_timeout: 0.2 + freq: 15.0 + dt: 0.05 + lin_acc: 1.0 + lin_vel: 2.0 + ang_acc: 1.57 + ang_vel: 3.14 + d_margin: 0.1 + yaw_margin: 0.0 + z_range_min: -100.0 + z_range_max: 100.0 diff --git a/safety_limiter/test/include/test_safety_limiter_base.h b/safety_limiter/test/include/test_safety_limiter_base.h index 923fdc902..66e72563d 100644 --- a/safety_limiter/test/include/test_safety_limiter_base.h +++ b/safety_limiter/test/include/test_safety_limiter_base.h @@ -10,8 +10,8 @@ * * 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 + * * 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" @@ -32,22 +32,22 @@ #include -#include +#include "rclcpp/rclcpp.hpp" -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include #include namespace { -inline void GenerateEmptyPointcloud2(sensor_msgs::PointCloud2& cloud) +inline void GenerateEmptyPointcloud2(sensor_msgs::msg::PointCloud2& cloud) { cloud.height = 1; cloud.width = 0; @@ -56,11 +56,8 @@ inline void GenerateEmptyPointcloud2(sensor_msgs::PointCloud2& cloud) sensor_msgs::PointCloud2Modifier modifier(cloud); modifier.setPointCloud2FieldsByString(1, "xyz"); } -inline void GenerateSinglePointPointcloud2( - sensor_msgs::PointCloud2& cloud, - const float x, - const float y, - const float z) +inline void GenerateSinglePointPointcloud2(sensor_msgs::msg::PointCloud2& cloud, const float x, const float y, + const float z) { cloud.height = 1; cloud.width = 1; @@ -78,128 +75,128 @@ inline void GenerateSinglePointPointcloud2( } } // namespace -class SafetyLimiterTest : public ::testing::Test +class SafetyLimiterTest : public ::testing::Test, public rclcpp::Node { protected: - ros::NodeHandle nh_; - ros::Publisher pub_cmd_vel_; - ros::Publisher pub_cloud_; - ros::Publisher pub_watchdog_; - ros::Subscriber sub_diag_; - ros::Subscriber sub_status_; - ros::Subscriber sub_cmd_vel_; + rclcpp::Publisher::SharedPtr pub_cmd_vel_; + rclcpp::Publisher::SharedPtr pub_cloud_; + rclcpp::Publisher::SharedPtr pub_watchdog_; + rclcpp::SubscriptionBase::SharedPtr sub_diag_; + rclcpp::SubscriptionBase::SharedPtr sub_status_; + rclcpp::SubscriptionBase::SharedPtr sub_cmd_vel_; tf2_ros::TransformBroadcaster tfb_; + bool diag_received_ = false; + bool cmd_vel_received_ = false; + bool status_received_ = false; - inline void cbDiag(const diagnostic_msgs::DiagnosticArray::ConstPtr& msg) + void resetMessages() { + diag_received_ = false; + cmd_vel_received_ = false; + status_received_ = false; + } + + inline void cbDiag(const diagnostic_msgs::msg::DiagnosticArray& msg) + { + diag_received_ = true; diag_ = msg; } - inline void cbStatus(const safety_limiter_msgs::SafetyLimiterStatus::ConstPtr& msg) + inline void cbStatus(const safety_limiter_msgs::msg::SafetyLimiterStatus& msg) { + status_received_ = true; status_ = msg; } - inline void cbCmdVel(const geometry_msgs::Twist::ConstPtr& msg) + inline void cbCmdVel(const geometry_msgs::msg::Twist& msg) { + cmd_vel_received_ = true; cmd_vel_ = msg; } public: - diagnostic_msgs::DiagnosticArray::ConstPtr diag_; - safety_limiter_msgs::SafetyLimiterStatus::ConstPtr status_; - geometry_msgs::Twist::ConstPtr cmd_vel_; + diagnostic_msgs::msg::DiagnosticArray diag_; + safety_limiter_msgs::msg::SafetyLimiterStatus status_; + geometry_msgs::msg::Twist cmd_vel_; inline SafetyLimiterTest() - : nh_() + : Node("safety_limiter_test") + , tfb_(*this) { - pub_cmd_vel_ = nh_.advertise("cmd_vel_in", 1); - pub_cloud_ = nh_.advertise("cloud", 1); - pub_watchdog_ = nh_.advertise("watchdog_reset", 1); - sub_diag_ = nh_.subscribe("diagnostics", 1, &SafetyLimiterTest::cbDiag, this); - sub_status_ = nh_.subscribe("/safety_limiter/status", 1, &SafetyLimiterTest::cbStatus, this); - sub_cmd_vel_ = nh_.subscribe("cmd_vel", 1, &SafetyLimiterTest::cbCmdVel, this); - - ros::Rate wait(10.0); + pub_cmd_vel_ = create_publisher("cmd_vel_in", 1); + pub_cloud_ = create_publisher("cloud", 1); + pub_watchdog_ = create_publisher("watchdog_reset", 1); + sub_diag_ = create_subscription( + "diagnostics", 1, std::bind(&SafetyLimiterTest::cbDiag, this, std::placeholders::_1)); + sub_status_ = create_subscription( + "/safety_limiter/status", 1, std::bind(&SafetyLimiterTest::cbStatus, this, std::placeholders::_1)); + sub_cmd_vel_ = create_subscription( + "cmd_vel", 1, std::bind(&SafetyLimiterTest::cbCmdVel, this, std::placeholders::_1)); + + rclcpp::Rate wait(10.0); // Skip initial state - for (int i = 0; i < 10 && ros::ok(); ++i) + for (int i = 0; i < 10 && rclcpp::ok(); ++i) { - publishEmptyPointPointcloud2("base_link", ros::Time::now()); + publishEmptyPointPointcloud2("base_link", now()); publishWatchdogReset(); broadcastTF("odom", "base_link", 0.0, 0.0); wait.sleep(); - ros::spinOnce(); + rclcpp::spin_some(get_node_base_interface()); } - cmd_vel_.reset(); - diag_.reset(); - status_.reset(); } inline void publishWatchdogReset() { - std_msgs::Empty watchdog_reset; - pub_watchdog_.publish(watchdog_reset); + std_msgs::msg::Empty watchdog_reset; + pub_watchdog_->publish(watchdog_reset); } - inline void publishEmptyPointPointcloud2( - const std::string frame_id, - const ros::Time stamp) + inline void publishEmptyPointPointcloud2(const std::string frame_id, const rclcpp::Time stamp) { - sensor_msgs::PointCloud2 cloud; + sensor_msgs::msg::PointCloud2 cloud; cloud.header.frame_id = frame_id; cloud.header.stamp = stamp; GenerateEmptyPointcloud2(cloud); - pub_cloud_.publish(cloud); + pub_cloud_->publish(cloud); } - inline void publishSinglePointPointcloud2( - const float x, - const float y, - const float z, - const std::string frame_id, - const ros::Time stamp) + inline void publishSinglePointPointcloud2(const float x, const float y, const float z, const std::string frame_id, + const rclcpp::Time stamp) { - sensor_msgs::PointCloud2 cloud; + sensor_msgs::msg::PointCloud2 cloud; cloud.header.frame_id = frame_id; cloud.header.stamp = stamp; GenerateSinglePointPointcloud2(cloud, x, y, z); - pub_cloud_.publish(cloud); + pub_cloud_->publish(cloud); } - inline void publishTwist( - const float lin, - const float ang, - const float lin_y = 0.0) + inline void publishTwist(const float lin, const float ang, const float lin_y = 0.0) { - geometry_msgs::Twist cmd_vel_out; + geometry_msgs::msg::Twist cmd_vel_out; cmd_vel_out.linear.x = lin; cmd_vel_out.linear.y = lin_y; cmd_vel_out.angular.z = ang; - pub_cmd_vel_.publish(cmd_vel_out); + pub_cmd_vel_->publish(cmd_vel_out); } - inline void broadcastTF( - const std::string parent_frame_id, - const std::string child_frame_id, - const float lin, - const float ang) + inline void broadcastTF(const std::string parent_frame_id, const std::string child_frame_id, const float lin, + const float ang) { - geometry_msgs::TransformStamped trans; - trans.header.stamp = ros::Time::now(); - trans.transform = tf2::toMsg( - tf2::Transform(tf2::Quaternion(tf2::Vector3(0, 0, 1), ang), tf2::Vector3(lin, 0, 0))); + geometry_msgs::msg::TransformStamped trans; + trans.header.stamp = now(); + trans.transform = tf2::toMsg(tf2::Transform(tf2::Quaternion(tf2::Vector3(0, 0, 1), ang), tf2::Vector3(lin, 0, 0))); trans.header.frame_id = parent_frame_id; trans.child_frame_id = child_frame_id; tfb_.sendTransform(trans); } inline bool hasDiag() const { - if (!diag_) + if (!diag_received_) return false; - if (diag_->status.size() == 0) + if (diag_.status.size() == 0) return false; return true; } inline bool hasStatus() const { - return static_cast(status_); + return diag_received_; } }; diff --git a/safety_limiter/test/src/test_safety_limiter.cpp b/safety_limiter/test/src/test_safety_limiter.cpp index a67fb96ef..7e40f275d 100644 --- a/safety_limiter/test/src/test_safety_limiter.cpp +++ b/safety_limiter/test/src/test_safety_limiter.cpp @@ -31,33 +31,115 @@ #include #include -#include +#include "rclcpp/rclcpp.hpp" -#include -#include -#include +#include +#include +#include #include #include +using std::chrono::duration; +using std::chrono::duration_cast; +using std::chrono::nanoseconds; + +class RosRate : public rclcpp::RateBase +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(RosRate) + + explicit RosRate(double rate, rclcpp::Node& node) + : RosRate(duration_cast(duration(1.0 / rate)), node) + { + } + explicit RosRate(std::chrono::nanoseconds period, rclcpp::Node& node) + : node_(node.get_node_base_interface()) + , clock_(node.get_clock()) + , period_(period) + { + last_interval_ = clock_->now(); + } + + virtual bool sleep() + { + // Time coming into sleep + auto now = clock_->now(); + // Time of next interval + auto next_interval = last_interval_ + period_; + // Detect backwards time flow + if (now < last_interval_) + { + // Best thing to do is to set the next_interval to now + period + next_interval = now + period_; + } + // Calculate the time to sleep + auto time_to_sleep = next_interval - now; + // Update the interval + last_interval_ += period_; + // If the time_to_sleep is negative or zero, don't sleep + if (time_to_sleep <= std::chrono::seconds(0)) + { + // If an entire cycle was missed then reset next interval. + // This might happen if the loop took more than a cycle. + // Or if time jumps forward. + if (now > next_interval + period_) + { + last_interval_ = now + period_; + } + // Either way do not sleep and return false + return false; + } + // Sleep (will get interrupted by ctrl-c, may not sleep full time) + while (clock_->now() < next_interval) + { + rclcpp::spin_some(node_); + clock_->sleep_for(std::chrono::milliseconds(1)); + } + return true; + } + + virtual bool is_steady() const + { + return false; + } + + virtual void reset() + { + last_interval_ = clock_->now(); + } + + std::chrono::nanoseconds period() const + { + return std::chrono::nanoseconds(period_.nanoseconds()); + } + +private: + RCLCPP_DISABLE_COPY(RosRate) + + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_; + rclcpp::Clock::SharedPtr clock_; + rclcpp::Duration period_; + rclcpp::Time last_interval_; +}; + TEST_F(SafetyLimiterTest, Timeouts) { - ros::Rate wait(10.0); + resetMessages(); + RosRate wait(10.0, *this); for (int with_cloud = 0; with_cloud < 3; ++with_cloud) { for (int with_watchdog_reset = 0; with_watchdog_reset < 2; ++with_watchdog_reset) { const std::string test_condition = - "with_watchdog_reset: " + std::to_string(with_watchdog_reset) + - ", with_cloud: " + std::to_string(with_cloud); - ros::Duration(0.3).sleep(); + "with_watchdog_reset: " + std::to_string(with_watchdog_reset) + ", with_cloud: " + std::to_string(with_cloud); + this->get_clock()->sleep_for(rclcpp::Duration::from_seconds(0.3)); - cmd_vel_.reset(); for (int i = 0; i < 20; ++i) { - ASSERT_TRUE(ros::ok()); + ASSERT_TRUE(rclcpp::ok()); if (with_watchdog_reset > 0) publishWatchdogReset(); @@ -66,9 +148,9 @@ TEST_F(SafetyLimiterTest, Timeouts) { // cloud must have timestamp, otherwise the robot stops if (with_cloud > 1) - publishSinglePointPointcloud2(1000, 1000, 0, "base_link", ros::Time::now()); + publishSinglePointPointcloud2(1000, 1000, 0, "base_link", now()); else - publishSinglePointPointcloud2(1000, 1000, 0, "base_link", ros::Time()); + publishSinglePointPointcloud2(1000, 1000, 0, "base_link", rclcpp::Time()); } const float vel = 0.1; @@ -77,53 +159,53 @@ TEST_F(SafetyLimiterTest, Timeouts) broadcastTF("odom", "base_link", 0.0, 0.0); wait.sleep(); - ros::spinOnce(); + rclcpp::spin_some(get_node_base_interface()); - if (i > 5 && cmd_vel_) + if (i > 5 && cmd_vel_received_) { if (with_watchdog_reset > 0 && with_cloud > 1) { - ASSERT_EQ(cmd_vel_->linear.x, vel) << test_condition; - ASSERT_EQ(cmd_vel_->linear.y, 0.0) << test_condition; - ASSERT_EQ(cmd_vel_->linear.z, 0.0) << test_condition; - ASSERT_EQ(cmd_vel_->angular.x, 0.0) << test_condition; - ASSERT_EQ(cmd_vel_->angular.y, 0.0) << test_condition; - ASSERT_EQ(cmd_vel_->angular.z, ang_vel) << test_condition; + ASSERT_EQ(cmd_vel_.linear.x, vel) << test_condition; + ASSERT_EQ(cmd_vel_.linear.y, 0.0) << test_condition; + ASSERT_EQ(cmd_vel_.linear.z, 0.0) << test_condition; + ASSERT_EQ(cmd_vel_.angular.x, 0.0) << test_condition; + ASSERT_EQ(cmd_vel_.angular.y, 0.0) << test_condition; + ASSERT_EQ(cmd_vel_.angular.z, ang_vel) << test_condition; } else { - ASSERT_EQ(cmd_vel_->linear.x, 0.0) << test_condition; - ASSERT_EQ(cmd_vel_->linear.y, 0.0) << test_condition; - ASSERT_EQ(cmd_vel_->linear.z, 0.0) << test_condition; - ASSERT_EQ(cmd_vel_->angular.x, 0.0) << test_condition; - ASSERT_EQ(cmd_vel_->angular.y, 0.0) << test_condition; - ASSERT_EQ(cmd_vel_->angular.z, 0.0) << test_condition; + ASSERT_EQ(cmd_vel_.linear.x, 0.0) << test_condition; + ASSERT_EQ(cmd_vel_.linear.y, 0.0) << test_condition; + ASSERT_EQ(cmd_vel_.linear.z, 0.0) << test_condition; + ASSERT_EQ(cmd_vel_.angular.x, 0.0) << test_condition; + ASSERT_EQ(cmd_vel_.angular.y, 0.0) << test_condition; + ASSERT_EQ(cmd_vel_.angular.z, 0.0) << test_condition; } } } ASSERT_TRUE(hasStatus()) << test_condition; - EXPECT_EQ(with_cloud > 1, status_->is_cloud_available) << test_condition; - EXPECT_EQ(status_->stuck_started_since, ros::Time(0)) << test_condition; + EXPECT_EQ(with_cloud > 1, status_.is_cloud_available) << test_condition; + EXPECT_EQ(status_.stuck_started_since, rclcpp::Time(0)) << test_condition; if (with_watchdog_reset > 0 && with_cloud > 1) { ASSERT_TRUE(hasDiag()) << test_condition; - EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, diag_->status[0].level) + EXPECT_EQ(diagnostic_msgs::msg::DiagnosticStatus::OK, diag_.status[0].level) << test_condition << ", " - << "message: " << diag_->status[0].message; + << "message: " << diag_.status[0].message; - EXPECT_FALSE(status_->has_watchdog_timed_out) << test_condition; + EXPECT_FALSE(status_.has_watchdog_timed_out) << test_condition; } else { ASSERT_TRUE(hasDiag()) << test_condition; - EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::ERROR, diag_->status[0].level) + EXPECT_EQ(diagnostic_msgs::msg::DiagnosticStatus::ERROR, diag_.status[0].level) << test_condition << ", " - << "message: " << diag_->status[0].message; + << "message: " << diag_.status[0].message; if (with_watchdog_reset == 0) { - EXPECT_TRUE(status_->has_watchdog_timed_out) << test_condition; + EXPECT_TRUE(status_.has_watchdog_timed_out) << test_condition; } } } @@ -132,23 +214,24 @@ TEST_F(SafetyLimiterTest, Timeouts) TEST_F(SafetyLimiterTest, CloudBuffering) { - ros::Rate wait(60.0); + resetMessages(); + RosRate wait(60.0, *this); // Skip initial state - for (int i = 0; i < 30 && ros::ok(); ++i) + for (int i = 0; i < 30 && rclcpp::ok(); ++i) { - publishSinglePointPointcloud2(0.5, 0, 0, "base_link", ros::Time::now()); + publishSinglePointPointcloud2(0.5, 0, 0, "base_link", now()); publishWatchdogReset(); wait.sleep(); - ros::spinOnce(); + rclcpp::spin_some(get_node_base_interface()); } bool received = false; bool en = false; // 1.0 m/ss, obstacle at 0.5 m: limited to 1.0 m/s (t_margin: 0) - for (int i = 0; i < 60 * 6 && ros::ok(); ++i) + for (int i = 0; i < 60 * 6 && rclcpp::ok(); ++i) { // enable check after two cycles of safety_limiter if (i > 8) @@ -157,20 +240,19 @@ TEST_F(SafetyLimiterTest, CloudBuffering) // safety_limiter must check 4 buffered clouds // 3/4 of pointclouds have collision point if ((i % 4) == 0) - publishSinglePointPointcloud2(10, 0, 0, "base_link", ros::Time::now()); + publishSinglePointPointcloud2(10, 0, 0, "base_link", now()); else - publishSinglePointPointcloud2(0.5, 0, 0, "base_link", ros::Time::now()); + publishSinglePointPointcloud2(0.5, 0, 0, "base_link", now()); publishWatchdogReset(); publishTwist(2.0, 0); broadcastTF("odom", "base_link", 0.0, 0.0); - wait.sleep(); - ros::spinOnce(); - if (en && cmd_vel_) + rclcpp::spin_some(get_node_base_interface()); + if (en && cmd_vel_received_) { received = true; - ASSERT_NEAR(cmd_vel_->linear.x, 1.0, 1e-1); + ASSERT_NEAR(cmd_vel_.linear.x, 1.0, 1e-1); } } ASSERT_TRUE(received); @@ -178,16 +260,17 @@ TEST_F(SafetyLimiterTest, CloudBuffering) TEST_F(SafetyLimiterTest, SafetyLimitLinear) { - ros::Rate wait(20.0); + resetMessages(); + RosRate wait(20.0, *this); // Skip initial state - for (int i = 0; i < 10 && ros::ok(); ++i) + for (int i = 0; i < 10 && rclcpp::ok(); ++i) { - publishSinglePointPointcloud2(0.5, 0, 0, "base_link", ros::Time::now()); + publishSinglePointPointcloud2(0.5, 0, 0, "base_link", now()); publishWatchdogReset(); wait.sleep(); - ros::spinOnce(); + rclcpp::spin_some(get_node_base_interface()); } for (float vel = 0.0; vel < 2.0; vel += 0.4) @@ -196,32 +279,32 @@ TEST_F(SafetyLimiterTest, SafetyLimitLinear) bool received = false; bool en = false; - for (int i = 0; i < 10 && ros::ok(); ++i) + for (int i = 0; i < 10 && rclcpp::ok(); ++i) { if (i > 5) en = true; - publishSinglePointPointcloud2(0.5, 0, 0, "base_link", ros::Time::now()); + publishSinglePointPointcloud2(0.5, 0, 0, "base_link", now()); publishWatchdogReset(); publishTwist(vel, ((i % 5) - 2.0) * 0.01); broadcastTF("odom", "base_link", 0.0, 0.0); wait.sleep(); - ros::spinOnce(); - if (en && cmd_vel_) + rclcpp::spin_some(get_node_base_interface()); + if (en && cmd_vel_received_) { received = true; const float expected_vel = std::min(vel, 1.0); - ASSERT_NEAR(cmd_vel_->linear.x, expected_vel, 1e-1); + ASSERT_NEAR(cmd_vel_.linear.x, expected_vel, 1e-1); } } ASSERT_TRUE(hasDiag()); - EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, diag_->status[0].level) - << "message: " << diag_->status[0].message; + EXPECT_EQ(diagnostic_msgs::msg::DiagnosticStatus::OK, diag_.status[0].level) + << "message: " << diag_.status[0].message; ASSERT_TRUE(hasStatus()); - EXPECT_TRUE(status_->is_cloud_available); - EXPECT_FALSE(status_->has_watchdog_timed_out); - EXPECT_EQ(status_->stuck_started_since, ros::Time(0)); + EXPECT_TRUE(status_.is_cloud_available); + EXPECT_FALSE(status_.has_watchdog_timed_out); + EXPECT_EQ(status_.stuck_started_since, rclcpp::Time(0)); ASSERT_TRUE(received); } @@ -229,16 +312,17 @@ TEST_F(SafetyLimiterTest, SafetyLimitLinear) TEST_F(SafetyLimiterTest, SafetyLimitLinearBackward) { - ros::Rate wait(20.0); + resetMessages(); + RosRate wait(20.0, *this); // Skip initial state - for (int i = 0; i < 10 && ros::ok(); ++i) + for (int i = 0; i < 10 && rclcpp::ok(); ++i) { - publishSinglePointPointcloud2(-2.5, 0, 0, "base_link", ros::Time::now()); + publishSinglePointPointcloud2(-2.5, 0, 0, "base_link", now()); publishWatchdogReset(); wait.sleep(); - ros::spinOnce(); + rclcpp::spin_some(get_node_base_interface()); } for (float vel = 0.0; vel > -2.0; vel -= 0.4) @@ -247,32 +331,32 @@ TEST_F(SafetyLimiterTest, SafetyLimitLinearBackward) bool received = false; bool en = false; - for (int i = 0; i < 10 && ros::ok(); ++i) + for (int i = 0; i < 10 && rclcpp::ok(); ++i) { if (i > 5) en = true; - publishSinglePointPointcloud2(-2.5, 0, 0, "base_link", ros::Time::now()); + publishSinglePointPointcloud2(-2.5, 0, 0, "base_link", now()); publishWatchdogReset(); publishTwist(vel, ((i % 5) - 2.0) * 0.01); broadcastTF("odom", "base_link", 0.0, 0.0); wait.sleep(); - ros::spinOnce(); - if (en && cmd_vel_) + rclcpp::spin_some(get_node_base_interface()); + if (en && cmd_vel_received_) { received = true; const float expected_vel = std::max(vel, -1.0); - ASSERT_NEAR(cmd_vel_->linear.x, expected_vel, 1e-1); + ASSERT_NEAR(cmd_vel_.linear.x, expected_vel, 1e-1); } } ASSERT_TRUE(hasDiag()); - EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, diag_->status[0].level) - << "message: " << diag_->status[0].message; + EXPECT_EQ(diagnostic_msgs::msg::DiagnosticStatus::OK, diag_.status[0].level) + << "message: " << diag_.status[0].message; ASSERT_TRUE(hasStatus()); - EXPECT_TRUE(status_->is_cloud_available); - EXPECT_FALSE(status_->has_watchdog_timed_out); - EXPECT_EQ(status_->stuck_started_since, ros::Time(0)); + EXPECT_TRUE(status_.is_cloud_available); + EXPECT_FALSE(status_.has_watchdog_timed_out); + EXPECT_EQ(status_.stuck_started_since, rclcpp::Time(0)); ASSERT_TRUE(received); } @@ -280,16 +364,17 @@ TEST_F(SafetyLimiterTest, SafetyLimitLinearBackward) TEST_F(SafetyLimiterTest, SafetyLimitLinearEscape) { - ros::Rate wait(20.0); + resetMessages(); + RosRate wait(20.0, *this); // Skip initial state - for (int i = 0; i < 10 && ros::ok(); ++i) + for (int i = 0; i < 10 && rclcpp::ok(); ++i) { - publishSinglePointPointcloud2(-0.05, 0, 0, "base_link", ros::Time::now()); + publishSinglePointPointcloud2(-0.05, 0, 0, "base_link", now()); publishWatchdogReset(); wait.sleep(); - ros::spinOnce(); + rclcpp::spin_some(get_node_base_interface()); } const float vel_ref[] = {-0.2, -0.4, 0.2, 0.4}; @@ -299,49 +384,49 @@ TEST_F(SafetyLimiterTest, SafetyLimitLinearEscape) bool received = false; bool en = false; - for (int i = 0; i < 10 && ros::ok(); ++i) + for (int i = 0; i < 10 && rclcpp::ok(); ++i) { if (i > 5) en = true; - publishSinglePointPointcloud2(-0.05, 0, 0, "base_link", ros::Time::now()); + publishSinglePointPointcloud2(-0.05, 0, 0, "base_link", now()); publishWatchdogReset(); publishTwist(vel, 0.0); broadcastTF("odom", "base_link", 0.0, 0.0); wait.sleep(); - ros::spinOnce(); - if (en && cmd_vel_) + rclcpp::spin_some(get_node_base_interface()); + if (en && cmd_vel_received_) { received = true; if (vel < 0) { // escaping from collision must be allowed - ASSERT_NEAR(cmd_vel_->linear.x, vel, 1e-1); + ASSERT_NEAR(cmd_vel_.linear.x, vel, 1e-1); } else { // colliding motion must be limited - ASSERT_NEAR(cmd_vel_->linear.x, 0.0, 1e-1); + ASSERT_NEAR(cmd_vel_.linear.x, 0.0, 1e-1); } } } if (vel < 0) { ASSERT_TRUE(hasDiag()); - EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, diag_->status[0].level) - << "message: " << diag_->status[0].message; + EXPECT_EQ(diagnostic_msgs::msg::DiagnosticStatus::OK, diag_.status[0].level) + << "message: " << diag_.status[0].message; } else { ASSERT_TRUE(hasDiag()); - EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::WARN, diag_->status[0].level) - << "message: " << diag_->status[0].message; + EXPECT_EQ(diagnostic_msgs::msg::DiagnosticStatus::WARN, diag_.status[0].level) + << "message: " << diag_.status[0].message; } ASSERT_TRUE(hasStatus()); - EXPECT_TRUE(status_->is_cloud_available); - EXPECT_FALSE(status_->has_watchdog_timed_out); - EXPECT_NE(status_->stuck_started_since, ros::Time(0)); + EXPECT_TRUE(status_.is_cloud_available); + EXPECT_FALSE(status_.has_watchdog_timed_out); + EXPECT_NE(status_.stuck_started_since, rclcpp::Time(0)); ASSERT_TRUE(received); } @@ -349,16 +434,17 @@ TEST_F(SafetyLimiterTest, SafetyLimitLinearEscape) TEST_F(SafetyLimiterTest, SafetyLimitAngular) { - ros::Rate wait(20.0); + resetMessages(); + RosRate wait(20.0, *this); // Skip initial state - for (int i = 0; i < 10 && ros::ok(); ++i) + for (int i = 0; i < 10 && rclcpp::ok(); ++i) { - publishSinglePointPointcloud2(-1, -1, 0, "base_link", ros::Time::now()); + publishSinglePointPointcloud2(-1, -1, 0, "base_link", now()); publishWatchdogReset(); wait.sleep(); - ros::spinOnce(); + rclcpp::spin_some(get_node_base_interface()); } for (float vel = 0.0; vel < M_PI; vel += M_PI / 10) @@ -367,32 +453,32 @@ TEST_F(SafetyLimiterTest, SafetyLimitAngular) bool received = false; bool en = false; - for (int i = 0; i < 10 && ros::ok(); ++i) + for (int i = 0; i < 10 && rclcpp::ok(); ++i) { if (i > 5) en = true; - publishSinglePointPointcloud2(-1, -1.1, 0, "base_link", ros::Time::now()); + publishSinglePointPointcloud2(-1, -1.1, 0, "base_link", now()); publishWatchdogReset(); publishTwist((i % 3) * 0.01, vel); broadcastTF("odom", "base_link", 0.0, 0.0); wait.sleep(); - ros::spinOnce(); - if (en && cmd_vel_) + rclcpp::spin_some(get_node_base_interface()); + if (en && cmd_vel_received_) { received = true; const float expected_vel = std::min(vel, M_PI / 2); - ASSERT_NEAR(cmd_vel_->angular.z, expected_vel, M_PI / 20); + ASSERT_NEAR(cmd_vel_.angular.z, expected_vel, M_PI / 20); } } ASSERT_TRUE(hasDiag()); - EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, diag_->status[0].level) - << "message: " << diag_->status[0].message; + EXPECT_EQ(diagnostic_msgs::msg::DiagnosticStatus::OK, diag_.status[0].level) + << "message: " << diag_.status[0].message; ASSERT_TRUE(hasStatus()); - EXPECT_TRUE(status_->is_cloud_available); - EXPECT_FALSE(status_->has_watchdog_timed_out); - EXPECT_EQ(status_->stuck_started_since, ros::Time(0)); + EXPECT_TRUE(status_.is_cloud_available); + EXPECT_FALSE(status_.has_watchdog_timed_out); + EXPECT_EQ(status_.stuck_started_since, rclcpp::Time(0)); ASSERT_TRUE(received); } @@ -400,16 +486,17 @@ TEST_F(SafetyLimiterTest, SafetyLimitAngular) TEST_F(SafetyLimiterTest, SafetyLimitAngularEscape) { - ros::Rate wait(20.0); + resetMessages(); + RosRate wait(20.0, *this); // Skip initial state - for (int i = 0; i < 10 && ros::ok(); ++i) + for (int i = 0; i < 10 && rclcpp::ok(); ++i) { - publishSinglePointPointcloud2(-1, -0.09, 0, "base_link", ros::Time::now()); + publishSinglePointPointcloud2(-1, -0.09, 0, "base_link", now()); publishWatchdogReset(); wait.sleep(); - ros::spinOnce(); + rclcpp::spin_some(get_node_base_interface()); } const float vel_ref[] = {-0.2, -0.4, 0.2, 0.4}; @@ -419,49 +506,49 @@ TEST_F(SafetyLimiterTest, SafetyLimitAngularEscape) bool received = false; bool en = false; - for (int i = 0; i < 10 && ros::ok(); ++i) + for (int i = 0; i < 10 && rclcpp::ok(); ++i) { if (i > 5) en = true; - publishSinglePointPointcloud2(-1, -0.09, 0, "base_link", ros::Time::now()); + publishSinglePointPointcloud2(-1, -0.09, 0, "base_link", now()); publishWatchdogReset(); publishTwist(0.0, vel); broadcastTF("odom", "base_link", 0.0, 0.0); wait.sleep(); - ros::spinOnce(); - if (en && cmd_vel_) + rclcpp::spin_some(get_node_base_interface()); + if (en && cmd_vel_received_) { received = true; if (vel < 0) { // escaping from collision must be allowed - ASSERT_NEAR(cmd_vel_->angular.z, vel, 1e-1); + ASSERT_NEAR(cmd_vel_.angular.z, vel, 1e-1); } else { // colliding motion must be limited - ASSERT_NEAR(cmd_vel_->angular.z, 0.0, 1e-1); + ASSERT_NEAR(cmd_vel_.angular.z, 0.0, 1e-1); } } } if (vel < 0) { ASSERT_TRUE(hasDiag()); - EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, diag_->status[0].level) - << "message: " << diag_->status[0].message; + EXPECT_EQ(diagnostic_msgs::msg::DiagnosticStatus::OK, diag_.status[0].level) + << "message: " << diag_.status[0].message; } else { ASSERT_TRUE(hasDiag()); - EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::WARN, diag_->status[0].level) - << "message: " << diag_->status[0].message; + EXPECT_EQ(diagnostic_msgs::msg::DiagnosticStatus::WARN, diag_.status[0].level) + << "message: " << diag_.status[0].message; } ASSERT_TRUE(hasStatus()); - EXPECT_TRUE(status_->is_cloud_available); - EXPECT_FALSE(status_->has_watchdog_timed_out); - EXPECT_NE(status_->stuck_started_since, ros::Time(0)); + EXPECT_TRUE(status_.is_cloud_available); + EXPECT_FALSE(status_.has_watchdog_timed_out); + EXPECT_NE(status_.stuck_started_since, rclcpp::Time(0)); ASSERT_TRUE(received); } @@ -469,7 +556,8 @@ TEST_F(SafetyLimiterTest, SafetyLimitAngularEscape) TEST_F(SafetyLimiterTest, NoCollision) { - ros::Rate wait(20.0); + resetMessages(); + RosRate wait(20.0, *this); for (float vel = 0.0; vel < 1.0; vel += 0.2) { @@ -478,43 +566,44 @@ TEST_F(SafetyLimiterTest, NoCollision) bool received = false; bool en = false; - for (int i = 0; i < 10 && ros::ok(); ++i) + for (int i = 0; i < 10 && rclcpp::ok(); ++i) { if (i > 5) en = true; - publishSinglePointPointcloud2(1000, 1000, 0, "base_link", ros::Time::now()); + publishSinglePointPointcloud2(1000, 1000, 0, "base_link", now()); publishWatchdogReset(); publishTwist(vel, ang_vel); broadcastTF("odom", "base_link", 0.0, 0.0); wait.sleep(); - ros::spinOnce(); - if (en && cmd_vel_) + rclcpp::spin_some(get_node_base_interface()); + if (en && cmd_vel_received_) { received = true; - ASSERT_NEAR(cmd_vel_->linear.x, vel, 1e-3); - ASSERT_NEAR(cmd_vel_->angular.z, ang_vel, 1e-3); + ASSERT_NEAR(cmd_vel_.linear.x, vel, 1e-3); + ASSERT_NEAR(cmd_vel_.angular.z, ang_vel, 1e-3); } } ASSERT_TRUE(received); ASSERT_TRUE(hasDiag()); - EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, diag_->status[0].level) - << "message: " << diag_->status[0].message; + EXPECT_EQ(diagnostic_msgs::msg::DiagnosticStatus::OK, diag_.status[0].level) + << "message: " << diag_.status[0].message; ASSERT_TRUE(hasStatus()); - EXPECT_EQ(1.0, status_->limit_ratio); - EXPECT_TRUE(status_->is_cloud_available); - EXPECT_FALSE(status_->has_watchdog_timed_out); - EXPECT_EQ(status_->stuck_started_since, ros::Time(0)); + EXPECT_EQ(1.0, status_.limit_ratio); + EXPECT_TRUE(status_.is_cloud_available); + EXPECT_FALSE(status_.has_watchdog_timed_out); + EXPECT_EQ(status_.stuck_started_since, rclcpp::Time(0)); } } } TEST_F(SafetyLimiterTest, SafetyLimitLinearSimpleSimulation) { + resetMessages(); const float dt = 0.02; - ros::Rate wait(1.0 / dt); + RosRate wait(1.0 / dt, *this); const float velocities[] = {-0.8, -0.5, 0.5, 0.8}; for (const float vel : velocities) @@ -523,26 +612,26 @@ TEST_F(SafetyLimiterTest, SafetyLimitLinearSimpleSimulation) bool stopped = false; int count_after_stop = 10; - for (float t = 0; t < 10.0 && ros::ok() && count_after_stop > 0; t += dt) + for (float t = 0; t < 10.0 && rclcpp::ok() && count_after_stop > 0; t += dt) { if (vel > 0) - publishSinglePointPointcloud2(1.0 - x, 0, 0, "base_link", ros::Time::now()); + publishSinglePointPointcloud2(1.0 - x, 0, 0, "base_link", now()); else - publishSinglePointPointcloud2(-3.0 - x, 0, 0, "base_link", ros::Time::now()); + publishSinglePointPointcloud2(-3.0 - x, 0, 0, "base_link", now()); publishWatchdogReset(); publishTwist(vel, 0.0); broadcastTF("odom", "base_link", x, 0.0); wait.sleep(); - ros::spinOnce(); - if (cmd_vel_) + rclcpp::spin_some(get_node_base_interface()); + if (cmd_vel_received_) { - if (std::abs(cmd_vel_->linear.x) < 1e-4 && x > 0.5) + if (std::abs(cmd_vel_.linear.x) < 1e-4 && x > 0.5) { stopped = true; } - x += dt * cmd_vel_->linear.x; + x += dt * cmd_vel_.linear.x; } if (stopped) { @@ -564,16 +653,13 @@ TEST_F(SafetyLimiterTest, SafetyLimitLinearSimpleSimulation) TEST_F(SafetyLimiterTest, SafetyLimitMaxVelocitiesValues) { - ros::Rate wait(20); + resetMessages(); + RosRate wait(20, *this); - const float linear_velocities[] = - {-1.7, -1.5, 0.0, 1.5, 1.7}; - const float angular_velocities[] = - {-2.7, -2.5, 0.0, 2.5, 2.7}; - const float expected_linear_velocities[] = - {-1.5, -1.5, 0.0, 1.5, 1.5}; - const float expected_angular_velocities[] = - {-2.5, -2.5, 0.0, 2.5, 2.5}; + const float linear_velocities[] = {-1.7, -1.5, 0.0, 1.5, 1.7}; + const float angular_velocities[] = {-2.7, -2.5, 0.0, 2.5, 2.7}; + const float expected_linear_velocities[] = {-1.5, -1.5, 0.0, 1.5, 1.5}; + const float expected_angular_velocities[] = {-2.5, -2.5, 0.0, 2.5, 2.5}; for (int linear_index = 0; linear_index < 5; linear_index++) { @@ -582,53 +668,54 @@ TEST_F(SafetyLimiterTest, SafetyLimitMaxVelocitiesValues) bool received = false; bool en = false; - for (int i = 0; i < 10 && ros::ok(); ++i) + for (int i = 0; i < 10 && rclcpp::ok(); ++i) { if (i > 5) en = true; - publishSinglePointPointcloud2(1000, 1000, 0, "base_link", ros::Time::now()); + publishSinglePointPointcloud2(1000, 1000, 0, "base_link", now()); publishWatchdogReset(); publishTwist(linear_velocities[linear_index], angular_velocities[angular_index]); broadcastTF("odom", "base_link", 0.0, 0.0); wait.sleep(); - ros::spinOnce(); - if (en && cmd_vel_) + rclcpp::spin_some(get_node_base_interface()); + if (en && cmd_vel_received_) { received = true; - ASSERT_NEAR(expected_linear_velocities[linear_index], cmd_vel_->linear.x, 1e-3); - ASSERT_NEAR(expected_angular_velocities[angular_index], cmd_vel_->angular.z, 1e-3); + ASSERT_NEAR(expected_linear_velocities[linear_index], cmd_vel_.linear.x, 1e-3); + ASSERT_NEAR(expected_angular_velocities[angular_index], cmd_vel_.angular.z, 1e-3); } } ASSERT_TRUE(received); ASSERT_TRUE(hasDiag()); - EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, diag_->status[0].level) - << "message: " << diag_->status[0].message; + EXPECT_EQ(diagnostic_msgs::msg::DiagnosticStatus::OK, diag_.status[0].level) + << "message: " << diag_.status[0].message; ASSERT_TRUE(hasStatus()); - EXPECT_EQ(1.0, status_->limit_ratio); - EXPECT_TRUE(status_->is_cloud_available); - EXPECT_FALSE(status_->has_watchdog_timed_out); - EXPECT_EQ(status_->stuck_started_since, ros::Time(0)); + EXPECT_EQ(1.0, status_.limit_ratio); + EXPECT_TRUE(status_.is_cloud_available); + EXPECT_FALSE(status_.has_watchdog_timed_out); + EXPECT_EQ(status_.stuck_started_since, rclcpp::Time(0)); } } } TEST_F(SafetyLimiterTest, SafetyLimitOmniDirectional) { - ros::Rate wait(20.0); + resetMessages(); + RosRate wait(20, *this); const double vel = 1.5; const double threshold_angle = std::atan2(1.0, 0.1); for (double angle = -M_PI; angle < M_PI; angle += M_PI / 8) { - for (int i = 0; i < 10 && ros::ok(); ++i) + for (int i = 0; i < 10 && rclcpp::ok(); ++i) { - publishSinglePointPointcloud2(2.0, 0, 0, "base_link", ros::Time::now()); + publishSinglePointPointcloud2(2.0, 0, 0, "base_link", now()); publishWatchdogReset(); publishTwist(0, 0); wait.sleep(); - ros::spinOnce(); + rclcpp::spin_some(get_node_base_interface()); } double obstacle_x; @@ -661,37 +748,35 @@ TEST_F(SafetyLimiterTest, SafetyLimitOmniDirectional) // 1.0 m/ss, obstacle at 0.5 m: limited to 1.0 m/s bool received = false; bool en = false; - for (int i = 0; i < 10 && ros::ok(); ++i) + for (int i = 0; i < 10 && rclcpp::ok(); ++i) { if (i > 5) en = true; - publishSinglePointPointcloud2(obstacle_x, obstacle_y, 0, "base_link", ros::Time::now()); + publishSinglePointPointcloud2(obstacle_x, obstacle_y, 0, "base_link", now()); publishWatchdogReset(); publishTwist(vel * std::cos(angle), 0, vel * std::sin(angle)); broadcastTF("odom", "base_link", 0.0, 0.0); wait.sleep(); - ros::spinOnce(); - if (en && cmd_vel_) + rclcpp::spin_some(get_node_base_interface()); + if (en && cmd_vel_received_) { received = true; - const double current_speed = std::hypot(cmd_vel_->linear.x, cmd_vel_->linear.y); + const double current_speed = std::hypot(cmd_vel_.linear.x, cmd_vel_.linear.y); ASSERT_NEAR(current_speed, 1.0, 1e-1) - << " Angle: " << angle << " i: " << i - << " vel: (" << cmd_vel_->linear.x << "," << cmd_vel_->linear.y << ")"; - ASSERT_FLOAT_EQ(std::atan2(cmd_vel_->linear.y, cmd_vel_->linear.x), angle) - << " Angle: " << angle << " i: " << i - << " vel: (" << cmd_vel_->linear.x << "," << cmd_vel_->linear.y << ")"; + << " Angle: " << angle << " i: " << i << " vel: (" << cmd_vel_.linear.x << "," << cmd_vel_.linear.y << ")"; + ASSERT_FLOAT_EQ(std::atan2(cmd_vel_.linear.y, cmd_vel_.linear.x), angle) + << " Angle: " << angle << " i: " << i << " vel: (" << cmd_vel_.linear.x << "," << cmd_vel_.linear.y << ")"; } } ASSERT_TRUE(hasDiag()); - EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, diag_->status[0].level) - << "message: " << diag_->status[0].message; + EXPECT_EQ(diagnostic_msgs::msg::DiagnosticStatus::OK, diag_.status[0].level) + << "message: " << diag_.status[0].message; ASSERT_TRUE(hasStatus()); - EXPECT_TRUE(status_->is_cloud_available); - EXPECT_FALSE(status_->has_watchdog_timed_out); - EXPECT_EQ(status_->stuck_started_since, ros::Time(0)); + EXPECT_TRUE(status_.is_cloud_available); + EXPECT_FALSE(status_.has_watchdog_timed_out); + EXPECT_EQ(status_.stuck_started_since, rclcpp::Time(0)); ASSERT_TRUE(received); } @@ -700,7 +785,6 @@ TEST_F(SafetyLimiterTest, SafetyLimitOmniDirectional) int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "test_safety_limiter"); - + rclcpp::init(argc, argv); return RUN_ALL_TESTS(); } diff --git a/safety_limiter/test/src/test_safety_limiter2.cpp b/safety_limiter/test/src/test_safety_limiter2.cpp index 2955fc3e3..30ae1310a 100644 --- a/safety_limiter/test/src/test_safety_limiter2.cpp +++ b/safety_limiter/test/src/test_safety_limiter2.cpp @@ -31,10 +31,10 @@ #include #include -#include +#include "rclcpp/rclcpp.hpp" -#include -#include +#include +#include #include @@ -45,23 +45,22 @@ TEST_F(SafetyLimiterTest, SafetyLimitLinearSimpleSimulationWithMargin) const float dt = 0.02; const double ax = 2.0; // [m/ss] double v = 0.0; // [m/s] - ros::Rate wait(1.0 / dt); + rclcpp::Rate wait(1.0 / dt); const float velocities[] = {-0.8, -0.4, 0.4, 0.8}; for (const float vel : velocities) { float x = 0; bool stopped = false; - const boost::function cb_cmd_vel = - [dt, ax, &x, &v, &stopped](const geometry_msgs::Twist::ConstPtr& msg) -> void + const auto cb_cmd_vel = [dt, ax, &x, &v, &stopped](const geometry_msgs::msg::Twist& msg) -> void { - if (msg->linear.x >= v) + if (msg.linear.x >= v) { - v = std::min(v + ax * dt, msg->linear.x); + v = std::min(v + ax * dt, msg.linear.x); } else { - v = std::max(v - ax * dt, msg->linear.x); + v = std::max(v - ax * dt, msg.linear.x); } x += dt * v; @@ -70,21 +69,21 @@ TEST_F(SafetyLimiterTest, SafetyLimitLinearSimpleSimulationWithMargin) stopped = true; } }; - ros::Subscriber sub_cmd_vel = nh_.subscribe("cmd_vel", 1, cb_cmd_vel); + auto sub_cmd_vel = create_subscription("cmd_vel", 1, cb_cmd_vel); int count_after_stop = 10; - for (float t = 0; t < 10.0 && ros::ok() && count_after_stop > 0; t += dt) + for (float t = 0; t < 10.0 && rclcpp::ok() && count_after_stop > 0; t += dt) { if (vel > 0) - publishSinglePointPointcloud2(1.5 - x, 0, 0, "base_link", ros::Time::now()); + publishSinglePointPointcloud2(1.5 - x, 0, 0, "base_link", now()); else - publishSinglePointPointcloud2(-3.5 - x, 0, 0, "base_link", ros::Time::now()); + publishSinglePointPointcloud2(-3.5 - x, 0, 0, "base_link", now()); publishWatchdogReset(); publishTwist(vel, 0.0); broadcastTF("odom", "base_link", x, 0.0); wait.sleep(); - ros::spinOnce(); + rclcpp::spin_some(get_node_base_interface()); if (stopped) { count_after_stop--; @@ -93,26 +92,20 @@ TEST_F(SafetyLimiterTest, SafetyLimitLinearSimpleSimulationWithMargin) // margin is set to 0.2 if (vel > 0) { - EXPECT_GT(1.4, x) - << "vel: " << vel; // Collision point - margin - EXPECT_LT(1.3, x) - << "vel: " << vel; // Collision point - margin * 2 + EXPECT_GT(1.4, x) << "vel: " << vel; // Collision point - margin + EXPECT_LT(1.3, x) << "vel: " << vel; // Collision point - margin * 2 } else { - EXPECT_LT(-1.4, x) - << "vel: " << vel; // Collision point + margin - EXPECT_GT(-1.3, x) - << "vel: " << vel; // Collision point + margin * 2 + EXPECT_LT(-1.4, x) << "vel: " << vel; // Collision point + margin + EXPECT_GT(-1.3, x) << "vel: " << vel; // Collision point + margin * 2 } - sub_cmd_vel.shutdown(); } } int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "test_safety_limiter"); - + rclcpp::init(argc, argv); return RUN_ALL_TESTS(); } diff --git a/safety_limiter/test/test/test_safety_limiter2_launch.py b/safety_limiter/test/test/test_safety_limiter2_launch.py new file mode 100644 index 000000000..51f8d39c7 --- /dev/null +++ b/safety_limiter/test/test/test_safety_limiter2_launch.py @@ -0,0 +1,61 @@ +import os +import unittest +from typing import Tuple + +import launch_testing.markers +import pytest +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.launch_description_entity import LaunchDescriptionEntity +from launch.substitutions import LaunchConfiguration, TextSubstitution +from launch_ros.actions import Node +from launch_testing.actions import ReadyToTest +from launch_testing.proc_info_handler import ActiveProcInfoHandler + + +@pytest.mark.launch_test +@launch_testing.markers.keep_alive # type: ignore[misc] +def generate_test_description() -> Tuple[LaunchDescription, dict[str, LaunchDescriptionEntity]]: + node_log_level = [ + TextSubstitution(text="safety_limiter:="), + LaunchConfiguration("node_log_level", default="info"), + ] + params_file = os.path.join( + get_package_share_directory("safety_limiter"), # type: ignore[no-untyped-call] + "test", + "configs", + "test_params2.yaml", + ) + safety_limiter_cmd = Node( + package="safety_limiter", + executable="safety_limiter", + name="safety_limiter", + output="screen", + arguments=["--ros-args", "--log-level", node_log_level], + parameters=[params_file], + ) + test_cmd = Node( + package="safety_limiter", + executable="test_safety_limiter2", + name="test_safety_limiter2", + output="screen", + arguments=["--ros-args", "--log-level", "info"], + parameters=[params_file], + ) + + return LaunchDescription( + [ + safety_limiter_cmd, + test_cmd, + ReadyToTest(), # type: ignore[no-untyped-call] + ] + ), {"test_cmd": test_cmd} + + +class TestTerminatingProcessStops(unittest.TestCase): + + def test_proc_terminates(self, proc_info: ActiveProcInfoHandler, test_cmd: Node) -> None: + proc_info.assertWaitForShutdown( # type: ignore[no-untyped-call] + process=test_cmd, timeout=120.0 + ) + self.assertEqual(proc_info[test_cmd].returncode, 0) diff --git a/safety_limiter/test/test/test_safety_limiter_launch.py b/safety_limiter/test/test/test_safety_limiter_launch.py new file mode 100644 index 000000000..d1146edb2 --- /dev/null +++ b/safety_limiter/test/test/test_safety_limiter_launch.py @@ -0,0 +1,61 @@ +import os +import unittest +from typing import Tuple + +import launch_testing.markers +import pytest +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.launch_description_entity import LaunchDescriptionEntity +from launch.substitutions import LaunchConfiguration, TextSubstitution +from launch_ros.actions import Node +from launch_testing.actions import ReadyToTest +from launch_testing.proc_info_handler import ActiveProcInfoHandler + + +@pytest.mark.launch_test +@launch_testing.markers.keep_alive # type: ignore[misc] +def generate_test_description() -> Tuple[LaunchDescription, dict[str, LaunchDescriptionEntity]]: + node_log_level = [ + TextSubstitution(text="safety_limiter:="), + LaunchConfiguration("node_log_level", default="info"), + ] + params_file = os.path.join( + get_package_share_directory("safety_limiter"), # type: ignore[no-untyped-call] + "test", + "configs", + "test_params.yaml", + ) + safety_limiter_cmd = Node( + package="safety_limiter", + executable="safety_limiter", + name="safety_limiter", + output="screen", + arguments=["--ros-args", "--log-level", node_log_level], + parameters=[params_file], + ) + test_cmd = Node( + package="safety_limiter", + executable="test_safety_limiter", + name="test_safety_limiter", + output="screen", + arguments=["--ros-args", "--log-level", "info"], + parameters=[params_file], + ) + + return LaunchDescription( + [ + safety_limiter_cmd, + test_cmd, + ReadyToTest(), # type: ignore[no-untyped-call] + ] + ), {"test_cmd": test_cmd} + + +class TestTerminatingProcessStops(unittest.TestCase): + + def test_proc_terminates(self, proc_info: ActiveProcInfoHandler, test_cmd: Node) -> None: + proc_info.assertWaitForShutdown( # type: ignore[no-untyped-call] + process=test_cmd, timeout=120.0 + ) + self.assertEqual(proc_info[test_cmd].returncode, 0) diff --git a/track_odometry/COLCON_IGNORE b/track_odometry/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/trajectory_tracker/CMakeLists.txt b/trajectory_tracker/CMakeLists.txt index 0a99a8a22..fd142e8d9 100644 --- a/trajectory_tracker/CMakeLists.txt +++ b/trajectory_tracker/CMakeLists.txt @@ -1,79 +1,66 @@ -cmake_minimum_required(VERSION 3.1.3) +cmake_minimum_required(VERSION 3.5) project(trajectory_tracker) -set(CATKIN_DEPENDS - roscpp - - dynamic_reconfigure - geometry_msgs - interactive_markers - nav_msgs - std_srvs - tf2 - tf2_geometry_msgs - tf2_ros - - neonavigation_common - trajectory_tracker_msgs -) - -find_package(catkin REQUIRED COMPONENTS ${CATKIN_DEPENDS}) -find_package(Boost REQUIRED COMPONENTS thread) +find_package(ament_cmake_auto REQUIRED) find_package(Eigen3 REQUIRED) +ament_auto_find_build_dependencies() -generate_dynamic_reconfigure_options( - cfg/TrajectoryTracker.cfg -) - -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS ${CATKIN_DEPENDS} -) - - -set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) -include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) - - -add_executable(trajectory_tracker src/trajectory_tracker.cpp) -target_link_libraries(trajectory_tracker ${catkin_LIBRARIES}) -add_dependencies(trajectory_tracker ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) - -add_executable(trajectory_recorder src/trajectory_recorder.cpp) -target_link_libraries(trajectory_recorder ${catkin_LIBRARIES}) -add_dependencies(trajectory_recorder ${catkin_EXPORTED_TARGETS}) - -add_executable(trajectory_saver src/trajectory_saver.cpp) -target_link_libraries(trajectory_saver ${catkin_LIBRARIES}) -add_dependencies(trajectory_saver ${catkin_EXPORTED_TARGETS}) - -add_executable(trajectory_server src/trajectory_server.cpp) -target_link_libraries(trajectory_server ${catkin_LIBRARIES}) -add_dependencies(trajectory_server ${catkin_EXPORTED_TARGETS}) - - -if(CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - add_subdirectory(test) - - find_package(roslint REQUIRED) - roslint_cpp() - roslint_add_test() +include_directories(include ${Eigen_INCLUDE_DIRS}) +ament_auto_add_executable(trajectory_tracker src/trajectory_tracker.cpp) +# ament_auto_add_executable(trajectory_recorder src/trajectory_recorder.cpp) +# ament_auto_add_executable(trajectory_saver src/trajectory_saver.cpp) +# ament_auto_add_executable(trajectory_server src/trajectory_server.cpp) + +if(BUILD_TESTING) + find_package(GTest REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_uncrustify_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_auto_find_test_dependencies() + ament_lint_auto_find_test_dependencies() + + include_directories(test/include) + + ament_auto_add_executable(time_source test/src/time_source.cpp) + ament_auto_add_executable(test_trajectory_tracker test/src/test_trajectory_tracker.cpp) + target_link_libraries(test_trajectory_tracker ${GTEST_LIBRARIES}) + ament_auto_add_executable(test_trajectory_tracker_with_odom test/src/test_trajectory_tracker_with_odom.cpp) + target_link_libraries(test_trajectory_tracker_with_odom ${GTEST_LIBRARIES}) + ament_auto_add_executable(test_trajectory_tracker_overshoot test/src/test_trajectory_tracker_overshoot.cpp) + target_link_libraries(test_trajectory_tracker_overshoot ${GTEST_LIBRARIES}) + + install(DIRECTORY + test/configs + DESTINATION share/${PROJECT_NAME}/test/ + ) + add_launch_test(test/test/test_trajectory_tracker_launch.py + TARGET test_trajectory_tracker + ARGS use_sim_time:=true + TIMEOUT 120 + ) + add_launch_test(test/test/test_trajectory_tracker_launch.py + TARGET test_trajectory_tracker_with_odom_delay + ARGS use_sim_time:=true odom_delay:=0.04 use_odom:=true use_time_optimal_control:=true + TIMEOUT 120 + ) + add_launch_test(test/test/test_trajectory_tracker_launch.py + TARGET test_trajectory_tracker_without_time_optimal_control + ARGS use_sim_time:=true odom_delay:=0.04 use_odom:=true use_time_optimal_control:=false + TIMEOUT 120 + ) + add_launch_test(test/test/test_trajectory_tracker_overshoot_launch.py + TARGET test_trajectory_tracker_overshoot + ARGS test_executable:=test_trajectory_tracker_overshoot + TIMEOUT 60 + ) + add_launch_test(test/test/test_trajectory_tracker_overshoot_launch.py + TARGET test_trajectory_tracker_with_odom + ARGS test_executable:=test_trajectory_tracker_with_odom + TIMEOUT 60 + ) endif() - -install(TARGETS - trajectory_recorder - trajectory_saver - trajectory_server - trajectory_tracker - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) +ament_auto_package() diff --git a/trajectory_tracker/include/trajectory_tracker/basic_control.h b/trajectory_tracker/include/trajectory_tracker/basic_control.h index d4d778b75..82992ab94 100644 --- a/trajectory_tracker/include/trajectory_tracker/basic_control.h +++ b/trajectory_tracker/include/trajectory_tracker/basic_control.h @@ -69,13 +69,11 @@ class VelAccLimitter : val_prev_(0) { } - inline float increment( - const float v, const float vel, const float acc, const float dt) + inline float increment(const float v, const float vel, const float acc, const float dt) { return set(val_prev_ + v, vel, acc, dt); } - inline float set( - float v, const float vel, const float acc, const float dt) + inline float set(float v, const float vel, const float acc, const float dt) { v = clip(v, vel); diff --git a/trajectory_tracker/include/trajectory_tracker/eigen_line.h b/trajectory_tracker/include/trajectory_tracker/eigen_line.h index eca6446bd..0ef4e4f31 100644 --- a/trajectory_tracker/include/trajectory_tracker/eigen_line.h +++ b/trajectory_tracker/include/trajectory_tracker/eigen_line.h @@ -37,14 +37,10 @@ namespace trajectory_tracker { -inline float curv3p( - const Eigen::Vector2d& a, - const Eigen::Vector2d& b, - const Eigen::Vector2d& c) +inline float curv3p(const Eigen::Vector2d& a, const Eigen::Vector2d& b, const Eigen::Vector2d& c) { float ret; - ret = 2 * (a[0] * b[1] + b[0] * c[1] + c[0] * a[1] - - a[0] * c[1] - b[0] * a[1] - c[0] * b[1]); + ret = 2 * (a[0] * b[1] + b[0] * c[1] + c[0] * a[1] - a[0] * c[1] - b[0] * a[1] - c[0] * b[1]); ret /= std::sqrt((b - a).squaredNorm() * (b - c).squaredNorm() * (c - a).squaredNorm()); return ret; @@ -55,10 +51,7 @@ inline float cross2(const Eigen::Vector2d& a, const Eigen::Vector2d& b) return a[0] * b[1] - a[1] * b[0]; } -inline float lineDistance( - const Eigen::Vector2d& a, - const Eigen::Vector2d& b, - const Eigen::Vector2d& c) +inline float lineDistance(const Eigen::Vector2d& a, const Eigen::Vector2d& b, const Eigen::Vector2d& c) { return cross2((b - a), (c - a)) / (b - a).norm(); } @@ -66,10 +59,7 @@ inline float lineDistance( // lineStripDistanceSigned returns signed distance from the line strip. // If c is behind a, negative value will be returned. [ (c) (a)---(b) ] // Otherwise, positive value will be returned. [ (a)---(b) (c) ] [ (a)--(c)--(b) ] -inline float lineStripDistanceSigned( - const Eigen::Vector2d& a, - const Eigen::Vector2d& b, - const Eigen::Vector2d& c) +inline float lineStripDistanceSigned(const Eigen::Vector2d& a, const Eigen::Vector2d& b, const Eigen::Vector2d& c) { if ((b - a).dot(c - a) <= 0) return -(c - a).norm(); @@ -78,18 +68,12 @@ inline float lineStripDistanceSigned( return std::abs(lineDistance(a, b, c)); } -inline float lineStripDistance( - const Eigen::Vector2d& a, - const Eigen::Vector2d& b, - const Eigen::Vector2d& c) +inline float lineStripDistance(const Eigen::Vector2d& a, const Eigen::Vector2d& b, const Eigen::Vector2d& c) { return std::abs(lineStripDistanceSigned(a, b, c)); } -inline Eigen::Vector2d projection2d( - const Eigen::Vector2d& a, - const Eigen::Vector2d& b, - const Eigen::Vector2d& c) +inline Eigen::Vector2d projection2d(const Eigen::Vector2d& a, const Eigen::Vector2d& b, const Eigen::Vector2d& c) { const float r = (b - a).dot(c - a) / (b - a).squaredNorm(); return b * r + a * (1.0 - r); diff --git a/trajectory_tracker/include/trajectory_tracker/path2d.h b/trajectory_tracker/include/trajectory_tracker/path2d.h index 298a4a147..b73f2eb1c 100644 --- a/trajectory_tracker/include/trajectory_tracker/path2d.h +++ b/trajectory_tracker/include/trajectory_tracker/path2d.h @@ -37,15 +37,15 @@ #include #include -#include -#include +#include +#include #include -#include +#include #include #include -#include +#include "rclcpp/rclcpp.hpp" namespace trajectory_tracker { @@ -68,19 +68,19 @@ class Pose2D , velocity_(velocity) { } - inline Pose2D(const geometry_msgs::Pose& pose, float velocity) + inline Pose2D(const geometry_msgs::msg::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) + inline explicit Pose2D(const geometry_msgs::msg::PoseStamped& pose) : pos_(Eigen::Vector2d(pose.pose.position.x, pose.pose.position.y)) , yaw_(tf2::getYaw(pose.pose.orientation)) , velocity_(std::numeric_limits::quiet_NaN()) { } - inline explicit Pose2D(const trajectory_tracker_msgs::PoseStampedWithVelocity& pose) + inline explicit Pose2D(const trajectory_tracker_msgs::msg::PoseStampedWithVelocity& pose) : pos_(Eigen::Vector2d(pose.pose.position.x, pose.pose.position.y)) , yaw_(tf2::getYaw(pose.pose.orientation)) , velocity_(pose.linear_velocity.x) @@ -101,13 +101,13 @@ class Pose2D while (yaw_ > 2 * M_PI) yaw_ -= 2 * M_PI; } - void toMsg(geometry_msgs::PoseStamped& pose) const + void toMsg(geometry_msgs::msg::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 + void toMsg(trajectory_tracker_msgs::msg::PoseStampedWithVelocity& pose) const { pose.pose.position.x = pos_.x(); pose.pose.position.y = pos_.y(); @@ -131,12 +131,8 @@ class Path2D : public std::vector l += ((*this)[i - 1].pos_ - (*this)[i].pos_).norm(); return l; } - inline ConstIterator findLocalGoal( - const ConstIterator& begin, - const ConstIterator& end, - const bool allow_switch_back, - const bool allow_in_place_turn = true, - const double epsilon = 1e-6) const + inline ConstIterator findLocalGoal(const ConstIterator& begin, const ConstIterator& end, const bool allow_switch_back, + const bool allow_in_place_turn = true, const double epsilon = 1e-6) const { float sign_vel_prev = 0; ConstIterator it_prev = begin; @@ -168,21 +164,15 @@ class Path2D : public std::vector } return end; } - inline ConstIterator findNearest( - const ConstIterator& begin, - const ConstIterator& end, - const Eigen::Vector2d& target, - const float max_search_range = 0, - const float epsilon = 1e-6) const + inline ConstIterator findNearest(const ConstIterator& begin, const ConstIterator& end, 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 findNearestWithDistance( - const ConstIterator& begin, - const ConstIterator& end, - const Eigen::Vector2d& target, - const float max_search_range = 0, - const float epsilon = 1e-6) const + inline std::pair 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) { @@ -204,8 +194,7 @@ class Path2D : public std::vector if (max_search_range > 0 && distance_path_search > max_search_range) break; - const float d = - trajectory_tracker::lineStripDistanceSigned(it_prev->pos_, it->pos_, target); + const float d = trajectory_tracker::lineStripDistanceSigned(it_prev->pos_, it->pos_, target); // Use earlier point if the robot is same distance from two line strip // to avoid chattering. @@ -223,11 +212,8 @@ class Path2D : public std::vector } return std::make_pair(it_nearest, min_dist); } - inline double remainedDistance( - const ConstIterator& begin, - const ConstIterator& nearest, - const ConstIterator& end, - const Eigen::Vector2d& target_on_line) const + inline double remainedDistance(const ConstIterator& begin, const ConstIterator& nearest, const ConstIterator& end, + const Eigen::Vector2d& target_on_line) const { double remain = (nearest->pos_ - target_on_line).norm(); if (nearest + 1 >= end) @@ -257,11 +243,8 @@ class Path2D : public std::vector } return remain; } - inline float getCurvature( - const ConstIterator& begin, - const ConstIterator& end, - const Eigen::Vector2d& target_on_line, - const float max_search_range) const + inline float getCurvature(const ConstIterator& begin, const ConstIterator& end, const Eigen::Vector2d& target_on_line, + const float max_search_range) const { if (end - begin <= 1) { @@ -282,9 +265,8 @@ class Path2D : public std::vector } const float cos_v = std::cos(rel.yaw_); const float r1 = rel.pos_.y() + rel.pos_.x() * cos_v / sin_v; - const float r2 = std::copysign( - std::sqrt(std::pow(rel.pos_.x(), 2) + std::pow(rel.pos_.x() * cos_v / sin_v, 2)), - rel.pos_.x() * sin_v); + const float r2 = std::copysign(std::sqrt(std::pow(rel.pos_.x(), 2) + std::pow(rel.pos_.x() * cos_v / sin_v, 2)), + rel.pos_.x() * sin_v); return 1.0f / ((r1 + r2) / 2); } const float max_search_range_sq = max_search_range * max_search_range; @@ -301,7 +283,7 @@ class Path2D : public std::vector } return curv; } - // PATH_TYPE should be trajectory_tracker_msgs::PathWithVelocity or nav_msgs::Path + // PATH_TYPE should be trajectory_tracker_msgs::msg::PathWithVelocity or nav_msgs::msg::Path template inline void fromMsg(const PATH_TYPE& path, const double in_place_turn_eps = 1.0e-6) { @@ -327,8 +309,7 @@ class Path2D : public std::vector } else { - in_place_turn_end = trajectory_tracker::Pose2D( - back().pos_, next.yaw_, next.velocity_); + in_place_turn_end = trajectory_tracker::Pose2D(back().pos_, next.yaw_, next.velocity_); in_place_turning = true; } } @@ -337,7 +318,7 @@ class Path2D : public std::vector push_back(in_place_turn_end); } } - // PATH_TYPE should be trajectory_tracker_msgs::PathWithVelocity or nav_msgs::Path + // PATH_TYPE should be trajectory_tracker_msgs::msg::PathWithVelocity or nav_msgs::msg::Path template inline void toMsg(PATH_TYPE& path) const { @@ -350,12 +331,10 @@ class Path2D : public std::vector } } - inline std::vector enumerateLocalGoals( - const ConstIterator& begin, - const ConstIterator& end, - const bool allow_switch_back, - const bool allow_in_place_turn = true, - const double epsilon = 1e-6) const + inline std::vector enumerateLocalGoals(const ConstIterator& begin, const ConstIterator& end, + const bool allow_switch_back, + const bool allow_in_place_turn = true, + const double epsilon = 1e-6) const { ConstIterator it_search_begin = begin; std::vector results; @@ -371,12 +350,9 @@ class Path2D : public std::vector return results; } - inline std::vector getEstimatedTimeOfArrivals( - const ConstIterator& begin, - const ConstIterator& end, - const double linear_speed, - const double angular_speed, - const double initial_eta_sec = 0.0) const + inline std::vector getEstimatedTimeOfArrivals(const ConstIterator& begin, const ConstIterator& end, + const double linear_speed, const double angular_speed, + const double initial_eta_sec = 0.0) const { if (begin == end) { diff --git a/trajectory_tracker/package.xml b/trajectory_tracker/package.xml index 4dee16d1b..3b53c0af4 100644 --- a/trajectory_tracker/package.xml +++ b/trajectory_tracker/package.xml @@ -8,13 +8,9 @@ BSD Atsushi Watanabe - catkin + ament_cmake + ament_cmake_auto - roscpp - roslint - rostest - - dynamic_reconfigure geometry_msgs interactive_markers nav_msgs @@ -22,9 +18,18 @@ tf2 tf2_geometry_msgs tf2_ros - + rclcpp eigen - neonavigation_common - trajectory_tracker_msgs + trajectory_tracker_msgs + + ament_lint_auto + ament_lint_common + launch_testing_ament_cmake + ament_cmake_gtest + nav2_common + + + ament_cmake + diff --git a/trajectory_tracker/src/trajectory_tracker.cpp b/trajectory_tracker/src/trajectory_tracker.cpp index b7df6d834..c9ca7b212 100644 --- a/trajectory_tracker/src/trajectory_tracker.cpp +++ b/trajectory_tracker/src/trajectory_tracker.cpp @@ -44,42 +44,42 @@ #include #include #include +#include #include #include #include -#include +#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include -#include +#include +#include #include +#include -#include -#include -#include +#include +#include +#include -#include -#include #include #include +#include namespace trajectory_tracker { -class TrackerNode +class TrackerNode : public neonavigation_common::NeonavigationNode { public: - TrackerNode(); + explicit TrackerNode(const std::string& name); ~TrackerNode(); - void spin(); private: std::string topic_path_; @@ -96,6 +96,8 @@ class TrackerNode double vel_[2]; double acc_[2]; double acc_toc_[2]; + double acc_toc_factor_; + double angacc_toc_factor_; trajectory_tracker::VelAccLimitter v_lim_; trajectory_tracker::VelAccLimitter w_lim_; double rotate_ang_; @@ -105,8 +107,8 @@ class TrackerNode double stop_tolerance_ang_; double no_pos_cntl_dist_; double min_track_path_; - int path_step_; - int path_step_done_; + int64_t path_step_; + int64_t path_step_done_; bool allow_backward_; bool limit_vel_by_avel_; bool check_old_path_; @@ -119,30 +121,26 @@ class TrackerNode double goal_tolerance_lin_vel_; double goal_tolerance_ang_vel_; - ros::Subscriber sub_path_; - ros::Subscriber sub_path_velocity_; - ros::Subscriber sub_vel_; - ros::Subscriber sub_odom_; - ros::Publisher pub_vel_; - ros::Publisher pub_status_; - ros::Publisher pub_tracking_; - ros::NodeHandle nh_; - ros::NodeHandle pnh_; + rclcpp::SubscriptionBase::SharedPtr sub_path_; + rclcpp::SubscriptionBase::SharedPtr sub_path_velocity_; + rclcpp::SubscriptionBase::SharedPtr sub_speed_; + rclcpp::SubscriptionBase::SharedPtr sub_odom_; + rclcpp::Publisher::SharedPtr pub_vel_; + rclcpp::Publisher::SharedPtr pub_status_; + rclcpp::Publisher::SharedPtr pub_tracking_; tf2_ros::Buffer tfbuf_; tf2_ros::TransformListener tfl_; - ros::Timer odom_timeout_timer_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::TimerBase::SharedPtr odom_timeout_timer_; double odom_timeout_sec_; trajectory_tracker::Path2D path_; - std_msgs::Header path_header_; + std_msgs::msg::Header path_header_; bool is_path_updated_; - mutable boost::recursive_mutex parameter_server_mutex_; - dynamic_reconfigure::Server parameter_server_; - bool use_odom_; bool predict_odom_; - ros::Time prev_odom_stamp_; + rclcpp::Time prev_odom_stamp_; struct TrackingResult { @@ -163,7 +161,7 @@ class TrackerNode { } - int status; // same as trajectory_tracker_msgs::TrajectoryTrackerStatus::status + int status; // same as trajectory_tracker_msgs::msg::TrajectoryTrackerStatus::status double distance_remains; double angle_remains; double distance_remains_raw; // remained distance without prediction @@ -178,170 +176,164 @@ class TrackerNode int path_step_done; }; + void onDynamicParameterUpdated(const std::vector&) final; + template - void cbPath(const typename MSG_TYPE::ConstPtr&); - void cbSpeed(const std_msgs::Float32::ConstPtr&); - void cbOdometry(const nav_msgs::Odometry::ConstPtr&); - void cbTimer(const ros::TimerEvent&); - void cbOdomTimeout(const ros::TimerEvent&); + void cbPath(const MSG_TYPE&); + void cbSpeed(const std_msgs::msg::Float32&); + void cbOdometry(const nav_msgs::msg::Odometry&); + + void cbTimer(); + void cbOdomTimeout(); void control(const tf2::Stamped&, const Eigen::Vector3d&, const double, const double, const double); - TrackingResult getTrackingResult( - const tf2::Stamped&, const Eigen::Vector3d&, const double, const double) const; - void cbParameter(const TrajectoryTrackerConfig& config, const uint32_t /* level */); + TrackingResult getTrackingResult(const tf2::Stamped&, const Eigen::Vector3d&, const double, + const double) const; }; -TrackerNode::TrackerNode() - : nh_() - , pnh_("~") +TrackerNode::TrackerNode(const std::string& name = "trajectory_tracker") + : neonavigation_common::NeonavigationNode(name, rclcpp::NodeOptions()) + , tfbuf_(get_clock()) , tfl_(tfbuf_) , is_path_updated_(false) { - neonavigation_common::compat::checkCompatMode(); - pnh_.param("frame_robot", frame_robot_, std::string("base_link")); - pnh_.param("frame_odom", frame_odom_, std::string("odom")); - neonavigation_common::compat::deprecatedParam(pnh_, "path", topic_path_, std::string("path")); - neonavigation_common::compat::deprecatedParam(pnh_, "cmd_vel", topic_cmd_vel_, std::string("cmd_vel")); - pnh_.param("hz", hz_, 50.0); - pnh_.param("use_odom", use_odom_, false); - pnh_.param("predict_odom", predict_odom_, true); - pnh_.param("max_dt", max_dt_, 0.1); - pnh_.param("odom_timeout_sec", odom_timeout_sec_, 0.1); - - sub_path_ = neonavigation_common::compat::subscribe( - nh_, "path", - pnh_, topic_path_, 2, - boost::bind(&TrackerNode::cbPath, this, _1)); - sub_path_velocity_ = nh_.subscribe( + declare_dynamic_parameter("look_forward", &look_forward_, 0.5); + declare_dynamic_parameter("curv_forward", &curv_forward_, 0.5); + declare_dynamic_parameter("k_dist", &k_[0], 1.0); + declare_dynamic_parameter("k_ang", &k_[1], 1.0); + declare_dynamic_parameter("k_avel", &k_[2], 1.0); + declare_dynamic_parameter("gain_at_vel", &gain_at_vel_, 0.0); + declare_dynamic_parameter("dist_lim", &d_lim_, 0.5); + declare_dynamic_parameter("dist_stop", &d_stop_, 2.0); + declare_dynamic_parameter("rotate_ang", &rotate_ang_, 0.78539816339); + declare_dynamic_parameter("max_vel", &vel_[0], 0.5); + declare_dynamic_parameter("max_angvel", &vel_[1], 1.0); + declare_dynamic_parameter("max_acc", &acc_[0], 1.0); + declare_dynamic_parameter("max_angacc", &acc_[1], 2.0); + declare_dynamic_parameter("acc_toc_factor", &acc_toc_factor_, 0.9); + declare_dynamic_parameter("angacc_toc_factor", &angacc_toc_factor_, 0.9); + declare_dynamic_parameter("path_step", &path_step_, 1l); + declare_dynamic_parameter("goal_tolerance_dist", &goal_tolerance_dist_, 0.2); + declare_dynamic_parameter("goal_tolerance_ang", &goal_tolerance_ang_, 0.1); + declare_dynamic_parameter("stop_tolerance_dist", &stop_tolerance_dist_, 0.1); + declare_dynamic_parameter("stop_tolerance_ang", &stop_tolerance_ang_, 0.05); + declare_dynamic_parameter("no_position_control_dist", &no_pos_cntl_dist_, 0.0); + declare_dynamic_parameter("min_tracking_path", &min_track_path_, 0.0); + declare_dynamic_parameter("allow_backward", &allow_backward_, true); + declare_dynamic_parameter("limit_vel_by_avel", &limit_vel_by_avel_, false); + declare_dynamic_parameter("check_old_path", &check_old_path_, false); + declare_dynamic_parameter("epsilon", &epsilon_, 0.001); + declare_dynamic_parameter("use_time_optimal_control", &use_time_optimal_control_, true); + declare_dynamic_parameter("time_optimal_control_future_gain", &time_optimal_control_future_gain_, 1.5); + declare_dynamic_parameter("k_ang_rotation", &k_ang_rotation_, 1.0); + declare_dynamic_parameter("k_avel_rotation", &k_avel_rotation_, 1.0); + declare_dynamic_parameter("goal_tolerance_lin_vel", &goal_tolerance_lin_vel_, 0.0); + declare_dynamic_parameter("goal_tolerance_ang_vel", &goal_tolerance_ang_vel_, 0.0); + declare_dynamic_parameter("frame_robot", &frame_robot_, std::string("base_link")); + declare_dynamic_parameter("frame_odom", &frame_odom_, std::string("odom")); + declare_dynamic_parameter("hz", &hz_, 50.0); + declare_dynamic_parameter("use_odom", &use_odom_, false); + declare_dynamic_parameter("predict_odom", &predict_odom_, true); + declare_dynamic_parameter("max_dt", &max_dt_, 0.1); + declare_dynamic_parameter("odom_timeout_sec", &odom_timeout_sec_, 0.1); + onDynamicParameterUpdated({}); + + sub_path_ = create_subscription( + "path", 2, std::bind(&TrackerNode::cbPath, this, std::placeholders::_1)); + sub_path_velocity_ = create_subscription( "path_velocity", 2, - boost::bind(&TrackerNode::cbPath, this, _1)); - sub_vel_ = neonavigation_common::compat::subscribe( - nh_, "speed", - pnh_, "speed", 20, &TrackerNode::cbSpeed, this); - pub_vel_ = neonavigation_common::compat::advertise( - nh_, "cmd_vel", - pnh_, topic_cmd_vel_, 10); - pub_status_ = pnh_.advertise("status", 10, true); - pub_tracking_ = pnh_.advertise("tracking", 10, true); + std::bind(&TrackerNode::cbPath, this, std::placeholders::_1)); + sub_speed_ = create_subscription( + "speed", 20, std::bind(&TrackerNode::cbSpeed, this, std::placeholders::_1)); + + pub_vel_ = create_publisher("cmd_vel", 10); + pub_status_ = create_publisher("~/status", 10); + pub_tracking_ = create_publisher("~/tracking", 10); if (use_odom_) { - sub_odom_ = nh_.subscribe("odom", 10, &TrackerNode::cbOdometry, this, - ros::TransportHints().reliable().tcpNoDelay(true)); + sub_odom_ = create_subscription( + "odom", 10, std::bind(&TrackerNode::cbOdometry, this, std::placeholders::_1)); + } + else + { + timer_ = rclcpp::create_timer(this, get_clock(), std::chrono::duration(1.0 / hz_), + std::bind(&TrackerNode::cbTimer, this)); } - - boost::recursive_mutex::scoped_lock lock(parameter_server_mutex_); - parameter_server_.setCallback(boost::bind(&TrackerNode::cbParameter, this, _1, _2)); -} - -void TrackerNode::cbParameter(const TrajectoryTrackerConfig& config, const uint32_t /* level */) -{ - boost::recursive_mutex::scoped_lock lock(parameter_server_mutex_); - look_forward_ = config.look_forward; - curv_forward_ = config.curv_forward; - k_[0] = config.k_dist; - k_[1] = config.k_ang; - k_[2] = config.k_avel; - gain_at_vel_ = config.gain_at_vel; - d_lim_ = config.dist_lim; - d_stop_ = config.dist_stop; - rotate_ang_ = config.rotate_ang; - vel_[0] = config.max_vel; - vel_[1] = config.max_angvel; - acc_[0] = config.max_acc; - acc_[1] = config.max_angacc; - acc_toc_[0] = acc_[0] * config.acc_toc_factor; - acc_toc_[1] = acc_[1] * config.angacc_toc_factor; - path_step_ = config.path_step; - goal_tolerance_dist_ = config.goal_tolerance_dist; - goal_tolerance_ang_ = config.goal_tolerance_ang; - stop_tolerance_dist_ = config.stop_tolerance_dist; - stop_tolerance_ang_ = config.stop_tolerance_ang; - no_pos_cntl_dist_ = config.no_position_control_dist; - min_track_path_ = config.min_tracking_path; - allow_backward_ = config.allow_backward; - limit_vel_by_avel_ = config.limit_vel_by_avel; - check_old_path_ = config.check_old_path; - epsilon_ = config.epsilon; - use_time_optimal_control_ = config.use_time_optimal_control; - 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() { - geometry_msgs::Twist cmd_vel; + geometry_msgs::msg::Twist cmd_vel; cmd_vel.linear.x = 0; cmd_vel.angular.z = 0; - pub_vel_.publish(cmd_vel); + pub_vel_->publish(cmd_vel); } -void TrackerNode::cbSpeed(const std_msgs::Float32::ConstPtr& msg) +void TrackerNode::onDynamicParameterUpdated(const std::vector&) { - vel_[0] = msg->data; + acc_toc_[0] = acc_[0] * acc_toc_factor_; + acc_toc_[1] = acc_[1] * angacc_toc_factor_; +} + +void TrackerNode::cbSpeed(const std_msgs::msg::Float32& msg) +{ + vel_[0] = msg.data; } template -void TrackerNode::cbPath(const typename MSG_TYPE::ConstPtr& msg) +void TrackerNode::cbPath(const MSG_TYPE& msg) { - path_header_ = msg->header; + path_header_ = msg.header; is_path_updated_ = true; path_step_done_ = 0; - path_.fromMsg(*msg, epsilon_); + path_.fromMsg(msg, epsilon_); for (const auto& path_pose : path_) { if (std::isfinite(path_pose.velocity_) && path_pose.velocity_ < -0.0) { - ROS_ERROR_THROTTLE(1.0, "path_velocity.velocity.x must be positive"); + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 1000, "path_velocity.velocity.x must be positive"); path_.clear(); return; } } } -void TrackerNode::cbOdometry(const nav_msgs::Odometry::ConstPtr& odom) +void TrackerNode::cbOdometry(const nav_msgs::msg::Odometry& odom) { - if (odom->header.frame_id != frame_odom_) + if (odom.header.frame_id != frame_odom_) { - ROS_WARN("frame_odom is invalid. Update from \"%s\" to \"%s\"", frame_odom_.c_str(), odom->header.frame_id.c_str()); - frame_odom_ = odom->header.frame_id; + RCLCPP_WARN(get_logger(), "frame_odom is invalid. Update from \"%s\" to \"%s\"", frame_odom_.c_str(), + odom.header.frame_id.c_str()); + frame_odom_ = odom.header.frame_id; } - if (odom->child_frame_id != frame_robot_) + if (odom.child_frame_id != frame_robot_) { - ROS_WARN("frame_robot is invalid. Update from \"%s\" to \"%s\"", - frame_robot_.c_str(), odom->child_frame_id.c_str()); - frame_robot_ = odom->child_frame_id; + RCLCPP_WARN(get_logger(), "frame_robot is invalid. Update from \"%s\" to \"%s\"", frame_robot_.c_str(), + odom.child_frame_id.c_str()); + frame_robot_ = odom.child_frame_id; } if (odom_timeout_sec_ != 0.0) { - if (odom_timeout_timer_.isValid()) - { - odom_timeout_timer_.setPeriod(ros::Duration(odom_timeout_sec_), true); - } - else - { - odom_timeout_timer_ = - nh_.createTimer(ros::Duration(odom_timeout_sec_), &TrackerNode::cbOdomTimeout, this, true, true); - } + odom_timeout_timer_ = rclcpp::create_timer(this, get_clock(), std::chrono::duration(odom_timeout_sec_), + std::bind(&TrackerNode::cbOdomTimeout, this)); } - if (prev_odom_stamp_ != ros::Time()) + if (prev_odom_stamp_.nanoseconds() != 0L) { - const double dt = std::min(max_dt_, (odom->header.stamp - prev_odom_stamp_).toSec()); - nav_msgs::Odometry odom_compensated = *odom; + const rclcpp::Time odom_stamp(odom.header.stamp); + const double dt = std::min(max_dt_, (odom_stamp - prev_odom_stamp_).seconds()); + nav_msgs::msg::Odometry odom_compensated = odom; Eigen::Vector3d prediction_offset(0, 0, 0); if (predict_odom_) { - const double predict_dt = std::max(0.0, std::min(max_dt_, (ros::Time::now() - odom->header.stamp).toSec())); + const double predict_dt = std::max(0.0, std::min(max_dt_, (now() - odom_stamp).seconds())); tf2::Transform trans; - const tf2::Quaternion rotation(tf2::Vector3(0, 0, 1), odom->twist.twist.angular.z * predict_dt); - const tf2::Vector3 translation(odom->twist.twist.linear.x * predict_dt, 0, 0); + const tf2::Quaternion rotation(tf2::Vector3(0, 0, 1), odom.twist.twist.angular.z * predict_dt); + const tf2::Vector3 translation(odom.twist.twist.linear.x * predict_dt, 0, 0); - prediction_offset[0] = odom->twist.twist.linear.x * predict_dt; - prediction_offset[2] = odom->twist.twist.angular.z * predict_dt; + prediction_offset[0] = odom.twist.twist.linear.x * predict_dt; + prediction_offset[2] = odom.twist.twist.angular.z * predict_dt; - tf2::fromMsg(odom->pose.pose, trans); + tf2::fromMsg(odom.pose.pose, trans); trans.setOrigin(trans.getOrigin() + tf2::Transform(trans.getRotation()) * translation); trans.setRotation(trans.getRotation() * rotation); tf2::toMsg(trans, odom_compensated.pose.pose); @@ -349,76 +341,63 @@ void TrackerNode::cbOdometry(const nav_msgs::Odometry::ConstPtr& odom) tf2::Transform odom_to_robot; tf2::fromMsg(odom_compensated.pose.pose, odom_to_robot); - const tf2::Stamped robot_to_odom( - odom_to_robot.inverse(), - odom->header.stamp, odom->header.frame_id); + const tf2::Stamped robot_to_odom(odom_to_robot.inverse(), tf2_ros::fromMsg(odom_stamp), + odom.header.frame_id); - control(robot_to_odom, prediction_offset, odom->twist.twist.linear.x, odom->twist.twist.angular.z, dt); + control(robot_to_odom, prediction_offset, odom.twist.twist.linear.x, odom.twist.twist.angular.z, dt); } - prev_odom_stamp_ = odom->header.stamp; + prev_odom_stamp_ = odom.header.stamp; } -void TrackerNode::cbTimer(const ros::TimerEvent& event) +void TrackerNode::cbTimer() { try { tf2::Stamped transform; - tf2::fromMsg( - tfbuf_.lookupTransform(frame_robot_, frame_odom_, ros::Time(0)), transform); + tf2::fromMsg(tfbuf_.lookupTransform(frame_robot_, frame_odom_, rclcpp::Time(0)), transform); control(transform, Eigen::Vector3d(0, 0, 0), 0, 0, 1.0 / hz_); } catch (tf2::TransformException& e) { - ROS_WARN_THROTTLE(1, "TF exception: %s", e.what()); - trajectory_tracker_msgs::TrajectoryTrackerStatus status; - status.header.stamp = ros::Time::now(); + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "TF exception: %s", e.what()); + trajectory_tracker_msgs::msg::TrajectoryTrackerStatus status; + status.header.stamp = now(); status.distance_remains = 0.0; status.angle_remains = 0.0; status.path_header = path_header_; - status.status = trajectory_tracker_msgs::TrajectoryTrackerStatus::NO_PATH; - pub_status_.publish(status); + status.status = trajectory_tracker_msgs::msg::TrajectoryTrackerStatus::NO_PATH; + pub_status_->publish(status); return; } } -void TrackerNode::cbOdomTimeout(const ros::TimerEvent& event) +void TrackerNode::cbOdomTimeout() { - ROS_WARN_STREAM("Odometry timeout. Last odometry stamp: " << prev_odom_stamp_); + RCLCPP_WARN_STREAM(get_logger(), "Odometry timeout. Last odometry stamp: " << prev_odom_stamp_.nanoseconds()); v_lim_.clear(); w_lim_.clear(); - geometry_msgs::Twist cmd_vel; + geometry_msgs::msg::Twist cmd_vel; cmd_vel.linear.x = 0.0; cmd_vel.angular.z = 0.0; - pub_vel_.publish(cmd_vel); + pub_vel_->publish(cmd_vel); - trajectory_tracker_msgs::TrajectoryTrackerStatus status; - status.header.stamp = ros::Time::now(); + trajectory_tracker_msgs::msg::TrajectoryTrackerStatus status; + status.header.stamp = now(); status.distance_remains = 0.0; status.angle_remains = 0.0; status.path_header = path_header_; - status.status = trajectory_tracker_msgs::TrajectoryTrackerStatus::NO_PATH; - pub_status_.publish(status); -} + status.status = trajectory_tracker_msgs::msg::TrajectoryTrackerStatus::NO_PATH; + pub_status_->publish(status); -void TrackerNode::spin() -{ - ros::Timer timer; - if (!use_odom_) - { - timer = nh_.createTimer(ros::Duration(1.0 / hz_), &TrackerNode::cbTimer, this); - } - ros::spin(); + // One shot timer + odom_timeout_timer_.reset(); } -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) +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; - status.header.stamp = ros::Time::now(); + trajectory_tracker_msgs::msg::TrajectoryTrackerStatus status; + status.header.stamp = now(); status.path_header = path_header_; if (is_path_updated_) { @@ -432,15 +411,15 @@ void TrackerNode::control( getTrackingResult(robot_to_odom, prediction_offset, odom_linear_vel, odom_angular_vel); switch (tracking_result.status) { - case trajectory_tracker_msgs::TrajectoryTrackerStatus::NO_PATH: - case trajectory_tracker_msgs::TrajectoryTrackerStatus::FAR_FROM_PATH: + case trajectory_tracker_msgs::msg::TrajectoryTrackerStatus::NO_PATH: + case trajectory_tracker_msgs::msg::TrajectoryTrackerStatus::FAR_FROM_PATH: { v_lim_.clear(); w_lim_.clear(); - geometry_msgs::Twist cmd_vel; + geometry_msgs::msg::Twist cmd_vel; cmd_vel.linear.x = 0; cmd_vel.angular.z = 0; - pub_vel_.publish(cmd_vel); + pub_vel_->publish(cmd_vel); break; } default: @@ -461,23 +440,20 @@ void TrackerNode::control( (-tracking_result.angle_remains * k_ang_rotation_ - w_lim_.get() * k_avel_rotation_) * dt; w_lim_.increment(wvel_increment, vel_[1], acc_[1], dt); } - ROS_DEBUG( - "trajectory_tracker: angular residual %0.3f, angular vel %0.3f", - tracking_result.angle_remains, w_lim_.get()); + RCLCPP_DEBUG(get_logger(), "angular residual %0.3f, angular vel %0.3f", tracking_result.angle_remains, + w_lim_.get()); } else { - v_lim_.set( - trajectory_tracker::timeOptimalControl(tracking_result.signed_local_distance, acc_toc_[0]), - tracking_result.target_linear_vel, acc_[0], dt); + v_lim_.set(trajectory_tracker::timeOptimalControl(tracking_result.signed_local_distance, acc_toc_[0]), + tracking_result.target_linear_vel, acc_[0], dt); float wref = std::abs(v_lim_.get()) * tracking_result.tracking_point_curv; if (limit_vel_by_avel_ && std::abs(wref) > vel_[1]) { - v_lim_.set( - std::copysign(1.0, v_lim_.get()) * std::abs(vel_[1] / tracking_result.tracking_point_curv), - tracking_result.target_linear_vel, acc_[0], dt); + v_lim_.set(std::copysign(1.0, v_lim_.get()) * std::abs(vel_[1] / tracking_result.tracking_point_curv), + tracking_result.target_linear_vel, acc_[0], dt); wref = std::copysign(1.0, wref) * vel_[1]; } @@ -488,10 +464,11 @@ void TrackerNode::control( const double wvel_diff = w_lim_.get() - wref; w_lim_.increment(dt * (-dist_diff * k_[0] - angle_diff * k_ang - wvel_diff * k_[2]), vel_[1], acc_[1], dt); - ROS_DEBUG( - "trajectory_tracker: distance residual %0.3f, angular residual %0.3f, ang vel residual %0.3f" - ", v_lim %0.3f, w_lim %0.3f signed_local_distance %0.3f, k_ang %0.3f", - dist_diff, angle_diff, wvel_diff, v_lim_.get(), w_lim_.get(), tracking_result.signed_local_distance, k_ang); + RCLCPP_DEBUG(get_logger(), + "distance residual %0.3f, angular residual %0.3f, ang vel residual %0.3f" + ", v_lim %0.3f, w_lim %0.3f signed_local_distance %0.3f, k_ang %0.3f", + dist_diff, angle_diff, wvel_diff, v_lim_.get(), w_lim_.get(), + tracking_result.signed_local_distance, k_ang); } if (std::abs(tracking_result.distance_remains) < stop_tolerance_dist_ && std::abs(tracking_result.angle_remains) < stop_tolerance_ang_ && @@ -501,10 +478,10 @@ void TrackerNode::control( v_lim_.clear(); w_lim_.clear(); } - geometry_msgs::Twist cmd_vel; + geometry_msgs::msg::Twist cmd_vel; cmd_vel.linear.x = v_lim_.get(); cmd_vel.angular.z = w_lim_.get(); - pub_vel_.publish(cmd_vel); + pub_vel_->publish(cmd_vel); path_step_done_ = tracking_result.path_step_done; break; } @@ -512,16 +489,17 @@ void TrackerNode::control( status.status = tracking_result.status; status.distance_remains = tracking_result.distance_remains; status.angle_remains = tracking_result.angle_remains; - pub_status_.publish(status); + pub_status_->publish(status); } -TrackerNode::TrackingResult TrackerNode::getTrackingResult( - const tf2::Stamped& robot_to_odom, const Eigen::Vector3d& prediction_offset, - const double odom_linear_vel, const double odom_angular_vel) const +TrackerNode::TrackingResult TrackerNode::getTrackingResult(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) { - return TrackingResult(trajectory_tracker_msgs::TrajectoryTrackerStatus::NO_PATH); + return TrackingResult(trajectory_tracker_msgs::msg::TrajectoryTrackerStatus::NO_PATH); } // Transform trajectory_tracker::Path2D lpath; @@ -530,32 +508,30 @@ TrackerNode::TrackingResult TrackerNode::getTrackingResult( try { tf2::Stamped odom_to_path; - tf2::fromMsg( - tfbuf_.lookupTransform(frame_odom_, path_header_.frame_id, ros::Time(0)), odom_to_path); + tf2::fromMsg(tfbuf_.lookupTransform(frame_odom_, path_header_.frame_id, rclcpp::Time(0)), odom_to_path); + transform *= odom_to_path; - transform_delay = (ros::Time::now() - transform.stamp_).toSec(); + transform_delay = tf2::durationToSec(tf2_ros::fromRclcpp(now()) - transform.stamp_); if (std::abs(transform_delay) > 0.1 && check_old_path_) { - ROS_ERROR_THROTTLE( - 1.0, "Timestamp of the transform is too old %f %f", - ros::Time::now().toSec(), transform.stamp_.toSec()); + const double seconds1 = now().seconds(); + const double seconds2 = tf2::timeToSec(transform.stamp_); + rclcpp::Clock clock(RCL_ROS_TIME); + RCLCPP_ERROR_THROTTLE(this->get_logger(), clock, 1000, "Timestamp of the transform is too old %f %f", seconds1, + seconds2); } - const float trans_yaw = tf2::getYaw(transform.getRotation()); const Eigen::Transform trans = - Eigen::Translation2d( - Eigen::Vector2d(transform.getOrigin().x(), transform.getOrigin().y())) * + Eigen::Translation2d(Eigen::Vector2d(transform.getOrigin().x(), transform.getOrigin().y())) * Eigen::Rotation2Dd(trans_yaw); for (size_t i = 0; i < path_.size(); i += path_step_) - lpath.push_back( - trajectory_tracker::Pose2D( - trans * path_[i].pos_, trans_yaw + path_[i].yaw_, path_[i].velocity_)); + lpath.push_back(trajectory_tracker::Pose2D(trans * path_[i].pos_, trans_yaw + path_[i].yaw_, path_[i].velocity_)); } catch (tf2::TransformException& e) { - ROS_WARN("TF exception: %s", e.what()); - return TrackingResult(trajectory_tracker_msgs::TrajectoryTrackerStatus::NO_PATH); + RCLCPP_WARN(get_logger(), "TF exception: %s", e.what()); + return TrackingResult(trajectory_tracker_msgs::msg::TrajectoryTrackerStatus::NO_PATH); } const Eigen::Vector2d origin_raw = prediction_offset.head<2>(); @@ -573,25 +549,23 @@ TrackerNode::TrackingResult TrackerNode::getTrackingResult( const float max_search_range = (path_step_done_ > 0) ? 1.0 : 0.0; const trajectory_tracker::Path2D::ConstIterator it_nearest = - lpath.findNearest(lpath.cbegin() + path_step_done_, it_local_goal, origin, - max_search_range, epsilon_); + lpath.findNearest(lpath.cbegin() + path_step_done_, it_local_goal, origin, max_search_range, epsilon_); if (it_nearest == lpath.end()) { - return TrackingResult(trajectory_tracker_msgs::TrajectoryTrackerStatus::NO_PATH); + return TrackingResult(trajectory_tracker_msgs::msg::TrajectoryTrackerStatus::NO_PATH); } - const int i_nearest = std::distance(lpath.cbegin(), it_nearest); - const int i_nearest_prev = std::max(0, i_nearest - 1); - const int i_local_goal = std::distance(lpath.cbegin(), it_local_goal); + const int64_t i_nearest = std::distance(lpath.cbegin(), it_nearest); + const int64_t i_nearest_prev = std::max(0l, i_nearest - 1); + const int64_t i_local_goal = std::distance(lpath.cbegin(), it_local_goal); const Eigen::Vector2d pos_on_line = trajectory_tracker::projection2d(lpath[i_nearest_prev].pos_, lpath[i_nearest].pos_, origin); const Eigen::Vector2d pos_on_line_raw = trajectory_tracker::projection2d(lpath[i_nearest_prev].pos_, lpath[i_nearest].pos_, origin_raw); - const float linear_vel = - std::isnan(lpath[i_nearest].velocity_) ? vel_[0] : lpath[i_nearest].velocity_; + const float linear_vel = std::isnan(lpath[i_nearest].velocity_) ? vel_[0] : lpath[i_nearest].velocity_; // Remained distance to the local goal float remain_local = lpath.remainedDistance(lpath.cbegin(), it_nearest, it_local_goal, pos_on_line); @@ -602,8 +576,7 @@ TrackerNode::TrackingResult TrackerNode::getTrackingResult( distance_remains = distance_remains_raw = remain_local = 0; // Signed distance error - const float dist_err = trajectory_tracker::lineDistance( - lpath[i_nearest_prev].pos_, lpath[i_nearest].pos_, origin); + const float dist_err = trajectory_tracker::lineDistance(lpath[i_nearest_prev].pos_, lpath[i_nearest].pos_, origin); // Angular error const Eigen::Vector2d vec = lpath[i_nearest].pos_ - lpath[i_nearest_prev].pos_; @@ -620,30 +593,27 @@ TrackerNode::TrackingResult TrackerNode::getTrackingResult( // Curvature const float curv = lpath.getCurvature(it_nearest, it_local_goal, pos_on_line, curv_forward_); - ROS_DEBUG( - "trajectory_tracker: nearest: %d, local goal: %d, done: %d, goal: %lu, remain: %0.3f, remain_local: %0.3f", - i_nearest, i_local_goal, path_step_done_, lpath.size(), distance_remains, remain_local); + RCLCPP_DEBUG(get_logger(), "nearest: %ld, local goal: %ld, done: %ld, goal: %lu, remain: %0.3f, remain_local: %0.3f", + i_nearest, i_local_goal, path_step_done_, lpath.size(), distance_remains, remain_local); bool arrive_local_goal(false); bool in_place_turning = (vec[1] == 0.0 && vec[0] == 0.0); - TrackingResult result(trajectory_tracker_msgs::TrajectoryTrackerStatus::FOLLOWING); + TrackingResult result(trajectory_tracker_msgs::msg::TrajectoryTrackerStatus::FOLLOWING); // Stop and rotate const bool large_angle_error = std::abs(rotate_ang_) < M_PI && std::cos(rotate_ang_) > std::cos(angle_remains); - if (large_angle_error || - std::abs(remain_local) < stop_tolerance_dist_ || - path_length < min_track_path_ || + if (large_angle_error || std::abs(remain_local) < stop_tolerance_dist_ || path_length < min_track_path_ || in_place_turning) { if (large_angle_error) { - ROS_INFO_THROTTLE(1.0, "Stop and rotate due to large angular error: %0.3f", angle_remains); + rclcpp::Clock clock(RCL_ROS_TIME); + RCLCPP_DEBUG_THROTTLE(get_logger(), clock, 1000, "Stop and rotate due to large angular error: %0.3f", + angle_remains); } - if (path_length < min_track_path_ || - std::abs(remain_local) < stop_tolerance_dist_ || - in_place_turning) + if (path_length < min_track_path_ || std::abs(remain_local) < stop_tolerance_dist_ || in_place_turning) { angle_remains = trajectory_tracker::angleNormalized(-(it_local_goal - 1)->yaw_); if (it_local_goal != lpath.end()) @@ -672,7 +642,7 @@ TrackerNode::TrackingResult TrackerNode::getTrackingResult( result.distance_remains_raw = distance_remains_raw; result.angle_remains = angle_remains; result.angle_remains_raw = angle_remains + yaw_raw; - result.status = trajectory_tracker_msgs::TrajectoryTrackerStatus::FAR_FROM_PATH; + result.status = trajectory_tracker_msgs::msg::TrajectoryTrackerStatus::FAR_FROM_PATH; return result; } @@ -698,7 +668,7 @@ TrackerNode::TrackingResult TrackerNode::getTrackingResult( (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; + result.status = trajectory_tracker_msgs::msg::TrajectoryTrackerStatus::GOAL; } if (arrive_local_goal) @@ -712,9 +682,8 @@ TrackerNode::TrackingResult TrackerNode::getTrackingResult( int main(int argc, char** argv) { - ros::init(argc, argv, "trajectory_tracker"); - trajectory_tracker::TrackerNode track; - track.spin(); - + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared("trajectory_tracker")); + rclcpp::shutdown(); return 0; } diff --git a/trajectory_tracker/test/configs/test_overshoot_params.yaml b/trajectory_tracker/test/configs/test_overshoot_params.yaml new file mode 100644 index 000000000..63c7d3ae9 --- /dev/null +++ b/trajectory_tracker/test/configs/test_overshoot_params.yaml @@ -0,0 +1,19 @@ +trajectory_tracker: + ros__parameters: + max_vel: 1.0 + max_acc: 2.0 + max_angvel: 0.5 + max_angacc: 2.0 + goal_tolerance_dist: 0.01 + goal_tolerance_ang: 0.0075 + stop_tolerance_dist: 0.01 + stop_tolerance_ang: 0.005 + look_forward: 0.0 + k_dist: 4.5 + k_ang: 3.0 + k_avel: 4.0 + gain_at_vel: 1.0 + dist_lim: 0.5 + use_odom: true + odom_timeout_sec: 0.1 + hz: 30.0 diff --git a/trajectory_tracker/test/configs/test_params.yaml b/trajectory_tracker/test/configs/test_params.yaml new file mode 100644 index 000000000..4e07bc593 --- /dev/null +++ b/trajectory_tracker/test/configs/test_params.yaml @@ -0,0 +1,29 @@ +test_trajectory_tracker: + ros__parameters: +# use_sim_time: true + odom_delay: 0.0 + error_lin: 0.03 + error_ang: 0.03 + +trajectory_tracker: + ros__parameters: +# use_sim_time: true + max_vel: 1.0 + max_acc: 2.0 + max_angvel: 0.5 + max_angacc: 2.0 + goal_tolerance_dist: 0.005 + goal_tolerance_ang: 0.005 + stop_tolerance_dist: 0.002 + stop_tolerance_ang: 0.002 + look_forward: 0.0 + k_dist: 4.5 + k_ang: 3.0 + k_avel: 4.0 + gain_at_vel: 1.0 + dist_lim: 0.5 + use_odom: false + odom_timeout_sec: 0.0 + use_time_optimal_control: true + k_ang_rotation: 8.0 + k_avel_rotation: 5.0 diff --git a/trajectory_tracker/test/include/trajectory_tracker_test.h b/trajectory_tracker/test/include/trajectory_tracker_test.h index f105d9d2f..add1848ce 100644 --- a/trajectory_tracker/test/include/trajectory_tracker_test.h +++ b/trajectory_tracker/test/include/trajectory_tracker_test.h @@ -37,65 +37,63 @@ #include #include -#include - #include #include -#include +#include -#include -#include -#include -#include -#include -#include #include +#include +#include +#include +#include +#include +#include -#include -#include -#include +#include +#include #include -class TrajectoryTrackerTest : public ::testing::Test +class TrajectoryTrackerTest : public ::testing::Test, public rclcpp::Node { private: - ros::NodeHandle nh_; - ros::NodeHandle pnh_; - ros::Subscriber sub_cmd_vel_; - ros::Subscriber sub_status_; - ros::Publisher pub_path_; - ros::Publisher pub_path_vel_; - ros::Publisher pub_odom_; + rclcpp::SubscriptionBase::SharedPtr sub_cmd_vel_; + rclcpp::SubscriptionBase::SharedPtr sub_status_; + rclcpp::Publisher::SharedPtr pub_path_; + rclcpp::Publisher::SharedPtr pub_path_vel_; + rclcpp::Publisher::SharedPtr pub_odom_; + rclcpp::Client::SharedPtr sc_set_parameters_; + tf2_ros::TransformBroadcaster tfb_; - ros::Time cmd_vel_time_; - ros::Time trans_stamp_last_; + rclcpp::Time cmd_vel_time_; + rclcpp::Time trans_stamp_last_; - ros::Time initial_cmd_vel_time_; + rclcpp::Time initial_cmd_vel_time_; int cmd_vel_count_; - std::list odom_buffer_; + std::list odom_buffer_; + + std::shared_ptr steady_clock_; protected: - std_msgs::Header last_path_header_; + std_msgs::msg::Header last_path_header_; 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) + void cbStatus(const trajectory_tracker_msgs::msg::TrajectoryTrackerStatus::ConstSharedPtr& msg) { status_ = msg; } - void cbCmdVel(const geometry_msgs::Twist::ConstPtr& msg) + void cbCmdVel(const geometry_msgs::msg::Twist::ConstSharedPtr& msg) { - const ros::Time now = ros::Time::now(); - if (cmd_vel_time_ == ros::Time(0)) + const auto now = this->now(); + + if (cmd_vel_time_.seconds() == 0.0) cmd_vel_time_ = now; - const float dt = std::min((now - cmd_vel_time_).toSec(), 0.1); + const float dt = std::min((now - cmd_vel_time_).seconds(), 0.1); yaw_ += msg->angular.z * dt; pos_ += Eigen::Vector2d(std::cos(yaw_), std::sin(yaw_)) * msg->linear.x * dt; @@ -107,100 +105,102 @@ class TrajectoryTrackerTest : public ::testing::Test public: Eigen::Vector2d pos_; double yaw_; - trajectory_tracker_msgs::TrajectoryTrackerStatus::ConstPtr status_; - geometry_msgs::Twist::ConstPtr cmd_vel_; - ros::Duration delay_; + trajectory_tracker_msgs::msg::TrajectoryTrackerStatus::ConstSharedPtr status_; + geometry_msgs::msg::Twist::ConstSharedPtr cmd_vel_; + rclcpp::Duration delay_; TrajectoryTrackerTest() - : nh_("") - , pnh_("~") + : Node("trajectory_tracker_test") + , tfb_(this) + , steady_clock_(std::make_shared(RCL_STEADY_TIME)) + , delay_(0, 0) { - sub_cmd_vel_ = nh_.subscribe( - "cmd_vel", 1, &TrajectoryTrackerTest::cbCmdVel, this); - sub_status_ = nh_.subscribe( - "trajectory_tracker/status", 1, &TrajectoryTrackerTest::cbStatus, this); - pub_path_ = nh_.advertise("path", 1, true); - pub_path_vel_ = nh_.advertise("path_velocity", 1, true); - pub_odom_ = nh_.advertise("odom", 10, true); - - double delay; - pnh_.param("odom_delay", delay, 0.0); - delay_ = ros::Duration(delay); - pnh_.param("error_lin", error_lin_, 0.01); - 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); + sub_cmd_vel_ = create_subscription( + "cmd_vel", 1, std::bind(&TrajectoryTrackerTest::cbCmdVel, this, std::placeholders::_1)); + sub_status_ = create_subscription( + "trajectory_tracker/status", 1, std::bind(&TrajectoryTrackerTest::cbStatus, this, std::placeholders::_1)); + pub_path_ = create_publisher("path", 1); + pub_path_vel_ = create_publisher("path_velocity", 1); + pub_odom_ = create_publisher("odom", 10); + sc_set_parameters_ = create_client("/trajectory_tracker/set_parameters"); + + declare_parameter("odom_delay", 0.0); + declare_parameter("error_lin", 0.01); + declare_parameter("error_large_lin", 0.1); + declare_parameter("error_ang", 0.01); + + delay_ = rclcpp::Duration::from_seconds(get_parameter("odom_delay").as_double()); + get_parameter("error_lin", error_lin_); + get_parameter("error_large_lin", error_large_lin_); + get_parameter("error_ang", error_ang_); + rclcpp::Rate wait(10); for (size_t i = 0; i < 100; ++i) { wait.sleep(); - ros::spinOnce(); - if (pub_path_.getNumSubscribers() > 0) + rclcpp::spin_some(get_node_base_interface()); + if (pub_path_->get_subscription_count() > 0) break; } } + void initState(const Eigen::Vector2d& pos, const float yaw) { // Wait trajectory_tracker node - ros::Rate rate(10); - const auto start = ros::WallTime::now(); - while (ros::ok()) + rclcpp::Rate rate(10); + const auto start = steady_clock_->now(); + while (rclcpp::ok()) { - nav_msgs::Path path; + nav_msgs::msg::Path path; path.header.frame_id = "odom"; - path.header.stamp = ros::Time::now(); - pub_path_.publish(path); + path.header.stamp = now(); + pub_path_->publish(path); yaw_ = yaw; pos_ = pos; publishTransform(); rate.sleep(); - ros::spinOnce(); - if (status_ && - status_->status != trajectory_tracker_msgs::TrajectoryTrackerStatus::FOLLOWING) + rclcpp::spin_some(get_node_base_interface()); + if (status_ && status_->status != trajectory_tracker_msgs::msg::TrajectoryTrackerStatus::FOLLOWING) break; - ASSERT_LT(ros::WallTime::now(), start + ros::WallDuration(10.0)) + ASSERT_LT(steady_clock_->now(), start + rclcpp::Duration::from_seconds(10.0)) << "trajectory_tracker status timeout, status: " << (status_ ? std::to_string(static_cast(status_->status)) : "none"); } - ros::Duration(0.5).sleep(); + rclcpp::sleep_for(std::chrono::milliseconds(500)); } void waitUntilStart(const std::function func = nullptr) { - ros::Rate rate(50); - const auto start = ros::WallTime::now(); - while (ros::ok()) + rclcpp::Rate rate(50); + const auto start = steady_clock_->now(); + while (rclcpp::ok()) { if (func) func(); publishTransform(); rate.sleep(); - ros::spinOnce(); - if (status_ && - status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::FOLLOWING) + rclcpp::spin_some(get_node_base_interface()); + if (status_ && status_->status == trajectory_tracker_msgs::msg::TrajectoryTrackerStatus::FOLLOWING) break; - ASSERT_LT(ros::WallTime::now(), start + ros::WallDuration(10.0)) + ASSERT_LT(steady_clock_->now(), start + rclcpp::Duration::from_seconds(10.0)) << "trajectory_tracker status timeout, status: " << (status_ ? std::to_string(static_cast(status_->status)) : "none"); } - initial_cmd_vel_time_ = ros::Time::now(); + initial_cmd_vel_time_ = now(); cmd_vel_count_ = 0; } void publishPath(const std::vector& poses) { - nav_msgs::Path path; + nav_msgs::msg::Path path; path.header.frame_id = "odom"; - path.header.stamp = ros::Time::now(); + path.header.stamp = now(); for (const Eigen::Vector3d& p : poses) { const Eigen::Quaterniond q(Eigen::AngleAxisd(p[2], Eigen::Vector3d(0, 0, 1))); - geometry_msgs::PoseStamped pose; + geometry_msgs::msg::PoseStamped pose; pose.header.frame_id = path.header.frame_id; pose.pose.position.x = p[0]; pose.pose.position.y = p[1]; @@ -211,20 +211,20 @@ class TrajectoryTrackerTest : public ::testing::Test path.poses.push_back(pose); } - pub_path_.publish(path); + pub_path_->publish(path); last_path_header_ = path.header; } void publishPathVelocity(const std::vector& poses) { - trajectory_tracker_msgs::PathWithVelocity path; + trajectory_tracker_msgs::msg::PathWithVelocity path; path.header.frame_id = "odom"; - path.header.stamp = ros::Time::now(); + path.header.stamp = now(); for (const Eigen::Vector4d& p : poses) { const Eigen::Quaterniond q(Eigen::AngleAxisd(p[2], Eigen::Vector3d(0, 0, 1))); - trajectory_tracker_msgs::PoseStampedWithVelocity pose; + trajectory_tracker_msgs::msg::PoseStampedWithVelocity pose; pose.header.frame_id = path.header.frame_id; pose.pose.position.x = p[0]; pose.pose.position.y = p[1]; @@ -237,16 +237,17 @@ class TrajectoryTrackerTest : public ::testing::Test path.poses.push_back(pose); } // needs sleep to prevent that the empty path from initState arrives later. - ros::Duration(0.5).sleep(); - pub_path_vel_.publish(path); + rclcpp::sleep_for(std::chrono::milliseconds(500)); + pub_path_vel_->publish(path); last_path_header_ = path.header; } + void publishTransform() { - const ros::Time now = ros::Time::now(); + const auto now = this->now(); const Eigen::Quaterniond q(Eigen::AngleAxisd(yaw_, Eigen::Vector3d(0, 0, 1))); - nav_msgs::Odometry odom; + nav_msgs::msg::Odometry odom; odom.header.frame_id = "odom"; odom.header.stamp = now; odom.child_frame_id = "base_link"; @@ -265,25 +266,23 @@ class TrajectoryTrackerTest : public ::testing::Test publishTransform(odom); } - void publishTransform(const nav_msgs::Odometry& odom) + void publishTransform(const nav_msgs::msg::Odometry& odom) { odom_buffer_.push_back(odom); - - const ros::Time pub_time = odom.header.stamp - delay_; - + const rclcpp::Time pub_time = rclcpp::Time(odom.header.stamp) - delay_; while (odom_buffer_.size() > 0) { - nav_msgs::Odometry odom = odom_buffer_.front(); - if (odom.header.stamp > pub_time) + nav_msgs::msg::Odometry odom = odom_buffer_.front(); + if (rclcpp::Time(odom.header.stamp) > pub_time) break; odom_buffer_.pop_front(); if (odom.header.stamp != trans_stamp_last_) { - geometry_msgs::TransformStamped trans; - trans.header = odom.header; - trans.header.stamp += ros::Duration(0.1); + geometry_msgs::msg::TransformStamped trans; + trans.header.frame_id = odom.header.frame_id; + trans.header.stamp = rclcpp::Time(odom.header.stamp) + rclcpp::Duration::from_seconds(0.1); trans.child_frame_id = odom.child_frame_id; trans.transform.translation.x = odom.pose.pose.position.x; trans.transform.translation.y = odom.pose.pose.position.y; @@ -293,7 +292,7 @@ class TrajectoryTrackerTest : public ::testing::Test trans.transform.rotation.w = odom.pose.pose.orientation.w; tfb_.sendTransform(trans); - pub_odom_.publish(odom); + pub_odom_->publish(odom); } trans_stamp_last_ = odom.header.stamp; } @@ -301,38 +300,68 @@ class TrajectoryTrackerTest : public ::testing::Test double getCmdVelFrameRate() const { - return cmd_vel_count_ / (cmd_vel_time_ - initial_cmd_vel_time_).toSec(); + return cmd_vel_count_ / (cmd_vel_time_ - initial_cmd_vel_time_).seconds(); } - bool getConfig(ParamType& config) const + bool setConfig(const std::vector& params) { - const ros::WallTime time_limit = ros::WallTime::now() + ros::WallDuration(10.0); - while (time_limit > ros::WallTime::now()) + if (!sc_set_parameters_->wait_for_service(std::chrono::seconds(5))) { - if (dynamic_reconfigure_client_->getCurrentConfiguration(config, ros::Duration(0.1))) - { - return true; - } - ros::spinOnce(); + return false; } - return false; - } - bool setConfig(const ParamType& config) - { - // Wait until parameter server becomes ready - ParamType dummy; - if (!getConfig(dummy)) + auto req = std::make_shared(); + for (const auto& param : params) + { + req->parameters.push_back(param.to_parameter_msg()); + } + auto result = sc_set_parameters_->async_send_request(req); + if (rclcpp::spin_until_future_complete(get_node_base_interface(), result) == rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_INFO(get_logger(), "Successfully set parameter."); + return true; + } + else { + RCLCPP_ERROR(get_logger(), "Failed to set parameter."); return false; } - return dynamic_reconfigure_client_->setConfiguration(config); } + + /* + 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, rclcpp::Duration::from_seconds(0.1))) + { + return true; + } + rclcpp::spin_some(node); + } + 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 + +/* namespace trajectory_tracker_msgs { -std::ostream& operator<<(std::ostream& os, const TrajectoryTrackerStatus::ConstPtr& msg) +std::ostream& operator<<(std::ostream& os, const TrajectoryTrackerStatus::ConstSharedPtr& msg) { if (!msg) { @@ -349,5 +378,4 @@ std::ostream& operator<<(std::ostream& os, const TrajectoryTrackerStatus::ConstP return os; } } // namespace trajectory_tracker_msgs - -#endif // TRAJECTORY_TRACKER_TEST_H +*/ \ No newline at end of file diff --git a/trajectory_tracker/test/src/test_path2d.cpp b/trajectory_tracker/test/src/test_path2d.cpp index 1a989b162..7563787d7 100644 --- a/trajectory_tracker/test/src/test_path2d.cpp +++ b/trajectory_tracker/test/src/test_path2d.cpp @@ -36,10 +36,10 @@ #include -#include +#include #include #include -#include +#include namespace { @@ -247,7 +247,7 @@ TEST(Path2D, FindNearestWithDistance) TEST(Path2D, Conversions) { - nav_msgs::Path path_msg_org; + nav_msgs::msg::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; @@ -295,9 +295,9 @@ TEST(Path2D, Conversions) EXPECT_TRUE(std::isnan(path[i].velocity_)); } - nav_msgs::Path path_msg; + nav_msgs::msg::Path path_msg; path_msg.header.frame_id = "map"; - path_msg.header.stamp = ros::Time(123.456); + path_msg.header.stamp = rclcpp::Time(123.456); path.toMsg(path_msg); ASSERT_EQ(path_msg.poses.size(), 6); for (size_t i = 0; i < path.size(); ++i) @@ -309,7 +309,7 @@ TEST(Path2D, Conversions) EXPECT_EQ(path_msg.poses[i].header.stamp, path_msg.header.stamp); } - trajectory_tracker_msgs::PathWithVelocity path_with_vel_msg_org; + trajectory_tracker_msgs::msg::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) { @@ -333,9 +333,9 @@ TEST(Path2D, Conversions) EXPECT_NEAR(path_with_vel[i].velocity_, org_index * 0.1, 1.0e-6) << "i: " << i << " org: " << org_index; } - trajectory_tracker_msgs::PathWithVelocity path_with_vel_msg; + trajectory_tracker_msgs::msg::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_msg.header.stamp = rclcpp::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) diff --git a/trajectory_tracker/test/src/test_trajectory_recorder.cpp b/trajectory_tracker/test/src/test_trajectory_recorder.cpp index b2f29af48..9d183b4aa 100644 --- a/trajectory_tracker/test/src/test_trajectory_recorder.cpp +++ b/trajectory_tracker/test/src/test_trajectory_recorder.cpp @@ -27,11 +27,11 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include +#include "rclcpp/rclcpp.hpp" #include #include -#include -#include +#include +#include #include #include @@ -40,17 +40,17 @@ TEST(TrajectoryRecorder, TfToPath) { - ros::NodeHandle nh(""); + rclcpp::Node nh(""); - nav_msgs::Path::ConstPtr path; + nav_msgs::msg::Path::ConstSharedPtr path; int received_count = 0; - const boost::function cb_path = - [&path, &received_count](const nav_msgs::Path::ConstPtr& msg) -> void + const std::function cb_path = + [&path, &received_count](const nav_msgs::msg::Path::ConstSharedPtr& msg) -> void { ++received_count; path = msg; }; - ros::Subscriber sub_path = nh.subscribe("path", 1, cb_path); + auto sub_path = nh.subscribe("path", 1, cb_path); tf2_ros::TransformBroadcaster tfb; const tf2::Transform points[] = @@ -62,20 +62,20 @@ TEST(TrajectoryRecorder, TfToPath) }; const size_t len = sizeof(points) / sizeof(tf2::Transform); - ros::Duration(1.0).sleep(); + rclcpp::Duration(1.0).sleep(); for (auto& p : points) { for (size_t i = 0; i < 3; ++i) { - geometry_msgs::TransformStamped trans = + geometry_msgs::msg::TransformStamped trans = tf2::toMsg(tf2::Stamped( - p, ros::Time::now() + ros::Duration(0.1), "map")); + p, rclcpp::Time::now() + rclcpp::Duration(0.1), "map")); trans.child_frame_id = "base_link"; tfb.sendTransform(trans); - ros::Duration(0.1).sleep(); + rclcpp::Duration(0.1).sleep(); } } - ros::spinOnce(); + rclcpp::spin_some(node); ASSERT_TRUE(static_cast(path)); ASSERT_EQ(received_count, 1); @@ -91,14 +91,14 @@ TEST(TrajectoryRecorder, TfToPath) ASSERT_EQ(path->poses[i].pose.orientation.w, points[i].getRotation().w()); } - ros::ServiceClient client = nh.serviceClient("/trajectory_recorder/clear_path"); - std_srvs::Empty empty; + ros::ServiceClient client = nh.serviceClient("/trajectory_recorder/clear_path"); + std_srvs::srv::Empty empty; ASSERT_TRUE(client.call(empty)); while (received_count != 2) { - ros::spinOnce(); - ros::Duration(0.1).sleep(); + rclcpp::spin_some(node); + rclcpp::Duration(0.1).sleep(); } ASSERT_EQ(static_cast(path->poses.size()), 1); ASSERT_EQ(path->poses.back().pose.position.x, points[len - 1].getOrigin().x()); @@ -113,7 +113,8 @@ TEST(TrajectoryRecorder, TfToPath) int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "test_trajectory_recorder"); + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("test_trajectory_recorder"); return RUN_ALL_TESTS(); } diff --git a/trajectory_tracker/test/src/test_trajectory_tracker.cpp b/trajectory_tracker/test/src/test_trajectory_tracker.cpp index 1dc6b5cbf..0857d7d9f 100644 --- a/trajectory_tracker/test/src/test_trajectory_tracker.cpp +++ b/trajectory_tracker/test/src/test_trajectory_tracker.cpp @@ -31,7 +31,90 @@ #include #include -#include +#include "trajectory_tracker_test.h" + +using std::chrono::duration; +using std::chrono::duration_cast; +using std::chrono::nanoseconds; + +class RosRate : public rclcpp::RateBase +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(RosRate) + + explicit RosRate(double rate, rclcpp::Node& node) + : RosRate(duration_cast(duration(1.0 / rate)), node) + { + } + explicit RosRate(std::chrono::nanoseconds period, rclcpp::Node& node) + : node_(node.get_node_base_interface()) + , clock_(node.get_clock()) + , period_(period) + { + last_interval_ = clock_->now(); + } + + virtual bool sleep() + { + // Time coming into sleep + auto now = clock_->now(); + // Time of next interval + auto next_interval = last_interval_ + period_; + // Detect backwards time flow + if (now < last_interval_) + { + // Best thing to do is to set the next_interval to now + period + next_interval = now + period_; + } + // Calculate the time to sleep + auto time_to_sleep = next_interval - now; + // Update the interval + last_interval_ += period_; + // If the time_to_sleep is negative or zero, don't sleep + if (time_to_sleep <= std::chrono::seconds(0)) + { + // If an entire cycle was missed then reset next interval. + // This might happen if the loop took more than a cycle. + // Or if time jumps forward. + if (now > next_interval + period_) + { + last_interval_ = now + period_; + } + // Either way do not sleep and return false + return false; + } + // Sleep (will get interrupted by ctrl-c, may not sleep full time) + while (clock_->now() < next_interval) + { + rclcpp::spin_some(node_); + clock_->sleep_for(std::chrono::milliseconds(1)); + } + return true; + } + + virtual bool is_steady() const + { + return false; + } + + virtual void reset() + { + last_interval_ = clock_->now(); + } + + std::chrono::nanoseconds period() const + { + return std::chrono::nanoseconds(period_.nanoseconds()); + } + +private: + RCLCPP_DISABLE_COPY(RosRate) + + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_; + rclcpp::Clock::SharedPtr clock_; + rclcpp::Duration period_; + rclcpp::Time last_interval_; +}; TEST_F(TrajectoryTrackerTest, StraightStop) { @@ -43,24 +126,22 @@ TEST_F(TrajectoryTrackerTest, StraightStop) poses.push_back(Eigen::Vector3d(0.5, 0.0, 0.0)); waitUntilStart(std::bind(&TrajectoryTrackerTest::publishPath, this, poses)); - ros::Rate rate(50); - const ros::Time start = ros::Time::now(); - while (ros::ok()) + RosRate rate(50, *this); + const rclcpp::Time start = now(); + while (rclcpp::ok()) { - if (ros::Time::now() > start + ros::Duration(10.0)) + if (now() > start + rclcpp::Duration::from_seconds(10.0)) { - FAIL() - << "Timeout" << std::endl - << "Pos " << pos_ << std::endl - << "Yaw " << yaw_ << std::endl - << "Status " << std::endl - << status_ << std::endl; + FAIL() << "Timeout" << std::endl + << "Pos " << pos_ << std::endl + << "Yaw " << yaw_ << std::endl + << "Status " << std::endl + << status_ << std::endl; } - publishTransform(); rate.sleep(); - ros::spinOnce(); - if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL) + rclcpp::spin_some(get_node_base_interface()); + if (status_->status == trajectory_tracker_msgs::msg::TrajectoryTrackerStatus::GOAL) break; } for (int j = 0; j < 5; ++j) @@ -69,28 +150,24 @@ TEST_F(TrajectoryTrackerTest, StraightStop) { publishTransform(); rate.sleep(); - ros::spinOnce(); + rclcpp::spin_some(get_node_base_interface()); } // Check multiple times to assert overshoot. - ASSERT_NEAR(yaw_, 0.0, error_ang_) - << "[overshoot after goal (" << j << ")] "; - ASSERT_NEAR(pos_[0], 0.5, error_lin_) - << "[overshoot after goal (" << j << ")] "; - ASSERT_NEAR(pos_[1], 0.0, error_lin_) - << "[overshoot after goal (" << j << ")] "; + ASSERT_NEAR(yaw_, 0.0, error_ang_) << "[overshoot after goal (" << j << ")] "; + ASSERT_NEAR(pos_[0], 0.5, error_lin_) << "[overshoot after goal (" << j << ")] "; + ASSERT_NEAR(pos_[1], 0.0, error_lin_) << "[overshoot after goal (" << j << ")] "; } ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp); } TEST_F(TrajectoryTrackerTest, StraightStopOvershoot) { - const double resolutions[] = - { - 0.1, - 0.001, // default epsilon - 0.0001, - }; + const double resolutions[] = { + 0.1, + 0.001, // default epsilon + 0.0001, + }; for (const double resolution : resolutions) { const std::string info_message = "resolution: " + std::to_string(resolution); @@ -104,25 +181,24 @@ TEST_F(TrajectoryTrackerTest, StraightStopOvershoot) poses.push_back(Eigen::Vector3d(0.5, 0, 0)); waitUntilStart(std::bind(&TrajectoryTrackerTest::publishPath, this, poses)); - ros::Rate rate(50); - const ros::Time start = ros::Time::now(); - while (ros::ok()) + RosRate rate(50, *this); + const rclcpp::Time start = now(); + while (rclcpp::ok()) { - if (ros::Time::now() > start + ros::Duration(10.0)) + if (now() > start + rclcpp::Duration::from_seconds(10.0)) { - FAIL() - << "Timeout" << std::endl - << "Pos " << pos_ << std::endl - << "Yaw " << yaw_ << std::endl - << "Status " << std::endl - << status_ << std::endl - << info_message; + FAIL() << "Timeout" << std::endl + << "Pos " << pos_ << std::endl + << "Yaw " << yaw_ << std::endl + << "Status " << std::endl + << status_ << std::endl + << info_message; } publishTransform(); rate.sleep(); - ros::spinOnce(); - if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL) + rclcpp::spin_some(get_node_base_interface()); + if (status_->status == trajectory_tracker_msgs::msg::TrajectoryTrackerStatus::GOAL) break; } for (int j = 0; j < 5; ++j) @@ -131,19 +207,13 @@ TEST_F(TrajectoryTrackerTest, StraightStopOvershoot) { publishTransform(); rate.sleep(); - ros::spinOnce(); + rclcpp::spin_some(get_node_base_interface()); } // Check multiple times to assert overshoot. - ASSERT_NEAR(yaw_, 0.0, error_ang_) - << "[overshoot after goal (" << j << ")] " - << info_message; - EXPECT_NEAR(pos_[0], 0.5, error_lin_) - << "[overshoot after goal (" << j << ")] " - << info_message; - ASSERT_NEAR(pos_[1], 0.0, error_lin_) - << "[overshoot after goal (" << j << ")] " - << info_message; + ASSERT_NEAR(yaw_, 0.0, error_ang_) << "[overshoot after goal (" << j << ")] " << info_message; + EXPECT_NEAR(pos_[0], 0.5, error_lin_) << "[overshoot after goal (" << j << ")] " << info_message; + ASSERT_NEAR(pos_[1], 0.0, error_lin_) << "[overshoot after goal (" << j << ")] " << info_message; } ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp) << info_message; } @@ -165,25 +235,24 @@ TEST_F(TrajectoryTrackerTest, StraightStopConvergence) poses.push_back(Eigen::Vector4d(path_length, 0.0, 0.0, vel)); waitUntilStart(std::bind(&TrajectoryTrackerTest::publishPathVelocity, this, poses)); - ros::Rate rate(50); - const ros::Time start = ros::Time::now(); - while (ros::ok()) + RosRate rate(50, *this); + const rclcpp::Time start = now(); + while (rclcpp::ok()) { - if (ros::Time::now() > start + ros::Duration(5.0 + path_length / vel)) + if (now() > start + rclcpp::Duration::from_seconds(5.0 + path_length / vel)) { - FAIL() - << "Timeout" << std::endl - << "Pos " << pos_ << std::endl - << "Yaw " << yaw_ << std::endl - << "Status " << std::endl - << status_ << std::endl - << info_message; + FAIL() << "Timeout" << std::endl + << "Pos " << pos_ << std::endl + << "Yaw " << yaw_ << std::endl + << "Status " << std::endl + << status_ << std::endl + << info_message; } publishTransform(); rate.sleep(); - ros::spinOnce(); - if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL) + rclcpp::spin_some(get_node_base_interface()); + if (status_->status == trajectory_tracker_msgs::msg::TrajectoryTrackerStatus::GOAL) break; } for (int j = 0; j < 5; ++j) @@ -192,19 +261,13 @@ TEST_F(TrajectoryTrackerTest, StraightStopConvergence) { publishTransform(); rate.sleep(); - ros::spinOnce(); + rclcpp::spin_some(get_node_base_interface()); } // Check multiple times to assert overshoot. - EXPECT_NEAR(yaw_, 0.0, error_ang_) - << "[overshoot after goal (" << j << ")] " - << info_message; - EXPECT_NEAR(pos_[0], path_length, error_lin_) - << "[overshoot after goal (" << j << ")] " - << info_message; - EXPECT_NEAR(pos_[1], 0.0, error_lin_) - << "[overshoot after goal (" << j << ")] " - << info_message; + EXPECT_NEAR(yaw_, 0.0, error_ang_) << "[overshoot after goal (" << j << ")] " << info_message; + EXPECT_NEAR(pos_[0], path_length, error_lin_) << "[overshoot after goal (" << j << ")] " << info_message; + EXPECT_NEAR(pos_[1], 0.0, error_lin_) << "[overshoot after goal (" << j << ")] " << info_message; } ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp); } @@ -222,23 +285,22 @@ TEST_F(TrajectoryTrackerTest, StraightVelocityChange) poses.push_back(Eigen::Vector4d(1.5, 0.0, 0.0, 0.5)); waitUntilStart(std::bind(&TrajectoryTrackerTest::publishPathVelocity, this, poses)); - ros::Rate rate(50); - const ros::Time start = ros::Time::now(); - while (ros::ok()) + RosRate rate(50, *this); + const rclcpp::Time start = now(); + while (rclcpp::ok()) { - if (ros::Time::now() > start + ros::Duration(10.0)) + if (now() > start + rclcpp::Duration::from_seconds(10.0)) { - FAIL() - << "Timeout" << std::endl - << "Pos " << pos_ << std::endl - << "Yaw " << yaw_ << std::endl - << "Status " << std::endl - << status_ << std::endl; + FAIL() << "Timeout" << std::endl + << "Pos " << pos_ << std::endl + << "Yaw " << yaw_ << std::endl + << "Status " << std::endl + << status_ << std::endl; } publishTransform(); rate.sleep(); - ros::spinOnce(); + rclcpp::spin_some(get_node_base_interface()); if (0.3 < pos_[0] && pos_[0] < 0.35) { @@ -249,7 +311,7 @@ TEST_F(TrajectoryTrackerTest, StraightVelocityChange) ASSERT_NEAR(cmd_vel_->linear.x, 0.5, error_lin_); } - if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL) + if (status_->status == trajectory_tracker_msgs::msg::TrajectoryTrackerStatus::GOAL) break; } for (int j = 0; j < 5; ++j) @@ -258,16 +320,13 @@ TEST_F(TrajectoryTrackerTest, StraightVelocityChange) { publishTransform(); rate.sleep(); - ros::spinOnce(); + rclcpp::spin_some(get_node_base_interface()); } // Check multiple times to assert overshoot. - ASSERT_NEAR(yaw_, 0.0, error_ang_) - << "[overshoot after goal (" << j << ")] "; - ASSERT_NEAR(pos_[0], 1.5, error_lin_) - << "[overshoot after goal (" << j << ")] "; - ASSERT_NEAR(pos_[1], 0.0, error_lin_) - << "[overshoot after goal (" << j << ")] "; + ASSERT_NEAR(yaw_, 0.0, error_ang_) << "[overshoot after goal (" << j << ")] "; + ASSERT_NEAR(pos_[0], 1.5, error_lin_) << "[overshoot after goal (" << j << ")] "; + ASSERT_NEAR(pos_[1], 0.0, error_lin_) << "[overshoot after goal (" << j << ")] "; } ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp); } @@ -290,24 +349,23 @@ TEST_F(TrajectoryTrackerTest, CurveFollow) } waitUntilStart(std::bind(&TrajectoryTrackerTest::publishPath, this, poses)); - ros::Rate rate(50); - const ros::Time start = ros::Time::now(); - while (ros::ok()) + RosRate rate(50, *this); + const rclcpp::Time start = now(); + while (rclcpp::ok()) { - if (ros::Time::now() > start + ros::Duration(20.0)) + if (now() > start + rclcpp::Duration::from_seconds(20.0)) { - FAIL() - << "Timeout" << std::endl - << "Pos " << pos_ << std::endl - << "Yaw " << yaw_ << std::endl - << "Status " << std::endl - << status_ << std::endl; + FAIL() << "Timeout" << std::endl + << "Pos " << pos_ << std::endl + << "Yaw " << yaw_ << std::endl + << "Status " << std::endl + << status_ << std::endl; } publishTransform(); rate.sleep(); - ros::spinOnce(); - if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL) + rclcpp::spin_some(get_node_base_interface()); + if (status_->status == trajectory_tracker_msgs::msg::TrajectoryTrackerStatus::GOAL) break; } for (int j = 0; j < 5; ++j) @@ -316,16 +374,13 @@ TEST_F(TrajectoryTrackerTest, CurveFollow) { publishTransform(); rate.sleep(); - ros::spinOnce(); + rclcpp::spin_some(get_node_base_interface()); } // Check multiple times to assert overshoot. - ASSERT_NEAR(yaw_, p[2], error_ang_) - << "[overshoot after goal (" << j << ")] "; - ASSERT_NEAR(pos_[0], p[0], error_large_lin_) - << "[overshoot after goal (" << j << ")] "; - ASSERT_NEAR(pos_[1], p[1], error_large_lin_) - << "[overshoot after goal (" << j << ")] "; + ASSERT_NEAR(yaw_, p[2], error_ang_) << "[overshoot after goal (" << j << ")] "; + ASSERT_NEAR(pos_[0], p[0], error_large_lin_) << "[overshoot after goal (" << j << ")] "; + ASSERT_NEAR(pos_[1], p[1], error_large_lin_) << "[overshoot after goal (" << j << ")] "; } ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp); } @@ -335,23 +390,19 @@ TEST_F(TrajectoryTrackerTest, InPlaceTurn) const float init_yaw_array[] = {0.0, 3.0}; for (const float init_yaw : init_yaw_array) { - const std::vector target_angle_array[] = - { - {0.5}, - {-0.5}, - {0.1, 0.2, 0.3, 0.4, 0.5}, - {-0.1, -0.2, -0.3, -0.4, -0.5}, - }; + const std::vector target_angle_array[] = { + {0.5}, + {-0.5}, + {0.1, 0.2, 0.3, 0.4, 0.5}, + {-0.1, -0.2, -0.3, -0.4, -0.5}, + }; for (const auto& angles : target_angle_array) { for (const bool& has_short_path : {false, true}) { std::stringstream condition_name; - condition_name - << "init_yaw: " << init_yaw - << ", angles: " << angles.front() << "-" << angles.back() - << ", has_short_path: " << has_short_path; - + condition_name << "init_yaw: " << init_yaw << ", angles: " << angles.front() << "-" << angles.back() + << ", has_short_path: " << has_short_path; initState(Eigen::Vector2d(0, 0), init_yaw); std::vector poses; @@ -365,39 +416,35 @@ TEST_F(TrajectoryTrackerTest, InPlaceTurn) } waitUntilStart(std::bind(&TrajectoryTrackerTest::publishPath, this, poses)); - ros::Rate rate(50); - const ros::Time start = ros::Time::now(); - for (int i = 0; ros::ok(); ++i) + RosRate rate(50, *this); + const rclcpp::Time start = now(); + for (int i = 0; rclcpp::ok(); ++i) { - if (ros::Time::now() > start + ros::Duration(10.0)) + if (now() > start + rclcpp::Duration::from_seconds(10.0)) { - FAIL() - << condition_name.str() - << "Timeout" << std::endl - << "Pos " << pos_ << std::endl - << "Yaw " << yaw_ << std::endl - << "Status " << std::endl - << status_ << std::endl; + FAIL() << condition_name.str() << "Timeout" << std::endl + << "Pos " << pos_ << std::endl + << "Yaw " << yaw_ << std::endl + << "Status " << std::endl + << status_ << std::endl; } publishTransform(); rate.sleep(); - ros::spinOnce(); + rclcpp::spin_some(get_node_base_interface()); if (cmd_vel_ && i > 5) { ASSERT_GT(cmd_vel_->angular.z * std::copysign(1.0, angles.back()), -error_ang_) - << "[overshoot detected] " - << condition_name.str(); + << "[overshoot detected] " << condition_name.str(); } if (status_ && i > 5) { ASSERT_LT(status_->angle_remains * std::copysign(1.0, angles.back()), error_ang_) - << "[overshoot detected] " - << condition_name.str(); + << "[overshoot detected] " << condition_name.str(); } - if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL) + if (status_->status == trajectory_tracker_msgs::msg::TrajectoryTrackerStatus::GOAL) break; } ASSERT_TRUE(static_cast(cmd_vel_)) << condition_name.str(); @@ -407,13 +454,12 @@ TEST_F(TrajectoryTrackerTest, InPlaceTurn) { publishTransform(); rate.sleep(); - ros::spinOnce(); + rclcpp::spin_some(get_node_base_interface()); } // Check multiple times to assert overshoot. ASSERT_NEAR(yaw_, init_yaw + angles.back(), error_ang_) - << "[overshoot after goal (" << j << ")] " - << condition_name.str(); + << "[overshoot after goal (" << j << ")] " << condition_name.str(); } ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp); } @@ -439,24 +485,23 @@ TEST_F(TrajectoryTrackerTest, SwitchBack) } waitUntilStart(std::bind(&TrajectoryTrackerTest::publishPath, this, poses)); - ros::Rate rate(50); - const ros::Time start = ros::Time::now(); - while (ros::ok()) + RosRate rate(50, *this); + const rclcpp::Time start = now(); + while (rclcpp::ok()) { - if (ros::Time::now() > start + ros::Duration(10.0)) + if (now() > start + rclcpp::Duration::from_seconds(10.0)) { - FAIL() - << "Timeout" << std::endl - << "Pos " << pos_ << std::endl - << "Yaw " << yaw_ << std::endl - << "Status " << std::endl - << status_ << std::endl; + FAIL() << "Timeout" << std::endl + << "Pos " << pos_ << std::endl + << "Yaw " << yaw_ << std::endl + << "Status " << std::endl + << status_ << std::endl; } publishTransform(); rate.sleep(); - ros::spinOnce(); - if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL) + rclcpp::spin_some(get_node_base_interface()); + if (status_->status == trajectory_tracker_msgs::msg::TrajectoryTrackerStatus::GOAL) break; } for (int j = 0; j < 5; ++j) @@ -465,16 +510,13 @@ TEST_F(TrajectoryTrackerTest, SwitchBack) { publishTransform(); rate.sleep(); - ros::spinOnce(); + rclcpp::spin_some(get_node_base_interface()); } // Check multiple times to assert overshoot. - ASSERT_NEAR(yaw_, p[2], error_ang_) - << "[overshoot after goal (" << j << ")] "; - ASSERT_NEAR(pos_[0], p[0], error_large_lin_) - << "[overshoot after goal (" << j << ")] "; - ASSERT_NEAR(pos_[1], p[1], error_large_lin_) - << "[overshoot after goal (" << j << ")] "; + ASSERT_NEAR(yaw_, p[2], error_ang_) << "[overshoot after goal (" << j << ")] "; + ASSERT_NEAR(pos_[0], p[0], error_large_lin_) << "[overshoot after goal (" << j << ")] "; + ASSERT_NEAR(pos_[1], p[1], error_large_lin_) << "[overshoot after goal (" << j << ")] "; } ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp); } @@ -501,24 +543,23 @@ TEST_F(TrajectoryTrackerTest, SwitchBackWithPathUpdate) waitUntilStart(std::bind(&TrajectoryTrackerTest::publishPath, this, poses)); int cnt_arrive_local_goal(0); - ros::Rate rate(50); - const ros::Time start = ros::Time::now(); - for (int i = 0; ros::ok(); i++) + RosRate rate(50, *this); + const rclcpp::Time start = now(); + for (int i = 0; rclcpp::ok(); i++) { - if (ros::Time::now() > start + ros::Duration(15.0)) + if (now() > start + rclcpp::Duration::from_seconds(15.0)) { - FAIL() - << "Timeout" << std::endl - << "Pos " << pos_ << std::endl - << "Yaw " << yaw_ << std::endl - << "Status " << std::endl - << status_ << std::endl; + FAIL() << "Timeout" << std::endl + << "Pos " << pos_ << std::endl + << "Yaw " << yaw_ << std::endl + << "Status " << std::endl + << status_ << std::endl; } publishTransform(); rate.sleep(); - ros::spinOnce(); - if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL) + rclcpp::spin_some(get_node_base_interface()); + if (status_->status == trajectory_tracker_msgs::msg::TrajectoryTrackerStatus::GOAL) break; if ((pos_local_goal - pos_).norm() < 0.1) @@ -537,56 +578,27 @@ TEST_F(TrajectoryTrackerTest, SwitchBackWithPathUpdate) } } } - ASSERT_GT(cnt_arrive_local_goal, 25) - << "failed to update path"; + ASSERT_GT(cnt_arrive_local_goal, 25) << "failed to update path"; for (int j = 0; j < 5; ++j) { for (int i = 0; i < 5; ++i) { publishTransform(); rate.sleep(); - ros::spinOnce(); + rclcpp::spin_some(get_node_base_interface()); } // Check multiple times to assert overshoot. - ASSERT_NEAR(yaw_, p[2], error_ang_) - << "[overshoot after goal (" << j << ")] "; - ASSERT_NEAR(pos_[0], p[0], error_large_lin_) - << "[overshoot after goal (" << j << ")] "; - ASSERT_NEAR(pos_[1], p[1], error_large_lin_) - << "[overshoot after goal (" << j << ")] "; + ASSERT_NEAR(yaw_, p[2], error_ang_) << "[overshoot after goal (" << j << ")] "; + ASSERT_NEAR(pos_[0], p[0], error_large_lin_) << "[overshoot after goal (" << j << ")] "; + ASSERT_NEAR(pos_[1], p[1], error_large_lin_) << "[overshoot after goal (" << j << ")] "; } ASSERT_EQ(last_path_header_.stamp, status_->path_header.stamp); } -void timeSource() -{ - ros::NodeHandle nh("/"); - bool use_sim_time; - nh.param("/use_sim_time", use_sim_time, false); - if (!use_sim_time) - return; - - ros::Publisher pub = nh.advertise("clock", 1); - - ros::WallRate rate(400.0); // 400% speed - ros::WallTime time = ros::WallTime::now(); - while (ros::ok()) - { - rosgraph_msgs::Clock clock; - clock.clock.fromNSec(time.toNSec()); - pub.publish(clock); - rate.sleep(); - time += ros::WallDuration(0.01); - } -} - int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "test_trajectory_tracker"); - - boost::thread time_thread(timeSource); - + rclcpp::init(argc, argv); return RUN_ALL_TESTS(); } diff --git a/trajectory_tracker/test/src/test_trajectory_tracker_overshoot.cpp b/trajectory_tracker/test/src/test_trajectory_tracker_overshoot.cpp index 89ffb135a..3d64fa469 100644 --- a/trajectory_tracker/test/src/test_trajectory_tracker_overshoot.cpp +++ b/trajectory_tracker/test/src/test_trajectory_tracker_overshoot.cpp @@ -30,15 +30,12 @@ #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) + 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); @@ -48,13 +45,12 @@ class TrajectoryTrackerOvershootTest : public TrajectoryTrackerTest 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)); + std::vector params; + params.emplace_back("goal_tolerance_lin_vel", goal_tolerance_lin_vel); + params.emplace_back("goal_tolerance_ang_vel", goal_tolerance_ang_vel); + ASSERT_TRUE(setConfig(params)); - nav_msgs::Odometry odom; + nav_msgs::msg::Odometry odom; odom.header.frame_id = "odom"; odom.child_frame_id = "base_link"; odom.pose.pose.position.x = 0.5; @@ -71,16 +67,17 @@ class TrajectoryTrackerOvershootTest : public TrajectoryTrackerTest 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()) + rclcpp::Rate rate(50); + const rclcpp::Time initial_time = now(); + const rclcpp::Time time_limit = initial_time + rclcpp::Duration::from_seconds(5.0); + while (rclcpp::ok() && time_limit > now()) { - odom.header.stamp = ros::Time::now(); + odom.header.stamp = now(); publishTransform(odom); rate.sleep(); - ros::spinOnce(); - if ((status_->header.stamp > initial_time + ros::Duration(0.5)) && (status_->status == expected_status)) + rclcpp::spin_some(get_node_base_interface()); + if ((rclcpp::Time(status_->header.stamp) > initial_time + rclcpp::Duration::from_seconds(0.5)) && + (status_->status == expected_status)) { return; } @@ -92,43 +89,44 @@ class TrajectoryTrackerOvershootTest : public TrajectoryTrackerTest TEST_F(TrajectoryTrackerOvershootTest, NoVelocityToleranceWithRemainingLinearVel) { SCOPED_TRACE("NoVelocityToleranceWithRemainingLinearVel"); - runTest(0.0, 0.0, 0.1, 0.0, trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL); + runTest(0.0, 0.0, 0.1, 0.0, trajectory_tracker_msgs::msg::TrajectoryTrackerStatus::GOAL); } TEST_F(TrajectoryTrackerOvershootTest, NoVelocityToleranceWithRemainingAngularVel) { SCOPED_TRACE("NoVelocityToleranceWithRemainingAngularVel"); - runTest(0.0, 0.0, 0.0, 0.1, trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL); + runTest(0.0, 0.0, 0.0, 0.1, trajectory_tracker_msgs::msg::TrajectoryTrackerStatus::GOAL); } TEST_F(TrajectoryTrackerOvershootTest, LinearVelocityToleranceWithRemainingLinearVel) { SCOPED_TRACE("LinearVelocityToleranceWithRemainingLinearVel"); - runTest(0.05, 0.0, 0.1, 0.0, trajectory_tracker_msgs::TrajectoryTrackerStatus::FOLLOWING); + runTest(0.05, 0.0, 0.1, 0.0, trajectory_tracker_msgs::msg::TrajectoryTrackerStatus::FOLLOWING); } TEST_F(TrajectoryTrackerOvershootTest, LinearVelocityToleranceWithRemainingAngularVel) { SCOPED_TRACE("LinearVelocityToleranceWithRemainingAngularVel"); - runTest(0.05, 0.0, 0.0, 0.1, trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL); + runTest(0.05, 0.0, 0.0, 0.1, trajectory_tracker_msgs::msg::TrajectoryTrackerStatus::GOAL); } TEST_F(TrajectoryTrackerOvershootTest, AngularrVelocityToleranceWithRemainingLinearVel) { SCOPED_TRACE("AngularrVelocityToleranceWithRemainingLinearVel"); - runTest(0.0, 0.05, 0.1, 0.0, trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL); + runTest(0.0, 0.05, 0.1, 0.0, trajectory_tracker_msgs::msg::TrajectoryTrackerStatus::GOAL); } TEST_F(TrajectoryTrackerOvershootTest, AngularrVelocityToleranceWithRemainingAngularVel) { SCOPED_TRACE("AngularrVelocityToleranceWithRemainingAngularVel"); - runTest(0.0, 0.05, 0.0, 0.1, trajectory_tracker_msgs::TrajectoryTrackerStatus::FOLLOWING); + runTest(0.0, 0.05, 0.0, 0.1, trajectory_tracker_msgs::msg::TrajectoryTrackerStatus::FOLLOWING); } int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "test_trajectory_tracker_overshoot"); + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("test_trajectory_tracker_overshoot"); return RUN_ALL_TESTS(); } diff --git a/trajectory_tracker/test/src/test_trajectory_tracker_with_odom.cpp b/trajectory_tracker/test/src/test_trajectory_tracker_with_odom.cpp index 805c09983..d2572f4d2 100644 --- a/trajectory_tracker/test/src/test_trajectory_tracker_with_odom.cpp +++ b/trajectory_tracker/test/src/test_trajectory_tracker_with_odom.cpp @@ -41,16 +41,16 @@ TEST_F(TrajectoryTrackerTest, FrameRate) poses.push_back(Eigen::Vector3d(0.5, 0.0, 0.0)); waitUntilStart(std::bind(&TrajectoryTrackerTest::publishPath, this, poses)); - ros::Rate rate(50); - const ros::Time start = ros::Time::now(); - while (ros::ok()) + rclcpp::Rate rate(50); + const rclcpp::Time start = now(); + while (rclcpp::ok()) { - ASSERT_LT(ros::Time::now() - start, ros::Duration(10.0)); + ASSERT_LT(now() - start, rclcpp::Duration::from_seconds(10.0)); publishTransform(); rate.sleep(); - ros::spinOnce(); - if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL) + rclcpp::spin_some(get_node_base_interface()); + if (status_->status == trajectory_tracker_msgs::msg::TrajectoryTrackerStatus::GOAL) break; } const double frame_rate = getCmdVelFrameRate(); @@ -58,7 +58,7 @@ TEST_F(TrajectoryTrackerTest, FrameRate) { publishTransform(); rate.sleep(); - ros::spinOnce(); + rclcpp::spin_some(get_node_base_interface()); } ASSERT_NEAR(yaw_, 0.0, 1e-2); ASSERT_NEAR(pos_[0], 0.5, 1e-2); @@ -78,36 +78,36 @@ TEST_F(TrajectoryTrackerTest, Timeout) poses.push_back(Eigen::Vector3d(2.0, 0.0, 0.0)); waitUntilStart(std::bind(&TrajectoryTrackerTest::publishPath, this, poses)); - ros::Rate rate(50); + rclcpp::Rate rate(50); for (int i = 0; i < 50; ++i) { publishTransform(); rate.sleep(); - ros::spinOnce(); + rclcpp::spin_some(get_node_base_interface()); } // Wait until odometry timeout - ros::Duration(0.2).sleep(); - ros::spinOnce(); + get_clock()->sleep_for(rclcpp::Duration::from_seconds(0.2)); + rclcpp::spin_some(get_node_base_interface()); ASSERT_FLOAT_EQ(cmd_vel_->linear.x, 0.0); ASSERT_FLOAT_EQ(cmd_vel_->angular.z, 0.0); ASSERT_GT(pos_[0], 0.0); ASSERT_LT(pos_[0], 2.0); - ASSERT_EQ(status_->status, trajectory_tracker_msgs::TrajectoryTrackerStatus::NO_PATH); + ASSERT_EQ(status_->status, trajectory_tracker_msgs::msg::TrajectoryTrackerStatus::NO_PATH); - while (ros::ok()) + while (rclcpp::ok()) { publishTransform(); rate.sleep(); - ros::spinOnce(); - if (status_->status == trajectory_tracker_msgs::TrajectoryTrackerStatus::GOAL) + rclcpp::spin_some(get_node_base_interface()); + if (status_->status == trajectory_tracker_msgs::msg::TrajectoryTrackerStatus::GOAL) break; } for (int i = 0; i < 25; ++i) { publishTransform(); rate.sleep(); - ros::spinOnce(); + rclcpp::spin_some(get_node_base_interface()); } ASSERT_NEAR(yaw_, 0.0, 1e-2); ASSERT_NEAR(pos_[0], 2.0, 1e-2); @@ -118,7 +118,8 @@ TEST_F(TrajectoryTrackerTest, Timeout) int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "test_trajectory_tracker_frame_rate"); + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("test_trajectory_tracker_frame_rate"); return RUN_ALL_TESTS(); } diff --git a/trajectory_tracker/test/src/time_source.cpp b/trajectory_tracker/test/src/time_source.cpp new file mode 100644 index 000000000..554c07f80 --- /dev/null +++ b/trajectory_tracker/test/src/time_source.cpp @@ -0,0 +1,63 @@ +#if 0 +void timeSource() +{ + rclcpp::Node nh("/"); + bool use_sim_time; + nh.param("/use_sim_time", use_sim_time, false); + if (!use_sim_time) + return; + + auto pub = nh.advertise("clock", 1); + + ros::WallRate rate(400.0); // 400% speed + ros::WallTime time = ros::WallTime::now(); + while (rclcpp::ok()) + { + rosgraph_msgs::msg::Clock clock; + clock.clock.fromNSec(time.toNSec()); + pub.publish(clock); + rate.sleep(); + time += ros::WallDuration(0.01); + } +} +#endif + +#include +#include +#include + +using namespace std::chrono_literals; + +class TimeSource : public rclcpp::Node +{ +public: + TimeSource() + : Node("sim_time_publisher") + , sim_time_(0) + { + clock_publisher_ = this->create_publisher("/clock", 1); + timer_ = this->create_wall_timer(2.5ms, std::bind(&TimeSource::publish_sim_time, this)); + } + +private: + void publish_sim_time() + { + rosgraph_msgs::msg::Clock clock; + clock.clock = rclcpp::Time(sim_time_); + clock_publisher_->publish(clock); + sim_time_ += rclcpp::Duration(10ms); + } + + rclcpp::Publisher::SharedPtr clock_publisher_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Time sim_time_; +}; + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/trajectory_tracker/test/test/test_trajectory_tracker_launch.py b/trajectory_tracker/test/test/test_trajectory_tracker_launch.py new file mode 100644 index 000000000..5435b426d --- /dev/null +++ b/trajectory_tracker/test/test/test_trajectory_tracker_launch.py @@ -0,0 +1,91 @@ +import os +import unittest +from typing import Tuple + +import launch_testing.markers +import pytest +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_entity import LaunchDescriptionEntity +from launch.substitutions import LaunchConfiguration, TextSubstitution +from launch_ros.actions import Node +from launch_ros.descriptions import ParameterFile +from launch_testing.actions import ReadyToTest +from launch_testing.proc_info_handler import ActiveProcInfoHandler +from nav2_common.launch import RewrittenYaml # type: ignore[attr-defined] + + +@pytest.mark.launch_test +@launch_testing.markers.keep_alive # type: ignore[misc] +def generate_test_description() -> Tuple[LaunchDescription, dict[str, LaunchDescriptionEntity]]: + use_sim_time = LaunchConfiguration("use_sim_time", default="false") + node_log_level = [ + TextSubstitution(text="trajectory_tracker:="), + LaunchConfiguration("node_log_level", default="info"), + ] + params_file = os.path.join( + get_package_share_directory("trajectory_tracker"), # type: ignore[no-untyped-call] + "test", + "configs", + "test_params.yaml", + ) + odom_delay = LaunchConfiguration("odom_delay", default="0.0") + use_odom = LaunchConfiguration("use_odom", default="false") + use_time_optimal_control = LaunchConfiguration("use_time_optimal_control", default="true") + param_substitutions = { + "odom_delay": odom_delay, + "use_odom": use_odom, + "use_time_optimal_control": use_time_optimal_control, + } + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + param_rewrites=param_substitutions, + convert_types=True, + ), + allow_substs=True, + ) + trajectory_tracker_cmd = Node( + package="trajectory_tracker", + executable="trajectory_tracker", + name="trajectory_tracker", + output="screen", + arguments=["--ros-args", "--log-level", node_log_level], + parameters=[configured_params, {"use_sim_time": use_sim_time}], + ) + test_cmd = Node( + package="trajectory_tracker", + executable="test_trajectory_tracker", + name="test_trajectory_tracker", + output="screen", + arguments=["--ros-args", "--log-level", "info"], + parameters=[configured_params, {"use_sim_time": use_sim_time}], + ) + time_soure_cmd = Node( + package="trajectory_tracker", + executable="time_source", + name="time_source", + output="screen", + arguments=["--ros-args", "--log-level", "info"], + parameters=[configured_params], + condition=IfCondition(use_sim_time), + ) + + return LaunchDescription( + [ + trajectory_tracker_cmd, + test_cmd, + time_soure_cmd, + ReadyToTest(), # type: ignore[no-untyped-call] + ] + ), {"test_cmd": test_cmd} + + +class TestTerminatingProcessStops(unittest.TestCase): + + def test_proc_terminates(self, proc_info: ActiveProcInfoHandler, test_cmd: Node) -> None: + proc_info.assertWaitForShutdown( # type: ignore[no-untyped-call] + process=test_cmd, timeout=120.0 + ) + self.assertEqual(proc_info[test_cmd].returncode, 0) diff --git a/trajectory_tracker/test/test/test_trajectory_tracker_overshoot_launch.py b/trajectory_tracker/test/test/test_trajectory_tracker_overshoot_launch.py new file mode 100644 index 000000000..7636d84b8 --- /dev/null +++ b/trajectory_tracker/test/test/test_trajectory_tracker_overshoot_launch.py @@ -0,0 +1,81 @@ +import unittest +from typing import Tuple + +import launch_testing.markers +import pytest +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_entity import LaunchDescriptionEntity +from launch.substitutions import ( + LaunchConfiguration, + PathJoinSubstitution, + TextSubstitution, +) +from launch_ros.actions import Node +from launch_testing.actions import ReadyToTest +from launch_testing.proc_info_handler import ActiveProcInfoHandler + + +@pytest.mark.launch_test +@launch_testing.markers.keep_alive # type: ignore[misc] +def generate_test_description() -> Tuple[LaunchDescription, dict[str, LaunchDescriptionEntity]]: + use_sim_time = LaunchConfiguration("use_sim_time", default="false") + node_log_level = [ + TextSubstitution(text="trajectory_tracker:="), + LaunchConfiguration("node_log_level", default="info"), + ] + params_file = PathJoinSubstitution( + [ + get_package_share_directory("trajectory_tracker"), # type: ignore[no-untyped-call] + "test", + "configs", + LaunchConfiguration("param_file", default="test_overshoot_params.yaml"), + ] + ) + test_executable = LaunchConfiguration( + "test_executable", default="test_trajectory_tracker_overshoot" + ) + trajectory_tracker_cmd = Node( + package="trajectory_tracker", + executable="trajectory_tracker", + name="trajectory_tracker", + output="screen", + arguments=["--ros-args", "--log-level", node_log_level], + parameters=[params_file, {"use_sim_time": use_sim_time}], + ) + test_cmd = Node( + package="trajectory_tracker", + executable=test_executable, + name=test_executable, + output="screen", + arguments=["--ros-args", "--log-level", "info"], + parameters=[params_file, {"use_sim_time": use_sim_time}], + ) + time_soure_cmd = Node( + package="trajectory_tracker", + executable="time_source", + name="time_source", + output="screen", + arguments=["--ros-args", "--log-level", "info"], + parameters=[params_file], + condition=IfCondition(use_sim_time), + ) + + return LaunchDescription( + [ + trajectory_tracker_cmd, + test_cmd, + time_soure_cmd, + ReadyToTest(), # type: ignore[no-untyped-call] + ] + ), {"test_cmd": test_cmd} + + +class TestTerminatingProcessStops(unittest.TestCase): + + def test_proc_terminates(self, proc_info: ActiveProcInfoHandler, test_cmd: Node) -> None: + proc_info.assertWaitForShutdown( # type: ignore[no-untyped-call] + process=test_cmd, timeout=60.0 + ) + self.assertEqual(proc_info[test_cmd].returncode, 0)