diff --git a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp index ebb7fb6f..40e8472d 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp @@ -37,7 +37,7 @@ #include "easynav_core/ControllerMethodBase.hpp" #include "easynav_common/types/NavState.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "easynav_mpc_controller/MPCOptimizer.hpp" diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp index 36116946..ac825d4c 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -142,7 +142,9 @@ MPCController::update_rt(NavState & nav_state) } } - if (!nav_state.has("path") || !nav_state.has("robot_pose") || !nav_state.has("points")) { + const auto & perceptions = nav_state.get_no_group(); + + if (!nav_state.has("path") || !nav_state.has("robot_pose") || perceptions.empty()) { if(verbose_) { std::cout << "No Path, No Points or No Robot Pose" << std::endl; } @@ -208,8 +210,6 @@ MPCController::update_rt(NavState & nav_state) local_horizon = num_elements - 1; } const auto & last_pose = path.poses[local_horizon].pose.position; - - const auto & perceptions = nav_state.get("points"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); const auto & filtered = PointPerceptionsOpsView(perceptions) diff --git a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp index c0fc3471..f80ca7ff 100644 --- a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp +++ b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp @@ -17,7 +17,7 @@ /// \brief Implementation of the MPPIController class. #include "easynav_mppi_controller/MPPIController.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "easynav_common/RTTFBuffer.hpp" #include "easynav_system/GoalManager.hpp" @@ -182,7 +182,7 @@ MPPIController::update_rt(NavState & nav_state) } const auto & pose = nav_state.get("robot_pose").pose.pose; - const auto & perceptions = nav_state.get("points"); + const auto & perceptions = nav_state.get_no_group(); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); const auto & filtered = PointPerceptionsOpsView(perceptions) .filter({-1.0, -1.0, -1.0}, {1.0, 1.0, 1.0}) diff --git a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp index cfed5193..9a32f1ff 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp @@ -23,7 +23,7 @@ #include "tf2/utils.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "easynav_common/RTTFBuffer.hpp" #include "easynav_serest_controller/SerestController.hpp" @@ -295,9 +295,9 @@ SerestController::closest_obstacle_distance( } // 2) Analyze distance sensors - if (!nav_state.has("points")) {return std::numeric_limits::infinity();} + if (!nav_state.has_group("points")) {return std::numeric_limits::infinity();} - const auto & perceptions = nav_state.get("points"); + const auto & perceptions = nav_state.get_no_group(); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); auto view = PointPerceptionsOpsView(perceptions); @@ -309,8 +309,14 @@ SerestController::closest_obstacle_distance( .downsample(0.3); const auto & fused = view.as_points(); - if (last_input_ts_ < view.get_latest_stamp()) { - last_input_ts_ = view.get_latest_stamp(); + { + const auto view_latest = view.get_latest_stamp(); + const rclcpp::Time view_latest_same_clock( + view_latest.nanoseconds(), + last_input_ts_.get_clock_type()); + if (last_input_ts_ < view_latest_same_clock) { + last_input_ts_ = view_latest_same_clock; + } } double min_dist = std::numeric_limits::infinity(); @@ -378,10 +384,10 @@ SerestController::fetch_required_inputs( odom = nav_state.get("robot_pose"); if (rclcpp::Time(path.header.stamp, last_input_ts_.get_clock_type()) > last_input_ts_) { - last_input_ts_ = path.header.stamp; + last_input_ts_ = rclcpp::Time(path.header.stamp, last_input_ts_.get_clock_type()); } if (rclcpp::Time(odom.header.stamp, last_input_ts_.get_clock_type()) > last_input_ts_) { - last_input_ts_ = odom.header.stamp; + last_input_ts_ = rclcpp::Time(odom.header.stamp, last_input_ts_.get_clock_type()); } if (path.poses.empty()) { diff --git a/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp b/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp index aed490e7..6e80af64 100644 --- a/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp +++ b/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp @@ -111,7 +111,10 @@ SimpleController::update_rt(NavState & nav_state) const auto & pose = nav_state.get("robot_pose").pose.pose; const auto & goal_pose = path.poses.back().pose; - rclcpp::Time latest_stamp = nav_state.get("robot_pose").header.stamp; + const auto clock_type = get_node()->get_clock()->get_clock_type(); + rclcpp::Time latest_stamp( + nav_state.get("robot_pose").header.stamp, + clock_type); if (rclcpp::Time(path.poses.back().header.stamp, latest_stamp.get_clock_type()) > latest_stamp) { diff --git a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp index 65d5e626..1fe63e46 100644 --- a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp +++ b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp @@ -18,7 +18,7 @@ #include "easynav_vff_controller/VffController.hpp" #include "easynav_common/types/NavState.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "nav_msgs/msg/odometry.hpp" #include "nav_msgs/msg/goals.hpp" @@ -252,7 +252,7 @@ void VffController::update_rt(NavState & nav_state) // Calculate the angle error double angle_error = normalize_angle(bearing - yaw); - const auto & perceptions = nav_state.get("points"); + const auto & perceptions = nav_state.get_no_group(); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); auto fused = diff --git a/localizers/easynav_costmap_localizer/CMakeLists.txt b/localizers/easynav_costmap_localizer/CMakeLists.txt index aabb1b11..15c2642b 100644 --- a/localizers/easynav_costmap_localizer/CMakeLists.txt +++ b/localizers/easynav_costmap_localizer/CMakeLists.txt @@ -58,7 +58,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) - # add_subdirectory(tests) + add_subdirectory(tests) endif() ament_export_include_directories("include/${PROJECT_NAME}") diff --git a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp index 30c3bbbb..7c26a2c0 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -22,7 +22,7 @@ #include "tf2/LinearMath/Vector3.hpp" #include "easynav_common/RTTFBuffer.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "easynav_costmap_common/costmap_2d.hpp" #include "easynav_costmap_common/cost_values.hpp" @@ -566,13 +566,12 @@ AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state) void AMCLLocalizer::correct(NavState & nav_state) { - if (!nav_state.has("points")) { - RCLCPP_WARN(get_node()->get_logger(), "There is yet no points perceptions"); + const auto & perceptions = nav_state.get_no_group(); + if (perceptions.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions"); return; } - const auto & perceptions = nav_state.get("points"); - if (!nav_state.has("map.static")) { RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.static map"); return; diff --git a/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp b/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp index bc03cbe6..6259a598 100644 --- a/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp +++ b/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp @@ -15,143 +15,169 @@ #include +#include +#include +#include +#include +#include + #include "easynav_costmap_localizer/AMCLLocalizer.hpp" -#include "easynav_costmap_common/costmap_2d.hpp" +#include "easynav_localizer/LocalizerNode.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "std_srvs/srv/trigger.hpp" - -#include -#include +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -/// \brief Fixture for AMCLLocalizer tests (minimal) -class AMCLLocalizerTest : public ::testing::Test +namespace { -protected: - void SetUp() override - { - rclcpp::init(0, nullptr); - } - void TearDown() override - { - rclcpp::shutdown(); - } -}; - -TEST_F(AMCLLocalizerTest, BasicDynamicUpdate) +double yaw_from_quat(const geometry_msgs::msg::Quaternion & q) { - auto node = std::make_shared("test_node"); - auto manager = std::make_shared(); - manager->initialize(node, "test"); - - auto static_map = std::make_shared(); - static_map->resizeMap(30, 30, 0.1, -1.5, -1.5); - static_map->setCost(15, 15, 254); // Occupied cell - manager->set_static_map(static_map); - - easynav::NavState navstate; - auto perception = std::make_shared(); - perception->data.points.resize(2); - perception->data.points[0].x = 0.0; - perception->data.points[0].y = 0.0; - perception->data.points[0].z = 0.2; - perception->data.points[1].x = 1.5; - perception->data.points[1].y = 1.5; - perception->data.points[1].z = 0.2; - perception->stamp = rclcpp::Time(0); - perception->frame_id = "map"; - perception->valid = true; - - navstate.set("perceptions", easynav::Perceptions()); - navstate.get_mutable("perceptions")->push_back(perception); - navstate.set("map.static", *static_map); - - manager->update(navstate); - - auto map_ptr = std::dynamic_pointer_cast(manager->get_static_map()); - ASSERT_TRUE(map_ptr != nullptr); - EXPECT_EQ(map_ptr->getCost(15, 15), 254u); // Cell (15, 15) + tf2::Quaternion tf_q(q.x, q.y, q.z, q.w); + double roll = 0.0, pitch = 0.0, yaw = 0.0; + tf2::Matrix3x3(tf_q).getRPY(roll, pitch, yaw); + return yaw; } -TEST_F(AMCLLocalizerTest, IncomingOccupancyGridUpdatesMaps) +double yaw_from_tf(const tf2::Transform & tf) { - auto node = std::make_shared("test_node2"); - auto manager = std::make_shared(); - manager->initialize(node, "test2"); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); - - auto pub = node->create_publisher( - "test_node2/test2/incoming_map", rclcpp::QoS(1).transient_local().reliable()); - - nav_msgs::msg::OccupancyGrid grid; - grid.header.frame_id = "map"; - grid.info.width = 10; - grid.info.height = 10; - grid.info.resolution = 0.2; - grid.info.origin.position.x = -1.0; - grid.info.origin.position.y = -0.6; - grid.data.assign(100, 0); - grid.data[55] = 100; - - pub->publish(grid); - - executor.spin_some(); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - - auto static_map = std::dynamic_pointer_cast(manager->get_static_map()); - ASSERT_TRUE(static_map != nullptr); - EXPECT_EQ(static_map->getCost(5, 5), 254u); + double roll = 0.0, pitch = 0.0, yaw = 0.0; + tf2::Matrix3x3(tf.getRotation()).getRPY(roll, pitch, yaw); + return yaw; } -class FriendAMCLLocalizer : public easynav::AMCLLocalizer { +class FriendAMCLLocalizer : public easynav::AMCLLocalizer +{ public: - void force_path(const std::string & path) {map_path_ = path;} + using easynav::AMCLLocalizer::init_pose_sub_; }; -TEST_F(AMCLLocalizerTest, SavemapServiceWorks) +class AMCLLocalizerInitialPoseTest : public ::testing::Test { - auto node = std::make_shared("test_savemap_node"); - auto manager = std::make_shared(); - manager->initialize(node, "test_savemap"); - - auto static_map = std::make_shared(); - static_map->resizeMap(4, 4, 0.5, -1.0, -1.0); - static_map->setCost(1, 1, 254); - static_map->setCost(2, 2, 254); - manager->set_static_map(static_map); - - const std::string test_map_file = "/tmp/savemap_test_map"; - std::static_pointer_cast(manager)->force_path(test_map_file); +protected: + void SetUp() override {rclcpp::init(0, nullptr);} + void TearDown() override {rclcpp::shutdown();} +}; - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); +} // namespace - auto client = node->create_client( - "/test_savemap_node/test_savemap/savemap"); +TEST_F(AMCLLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackGroup) +{ + const double x0 = 1.25; + const double y0 = -2.5; + const double yaw0 = 0.4; + + const double x1 = -0.75; + const double y1 = 0.9; + const double yaw1 = -1.2; + + rclcpp::NodeOptions options; + options.parameter_overrides({ + rclcpp::Parameter("test.num_particles", 100), + rclcpp::Parameter("test.initial_pose.x", x0), + rclcpp::Parameter("test.initial_pose.y", y0), + rclcpp::Parameter("test.initial_pose.yaw", yaw0), + rclcpp::Parameter("test.initial_pose.std_dev_xy", 0.0), + rclcpp::Parameter("test.initial_pose.std_dev_yaw", 0.0), + rclcpp::Parameter("test.min_noise_xy", 0.0), + rclcpp::Parameter("test.min_noise_yaw", 0.0), + rclcpp::Parameter("test.compute_odom_from_tf", true), + }); + + auto node = std::make_shared(options); + auto localizer = std::make_shared(); + + localizer->initialize(node, "test"); + + ASSERT_NE(localizer->init_pose_sub_, nullptr); - ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); + { + const tf2::Transform tf = localizer->getEstimatedPose(); + EXPECT_NEAR(tf.getOrigin().x(), x0, 1e-9); + EXPECT_NEAR(tf.getOrigin().y(), y0, 1e-9); + EXPECT_NEAR(yaw_from_tf(tf), yaw0, 1e-9); + } - auto request = std::make_shared(); - auto future = client->async_send_request(request); - executor.spin_until_future_complete(future); + { + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + EXPECT_NEAR(odom.pose.pose.position.x, x0, 1e-9); + EXPECT_NEAR(odom.pose.pose.position.y, y0, 1e-9); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw0, 1e-9); + } - auto response = future.get(); - EXPECT_TRUE(response->success); - EXPECT_NE(response->message.find("saved"), std::string::npos); + const auto infos = node->get_subscriptions_info_by_topic("initialpose"); + ASSERT_FALSE(infos.empty()); + + const bool has_expected_type = std::any_of( + infos.begin(), infos.end(), + [](const rclcpp::TopicEndpointInfo & info) { + return info.topic_type() == "geometry_msgs/msg/PoseWithCovarianceStamped"; + }); + EXPECT_TRUE(has_expected_type); + + auto pub_node = std::make_shared("initialpose_pub_node"); + auto pub = pub_node->create_publisher( + "initialpose", 10); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(node->get_node_base_interface()); + exec.add_node(pub_node->get_node_base_interface()); + + geometry_msgs::msg::PoseWithCovarianceStamped msg; + msg.header.stamp = pub_node->now(); + msg.header.frame_id = "map"; + msg.pose.pose.position.x = x1; + msg.pose.pose.position.y = y1; + msg.pose.pose.position.z = 0.0; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, yaw1); + msg.pose.pose.orientation = tf2::toMsg(q); + msg.pose.covariance.fill(0.0); - std::ifstream yamlfile(test_map_file + ".yaml"); - ASSERT_TRUE(yamlfile.is_open()); + { + const auto connect_deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(200); + while (std::chrono::steady_clock::now() < connect_deadline && + pub->get_subscription_count() == 0) + { + exec.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + pub->publish(msg); + + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500); + while (std::chrono::steady_clock::now() < deadline) { + exec.spin_some(); + const tf2::Transform tf = localizer->getEstimatedPose(); + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + + const bool pose_ok = + std::abs(tf.getOrigin().x() - x1) < 1e-9 && + std::abs(tf.getOrigin().y() - y1) < 1e-9 && + std::abs(yaw_from_tf(tf) - yaw1) < 1e-9 && + std::abs(odom.pose.pose.position.x - x1) < 1e-9 && + std::abs(odom.pose.pose.position.y - y1) < 1e-9 && + std::abs(yaw_from_quat(odom.pose.pose.orientation) - yaw1) < 1e-9; + + if (pose_ok) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } - std::string line; - std::getline(yamlfile, line); - EXPECT_NE(line.find("image:"), std::string::npos); - yamlfile.close(); + { + const tf2::Transform tf = localizer->getEstimatedPose(); + EXPECT_NEAR(tf.getOrigin().x(), x1, 1e-9); + EXPECT_NEAR(tf.getOrigin().y(), y1, 1e-9); + EXPECT_NEAR(yaw_from_tf(tf), yaw1, 1e-9); + } - std::remove((test_map_file + ".yaml").c_str()); - std::remove((test_map_file + ".pgm").c_str()); + { + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + EXPECT_NEAR(odom.pose.pose.position.x, x1, 1e-9); + EXPECT_NEAR(odom.pose.pose.position.y, y1, 1e-9); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw1, 1e-9); + } } diff --git a/localizers/easynav_fusion_localizer/CMakeLists.txt b/localizers/easynav_fusion_localizer/CMakeLists.txt index 4db3bed8..c0755cd4 100644 --- a/localizers/easynav_fusion_localizer/CMakeLists.txt +++ b/localizers/easynav_fusion_localizer/CMakeLists.txt @@ -72,11 +72,18 @@ install(TARGETS RUNTIME DESTINATION lib/${PROJECT_NAME} ) +install(DIRECTORY maps/ + DESTINATION share/${PROJECT_NAME}/maps +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) set(ament_cmake_copyright_FOUND TRUE) set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(tests) endif() ament_export_include_directories(include) diff --git a/localizers/easynav_fusion_localizer/README.md b/localizers/easynav_fusion_localizer/README.md index e48542e7..c641ef72 100644 --- a/localizers/easynav_fusion_localizer/README.md +++ b/localizers/easynav_fusion_localizer/README.md @@ -1,4 +1,4 @@ -# easynav_gps_localizer +# easynav_fusion_localizer [![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) @@ -23,15 +23,23 @@ Odometry fusion localizer based on Robot Localization that fuses any n odometry ## Parameters -TODO +All parameters are declared under the plugin namespace, i.e., `//easynav_fusion_localizer/FusionLocalizer/...`. -See [robot_localization documentation](https://docs.ros.org/en/melodic/api/robot_localization/html/configuring_robot_localization.html) +| Name | Type | Default | Description | +|---|---|---:|---| +| `.initial_pose.x` | `double` | `0.0` | Initial X position (m). | +| `.initial_pose.y` | `double` | `0.0` | Initial Y position (m). | +| `.initial_pose.yaw` | `double` | `0.0` | Initial yaw (rad). | + +For additional robot_localization-related configuration, see the upstream documentation: +https://docs.ros.org/en/melodic/api/robot_localization/html/configuring_robot_localization.html ## Interfaces (Topics and Services) ### Subscriptions and Publications | Direction | Topic | Type | Purpose | QoS | |---|---|---|---|---| +| Subscription | `initialpose` | `geometry_msgs/msg/PoseWithCovarianceStamped` | Seed/reset the filter pose from an external initial pose estimate. | depth=10 | | Publisher | `odometry/filtered` | `nav_msgs/msg/Odometry` | Odometry fused from all sources. | SensorDataQoS | @@ -41,13 +49,13 @@ This package does not create service servers or clients. ## NavState Keys | Key | Type | Access | Notes | |---|---|---|---| -| `robot_pose` | `nav_msgs::msg::Odometry` | **Write** | GPS-based odometry estimate. | +| `robot_pose` | `nav_msgs::msg::Odometry` | **Write** | Fused odometry estimate. | ## TF Frames | Role | Transform | Notes | |---|---|---| -| Publishes | `map -> odom` | Static or slowly varying transform aligning UTM to local odometry frame. | +| Publishes | `map -> odom` | Transform that aligns the odometry frame with the global frame. | ## License diff --git a/localizers/easynav_fusion_localizer/config/example_params.yaml b/localizers/easynav_fusion_localizer/config/example_params.yaml index 9c47f30b..905be4e5 100644 --- a/localizers/easynav_fusion_localizer/config/example_params.yaml +++ b/localizers/easynav_fusion_localizer/config/example_params.yaml @@ -25,7 +25,7 @@ localizer_node: latitude_origin: 40.2834931 longitude_origin: -3.8207253999999997 altitude_origin: 746.184 - local_filter: + global_filter: two_d_mode: true publish_tf: true diff --git a/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml new file mode 100644 index 00000000..40b6d005 --- /dev/null +++ b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml @@ -0,0 +1,280 @@ +controller_node: + ros__parameters: + use_sim_time: true + controller_types: [serest] + dummy: + rt_freq: 30.0 + plugin: easynav_controller/DummyController + cycle_time_nort: 0.01 + cycle_time_rt: 0.0001 + mppi: + rt_freq: 30.0 + plugin: easynav_mppi_controller/MPPIController + num_samples: 200 + horizon_steps: 5 + dt: 0.15 + lambda: 1.0 + fov: 1.57 + max_linear_velocity: 1.5 + max_angular_velocity: 1.0 + max_linear_acceleration: 0.5 + max_angular_acceleration: 0.5 + safety_radius: 0.3 + serest: + rt_freq: 30.0 + plugin: easynav_serest_controller/SerestController + allow_reverse: true + max_linear_speed: 0.8 + max_angular_speed: 1.2 + v_progress_min: 0.08 # 8 cm/s de crucero mínimo cuando alineado + k_s_share_max: 0.5 # succión lateral no cancela >50% del avance + k_theta: 2.5 # sube un poco + k_y: 1.5 + goal_pos_tol: 0.1 # 7 cm + goal_yaw_tol_deg: 6.0 # 6 grados + slow_radius: 0.60 + slow_min_speed: 0.03 + final_align_k: 2.0 + final_align_wmax: 0.6 + corner_guard_enable: true + corner_gain_ey: 1.8 + corner_gain_eth: 0.7 + corner_gain_kappa: 0.4 + corner_min_alpha: 0.35 + corner_boost_omega: 1.0 + a_lat_soft: 0.9 + apex_ey_des: 0.05 + +localizer_node: + ros__parameters: + use_sim_time: true + localizer_types: [Ukf] + dummy: + rt_freq: 30.0 + plugin: easynav_localizer/DummyLocalizer + cycle_time_nort: 0.01 + cycle_time_rt: 0.001 + Ukf: + rt_freq: 50.0 + freq: 5.0 + reseed_freq: 0.1 + plugin: easynav_fusion_localizer/FusionLocalizer + latitude_origin: 40.2834931 + longitude_origin: -3.8207253999999997 + altitude_origin: 746.184 + local_filter: + two_d_mode: true + publish_tf: true + + # --- Reference Frames --- + map_frame: map + odom_frame: odom + base_link_frame: base_link + world_frame: odom + + dynamic_process_noise_covariance: true + + #--- INPUT 2: Wheel Odometry --- + odom0: /robotnik_base_control/odom + odom0_config: + [true, true, false, + false, false, true, + true, true, false, + false, false, true, + false, false, false] + odom0_differential: false + + # --- INPUT 3: IMU --- + # imu0: /imu/data + # imu0_config: + # [false, false, false, + # false, false, false, + # false, false, false, + # false, false, true, + # true, true, true] + # imu0_differential: false + # imu0_remove_gravitational_acceleration: true + + # twist0: /gps/fix_velocity + # twist0_config: + # [false, false, false, + # false, false, false, + # true, false, false, + # false, false, false, + # false, false, false] + # twist0_differential: false + + # --- Process Variances --- + process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015] + global_filter: + two_d_mode: true + publish_tf: true + debug: true + debug_out_file: /tmp/ukf_global_debug.txt + transform_timeout: 0.1 + smooth_lagged_data: true + # Defines the time window in seconds to keep past states in memory. + # history_length: 5.0 + dynamic_process_noise_covariance: true + + # --- Reference Frames --- + map_frame: map + odom_frame: odom + base_link_frame: base_link + world_frame: map + + # --- INPUT 1: GPS Odometry --- + gps0: /gps/fix + gps0_config: + [true, true, false, + false, false, false, + false, false, false, + false, false, false, + false, false, false] + gps0_differential: false + + # --- INPUT 2: Wheel Odometry --- + odom0: /robotnik_base_control/odom + odom0_config: + [false, false, false, + false, false, false, + true, true, false, + false, false, true, + false, false, false] + odom0_differential: false + + # --- INPUT 3: IMU --- + imu0: /imu/data + imu0_config: + [false, false, false, + false, false, false, + false, false, false, + false, false, true, + false, false, false] + imu0_differential: false + imu0_remove_gravitational_acceleration: true + + # twist0: /gps/fix_velocity + # twist0_config: + # [false, false, false, + # false, false, false, + # true, true, false, + # false, false, false, + # false, false, false] + # twist0_differential: false + + # --- Process Variances --- + process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015] + + initial_estimate_covariance: [ + 5.0, 5.0, 1e-3, 1e-3, 1e-3, 1.0, + 0.5, 0.5, 1e-3, 1e-3, 1e-3, 0.1, + 0.1, 0.1, 0.1 + ] + + +maps_manager_node: + ros__parameters: + use_sim_time: true + map_types: [costmap] + routes: + freq: 10.0 + plugin: easynav_routes_maps_manager/RoutesMapsManager + filters: [routes_costmap] + package: easynav_fusion_localizer + map_path_file: routes/simple_square.yaml + routes_costmap: + plugin: easynav_routes_maps_manager/RoutesCostmapFilter + min_cost: 50 + route_width: 0.5 + cycle_time_nort: 0.001 + cycle_time_rt: 0.001 + costmap: + freq: 10.0 + plugin: easynav_costmap_maps_manager/CostmapMapsManager + package: easynav_fusion_localizer + map_path_file: maps/square.yaml + filters: [obstacles, inflation] + obstacles: + plugin: easynav_costmap_maps_manager/CostmapMapsManager/ObstaclesFilter + inflation: + plugin: easynav_costmap_maps_manager/CostmapMapsManager/InflationFilter + inflation_radius: 1.3 + inscribed_radius: 0.25 + cost_scaling_factor: 3.0 + dummy: + freq: 10.0 + plugin: easynav_maps_manager/DummyMapsManager + cycle_time_nort: 0.1 + cycle_time_rt: 0.0001 + +planner_node: + ros__parameters: + use_sim_time: true + planner_types: [costmap] + dummy: + freq: 1.0 + plugin: easynav_planner/DummyPlanner + cycle_time_nort: 0.2 + cycle_time_rt: 0.0001 + costmap: + freq: 10.0 + plugin: easynav_costmap_planner/CostmapPlanner + cost_factor: 10.0 + continuous_replan: true + simple: + freq: 0.5 + plugin: easynav_simple_planner/SimplePlanner + robot_radius: 0.3 + +sensors_node: + ros__parameters: + use_sim_time: true + forget_time: 0.5 + sensors: [imu, gps] + perception_default_frame: odom + imu: + topic: /imu/data + type: sensor_msgs/msg/Imu + gps: + topic: /gps/fix + type: sensor_msgs/msg/NavSatFix + laser1: + topic: /front_laser/points + type: sensor_msgs/msg/PointCloud2 + group: points + + +system_node: + ros__parameters: + use_sim_time: true + position_tolerance: 0.75 + angle_tolerance: 0.25 diff --git a/localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml b/localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml new file mode 100644 index 00000000..7ec21016 --- /dev/null +++ b/localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml @@ -0,0 +1,281 @@ +controller_node: + ros__parameters: + use_sim_time: true + controller_types: [serest] + dummy: + rt_freq: 30.0 + plugin: easynav_controller/DummyController + cycle_time_nort: 0.01 + cycle_time_rt: 0.0001 + mppi: + rt_freq: 30.0 + plugin: easynav_mppi_controller/MPPIController + num_samples: 200 + horizon_steps: 5 + dt: 0.15 + lambda: 1.0 + fov: 1.57 + max_linear_velocity: 1.5 + max_angular_velocity: 1.0 + max_linear_acceleration: 0.5 + max_angular_acceleration: 0.5 + safety_radius: 0.3 + serest: + rt_freq: 30.0 + plugin: easynav_serest_controller/SerestController + allow_reverse: true + max_linear_speed: 0.8 + max_angular_speed: 1.2 + v_progress_min: 0.08 # 8 cm/s de crucero mínimo cuando alineado + k_s_share_max: 0.5 # succión lateral no cancela >50% del avance + k_theta: 2.5 # sube un poco + k_y: 1.5 + goal_pos_tol: 0.1 # 7 cm + goal_yaw_tol_deg: 6.0 # 6 grados + slow_radius: 0.60 + slow_min_speed: 0.03 + final_align_k: 2.0 + final_align_wmax: 0.6 + corner_guard_enable: true + corner_gain_ey: 1.8 + corner_gain_eth: 0.7 + corner_gain_kappa: 0.4 + corner_min_alpha: 0.35 + corner_boost_omega: 1.0 + a_lat_soft: 0.9 + apex_ey_des: 0.05 + +localizer_node: + ros__parameters: + use_sim_time: true + localizer_types: [Ukf] + dummy: + rt_freq: 30.0 + plugin: easynav_localizer/DummyLocalizer + cycle_time_nort: 0.01 + cycle_time_rt: 0.001 + Ukf: + rt_freq: 50.0 + freq: 5.0 + reseed_freq: 0.1 + plugin: easynav_fusion_localizer/FusionLocalizer + latitude_origin: 40.284796759605115 + longitude_origin: -3.8214099132625017 + altitude_origin: 669.9730248888955 + local_filter: + two_d_mode: true + publish_tf: false + + # --- Reference Frames --- + map_frame: map + odom_frame: odom + base_link_frame: base_link + world_frame: odom + + dynamic_process_noise_covariance: true + + #--- INPUT 2: Wheel Odometry --- + odom0: /robotnik_base_control/odom + odom0_config: + [true, true, false, + false, false, true, + true, true, false, + false, false, true, + false, false, false] + odom0_differential: false + + # --- INPUT 3: IMU --- + # imu0: /imu/data + # imu0_config: + # [false, false, false, + # false, false, false, + # false, false, false, + # false, false, true, + # true, true, true] + # imu0_differential: false + # imu0_remove_gravitational_acceleration: true + + # twist0: /gps/fix_velocity + # twist0_config: + # [false, false, false, + # false, false, false, + # true, false, false, + # false, false, false, + # false, false, false] + # twist0_differential: false + + # --- Process Variances --- + process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015] + global_filter: + two_d_mode: true + publish_tf: true + debug: true + debug_out_file: /tmp/ukf_global_debug.txt + transform_timeout: 0.1 + smooth_lagged_data: true + # Defines the time window in seconds to keep past states in memory. + # history_length: 5.0 + dynamic_process_noise_covariance: true + + # --- Reference Frames --- + map_frame: map + odom_frame: odom + base_link_frame: base_link + world_frame: map + + # --- INPUT 1: GPS Odometry --- + gps0: /gps/fix + gps0_config: + [true, true, false, + false, false, false, + false, false, false, + false, false, false, + false, false, false] + gps0_differential: false + + # --- INPUT 2: Wheel Odometry --- + odom0: /robotnik_base_control/odom + odom0_config: + [false, false, false, + false, false, false, + true, true, false, + false, false, true, + false, false, false] + odom0_differential: false + + # --- INPUT 3: IMU --- + imu0: /imu/data + imu0_config: + [false, false, false, + false, false, false, + false, false, false, + false, false, true, + false, false, false] + imu0_differential: false + imu0_remove_gravitational_acceleration: true + + # twist0: /gps/fix_velocity + # twist0_config: + # [false, false, false, + # false, false, false, + # true, true, false, + # false, false, false, + # false, false, false] + # twist0_differential: false + + # --- Process Variances --- + process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015] + + initial_estimate_covariance: [ + 5.0, 5.0, 1e-3, 1e-3, 1e-3, 1.0, + 0.5, 0.5, 1e-3, 1e-3, 1e-3, 0.1, + 0.1, 0.1, 0.1 + ] + + +maps_manager_node: + ros__parameters: + use_sim_time: true + map_types: [costmap, routes] + routes: + freq: 10.0 + plugin: easynav_routes_maps_manager/RoutesMapsManager + filters: [routes_costmap] + package: easynav_fusion_localizer + map_path_file: routes/simple_square.yaml + routes_costmap: + plugin: easynav_routes_maps_manager/RoutesCostmapFilter + min_cost: 50 + route_width: 0.5 + cycle_time_nort: 0.001 + cycle_time_rt: 0.001 + costmap: + freq: 10.0 + plugin: easynav_costmap_maps_manager/CostmapMapsManager + package: easynav_fusion_localizer + map_path_file: maps/empty.yaml + filters: [obstacles, inflation] + obstacles: + plugin: easynav_costmap_maps_manager/CostmapMapsManager/ObstaclesFilter + inflation: + plugin: easynav_costmap_maps_manager/CostmapMapsManager/InflationFilter + inflation_radius: 1.3 + inscribed_radius: 0.25 + cost_scaling_factor: 3.0 + dummy: + freq: 10.0 + plugin: easynav_maps_manager/DummyMapsManager + cycle_time_nort: 0.1 + cycle_time_rt: 0.0001 + +planner_node: + ros__parameters: + use_sim_time: true + planner_types: [costmap] + dummy: + freq: 1.0 + plugin: easynav_planner/DummyPlanner + cycle_time_nort: 0.2 + cycle_time_rt: 0.0001 + costmap: + freq: 10.0 + plugin: easynav_costmap_planner/CostmapPlanner + cost_factor: 10.0 + continuous_replan: true + simple: + freq: 0.5 + plugin: easynav_simple_planner/SimplePlanner + robot_radius: 0.3 + +sensors_node: + ros__parameters: + use_sim_time: true + forget_time: 0.5 + sensors: [imu, gps] + perception_default_frame: odom + imu: + topic: /imu/data + type: sensor_msgs/msg/Imu + gps: + topic: /gps/fix + type: sensor_msgs/msg/NavSatFix + group: gnss + laser1: + topic: /front_laser/points + type: sensor_msgs/msg/PointCloud2 + group: points + + +system_node: + ros__parameters: + use_sim_time: true + position_tolerance: 0.75 + angle_tolerance: 0.25 diff --git a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp index c29ce6e9..740a03df 100644 --- a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp +++ b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp @@ -1,17 +1,21 @@ #pragma once #include +#include #include "easynav_core/LocalizerMethodBase.hpp" -#include "easynav_fusion_localizer/ukf_wrapper.hpp" // Tu wrapper refactorizado +#include "easynav_fusion_localizer/ukf_wrapper.hpp" #include "sensor_msgs/msg/imu.hpp" #include "sensor_msgs/msg/nav_sat_fix.hpp" #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" namespace easynav { +constexpr double kNoOrientationCovariance = 1e5; + /** * @class FusionLocalizer * @brief Plugin de localización para EasyNav que integra el UKF de @@ -23,6 +27,13 @@ class FusionLocalizer : public easynav::LocalizerMethodBase FusionLocalizer() = default; virtual ~FusionLocalizer() = default; +protected: + /// Subscriber for the initial pose. + rclcpp::Subscription::SharedPtr init_pose_sub_; + + /// Callback for /initialpose. + void init_pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + protected: /** * @brief Hook de inicialización de MethodBase. @@ -51,14 +62,20 @@ class FusionLocalizer : public easynav::LocalizerMethodBase void update(NavState & nav_state) override; private: - std::unique_ptr ukf_wrapper_; + std::unique_ptr ukf_local_{nullptr}; + std::unique_ptr ukf_global_{nullptr}; + + bool has_global_filter_{false}; + bool has_local_filter_{false}; - int n_imu_sensors_{0}; int n_gps_sensors_{0}; geometry_msgs::msg::PoseWithCovarianceStamped navsatfix_to_pose(const sensor_msgs::msg::NavSatFix & navsat_msg); + sensor_msgs::msg::NavSatFix + odom_to_navsatfix(const nav_msgs::msg::Odometry & odom_msg); + double latitude_origin_{0.0}; double longitude_origin_{0.0}; double altitude_origin_{0.0}; @@ -66,8 +83,15 @@ class FusionLocalizer : public easynav::LocalizerMethodBase double UTM_origin_y_{0.0}; double UTM_origin_z_{0.0}; std::string UTM_zone_; + int UTM_zone_number_{0}; + bool UTM_zone_northp_{true}; + + std::vector last_gps_stamp_; + + rclcpp::Publisher::SharedPtr navsat_pub_{nullptr}; + std::string navsatfix_topic_; - std::vector last_gps_stamp_; + bool first_pose_received_{false}; }; diff --git a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp index d9dbde9c..1c7f4d35 100644 --- a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp +++ b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp @@ -64,7 +64,6 @@ #include "tf2/LinearMath/Transform.hpp" #include #include -#include namespace robot_localization { @@ -108,7 +107,8 @@ class UkfWrapper explicit UkfWrapper( std::shared_ptr parent_node, const std::string & tf_prefix, - const std::string & plugin_name); + const std::string & plugin_name, + bool local_filter); //! @brief Destructor //! @@ -344,7 +344,7 @@ class UkfWrapper //! bool validateFilterOutput(nav_msgs::msg::Odometry * message); - std::vector getGpsCallbackDataArr() const + const std::vector & getGpsCallbackDataArr() const { return gps_callbackData_arr_; } @@ -820,13 +820,9 @@ class UkfWrapper //! rclcpp::Service::SharedPtr reset_srv_; - //! @brief Transform buffer for managing coordinate transforms + //! @brief Transform buffer for managing coordinate transforms (shared RTTFBuffer singleton) //! - std::unique_ptr tf_buffer_; - - //! @brief Transform listener for receiving transforms - //! - std::unique_ptr tf_listener_; + tf2_ros::Buffer * tf_buffer_{nullptr}; //! @brief broadcaster of worldTransform tfs //! @@ -870,6 +866,7 @@ class UkfWrapper std::string plugin_name_; std::string tf_prefix_; std::vector gps_callbackData_arr_; + bool local_filter_; }; diff --git a/localizers/easynav_fusion_localizer/maps/empty.png b/localizers/easynav_fusion_localizer/maps/empty.png new file mode 100644 index 00000000..cd6eb69a Binary files /dev/null and b/localizers/easynav_fusion_localizer/maps/empty.png differ diff --git a/localizers/easynav_fusion_localizer/maps/empty.yaml b/localizers/easynav_fusion_localizer/maps/empty.yaml new file mode 100644 index 00000000..629ca4cb --- /dev/null +++ b/localizers/easynav_fusion_localizer/maps/empty.yaml @@ -0,0 +1,7 @@ +image: empty.png +mode: trinary +resolution: 0.2 +origin: [-100.0, -100.0, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/localizers/easynav_fusion_localizer/maps/square.png b/localizers/easynav_fusion_localizer/maps/square.png new file mode 100644 index 00000000..2e6d8527 Binary files /dev/null and b/localizers/easynav_fusion_localizer/maps/square.png differ diff --git a/localizers/easynav_fusion_localizer/maps/square.yaml b/localizers/easynav_fusion_localizer/maps/square.yaml new file mode 100644 index 00000000..34ce4863 --- /dev/null +++ b/localizers/easynav_fusion_localizer/maps/square.yaml @@ -0,0 +1,7 @@ +image: square.png +mode: trinary +resolution: 0.2 +origin: [-100.0, -100.0, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp index b212b73f..a02905af 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -4,13 +4,17 @@ #include "easynav_common/RTTFBuffer.hpp" -#include "easynav_common/types/IMUPerception.hpp" -#include "easynav_common/types/GNSSPerception.hpp" +#include "easynav_sensors/types/IMUPerception.hpp" +#include "easynav_sensors/types/GNSSPerception.hpp" +#include "sensor_msgs/msg/nav_sat_status.hpp" #include #include "easynav_common/YTSession.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/LinearMath/Quaternion.hpp" + namespace easynav { @@ -21,9 +25,12 @@ void FusionLocalizer::on_initialize() auto node = get_node(); - auto localizer_node = std::dynamic_pointer_cast(node); + // Subscribe to initial pose + init_pose_sub_ = node->create_subscription( + "initialpose", 10, + std::bind(&FusionLocalizer::init_pose_callback, this, std::placeholders::_1)); - last_gps_stamp_.resize(10, 0.0); + auto localizer_node = std::dynamic_pointer_cast(node); const std::string & plugin_name = this->get_plugin_name(); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); @@ -32,75 +39,185 @@ void FusionLocalizer::on_initialize() RCLCPP_INFO(localizer_node->get_logger(), "Using parameter namespace: '%s'", plugin_name.c_str()); - ukf_wrapper_ = std::make_unique( - localizer_node, tf_info.tf_prefix, plugin_name + ".local_filter" - ); - ukf_wrapper_->initialize(); - localizer_node->declare_parameter(plugin_name + ".latitude_origin", double(0.0)); - localizer_node->get_parameter(plugin_name + ".latitude_origin", latitude_origin_); + // Detect which filters have parameters configured by checking + // parameter overrides (loaded from YAML before declaration) + const std::string global_prefix = plugin_name + ".global_filter."; + const std::string local_prefix = plugin_name + ".local_filter."; + + const auto & overrides = + localizer_node->get_node_parameters_interface()->get_parameter_overrides(); + + for (const auto & [key, _] : overrides) { + if (!has_global_filter_ && key.rfind(global_prefix, 0) == 0) { + has_global_filter_ = true; + } + if (!has_local_filter_ && key.rfind(local_prefix, 0) == 0) { + has_local_filter_ = true; + } + if (has_global_filter_ && has_local_filter_) { + break; + } + } + + if (!has_global_filter_ && !has_local_filter_) { + RCLCPP_FATAL( + localizer_node->get_logger(), + "No parameters found for either '%s' or '%s'. " + "At least one filter must be configured.", + global_prefix.c_str(), local_prefix.c_str()); + throw std::runtime_error( + "FusionLocalizer: no global_filter or local_filter parameters detected. " + "At least one filter must be configured."); + } + + if (has_global_filter_) { + ukf_global_ = std::make_unique( + localizer_node, tf_info.tf_prefix, plugin_name + ".global_filter", false + ); + ukf_global_->initialize(); + } else { + RCLCPP_WARN(localizer_node->get_logger(), + "No global_filter parameters found. Global filter will NOT be created."); + } + + if (has_local_filter_) { + ukf_local_ = std::make_unique( + localizer_node, tf_info.tf_prefix, plugin_name + ".local_filter", true + ); + ukf_local_->initialize(); + } else { + RCLCPP_WARN(localizer_node->get_logger(), + "No local_filter parameters found. Local filter will NOT be created."); + } + + // GPS-related setup only needed when global filter is active + if (has_global_filter_) { + localizer_node->declare_parameter(plugin_name + ".latitude_origin", double(0.0)); + localizer_node->get_parameter(plugin_name + ".latitude_origin", latitude_origin_); + + localizer_node->declare_parameter(plugin_name + ".longitude_origin", double(0.0)); + localizer_node->get_parameter(plugin_name + ".longitude_origin", longitude_origin_); + + localizer_node->declare_parameter(plugin_name + ".altitude_origin", double(0.0)); + localizer_node->get_parameter(plugin_name + ".altitude_origin", altitude_origin_); - localizer_node->declare_parameter(plugin_name + ".longitude_origin", double(0.0)); - localizer_node->get_parameter(plugin_name + ".longitude_origin", longitude_origin_); + localizer_node->declare_parameter( + plugin_name + ".navsatfix_topic", std::string("gps/filtered")); + localizer_node->get_parameter(plugin_name + ".navsatfix_topic", navsatfix_topic_); + navsat_pub_ = localizer_node->create_publisher( + navsatfix_topic_, rclcpp::QoS(10)); + } - localizer_node->declare_parameter(plugin_name + ".altitude_origin", double(0.0)); - localizer_node->get_parameter(plugin_name + ".altitude_origin", altitude_origin_); } catch (const std::exception & e) { RCLCPP_FATAL( get_node()->get_logger(), "Critical failure initializing UkfWrapper: %s", e.what()); - // Raise error throw std::runtime_error(std::string("Failed to initialize UkfWrapper: ") + e.what()); } + if (has_global_filter_) { + GeographicLib::UTMUPS::Forward(latitude_origin_, longitude_origin_, UTM_zone_number_, + UTM_zone_northp_, UTM_origin_x_, UTM_origin_y_); + UTM_zone_ = std::to_string(UTM_zone_number_) + (UTM_zone_northp_ ? "N" : "S"); + UTM_origin_z_ = altitude_origin_; - int zone; - bool northp; - - GeographicLib::UTMUPS::Forward(latitude_origin_, longitude_origin_, zone, northp, UTM_origin_x_, - UTM_origin_y_); - UTM_zone_ = std::to_string(zone) + (northp ? "N" : "S"); - UTM_origin_z_ = altitude_origin_; - - n_gps_sensors_ = static_cast(ukf_wrapper_->getGpsCallbackDataArr().size()); + n_gps_sensors_ = static_cast(ukf_global_->getGpsCallbackDataArr().size()); + last_gps_stamp_.resize(n_gps_sensors_, rclcpp::Time(0, 0, RCL_ROS_TIME)); + } RCLCPP_INFO(get_node()->get_logger(), "FusionLocalizer (UKF) initialized successfully."); } -// 2. Hook de actualización RT (Tu "Timer" de alta frecuencia) +void FusionLocalizer::init_pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) +{ + if (has_global_filter_) { + nav_msgs::msg::Odometry global_odom; + // Get current position to calculate the offset + if (ukf_global_->getFilteredOdometryMessage(&global_odom)) { + double current_x = global_odom.pose.pose.position.x; + double current_y = global_odom.pose.pose.position.y; + + // Shift the UTM origin so that the current GPS position will now map to msg's position + UTM_origin_x_ += (current_x - msg->pose.pose.position.x); + UTM_origin_y_ += (current_y - msg->pose.pose.position.y); + + RCLCPP_INFO(get_node()->get_logger(), + "Initial pose reset UTM origin. Shifted X by %.2f, Y by %.2f to match pose (%.2f, %.2f)", + (current_x - msg->pose.pose.position.x), (current_y - msg->pose.pose.position.y), + msg->pose.pose.position.x, msg->pose.pose.position.y); + } + + // Forward initial pose to global filter to make the state jump immediately + ukf_global_->setPoseCallback(msg); + first_pose_received_ = true; + } + + if (has_local_filter_) { + ukf_local_->setPoseCallback(msg); + } +} + void FusionLocalizer::update_rt(NavState & nav_state) { - if(n_gps_sensors_ && nav_state.has("gnss")) { - auto gps_data = nav_state.get(std::string("gnss")); - for (int i = 0; i < n_gps_sensors_; ++i) { - double gps_time = gps_data[i]->data.header.stamp.sec + - gps_data[i]->data.header.stamp.nanosec * 1e-9; - if (gps_time > last_gps_stamp_[i]) { - // if(true) { - last_gps_stamp_[i] = gps_time; - auto pose = navsatfix_to_pose(gps_data[i]->data); - // nav_state.set("UTM_gnss_pose", pose); - // Call the wrapper callback - const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); - ukf_wrapper_->poseCallback( - std::make_shared(pose), - ukf_wrapper_->getGpsCallbackDataArr()[i], // callback_data - tf_info.map_frame, // target_frame - tf_info.odom_frame, // pose_source_frame - false // imu_data - ); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + + if (has_global_filter_) { + if (n_gps_sensors_ && nav_state.has_group("gnss")) { + auto gps_data = nav_state.get_group(std::string("gnss")); + const auto & gps_cb_arr = ukf_global_->getGpsCallbackDataArr(); + for (int i = 0; i < n_gps_sensors_; ++i) { + if (gps_data[i]->data.status.status < sensor_msgs::msg::NavSatStatus::STATUS_FIX) { + continue; + } + rclcpp::Time gps_time(gps_data[i]->data.header.stamp); + if (gps_time > last_gps_stamp_[i]) { + EASYNAV_TRACE_NAMED_EVENT("fusion_localizer_process_gps"); + last_gps_stamp_[i] = gps_time; + auto pose = + std::make_shared(navsatfix_to_pose( + gps_data[i]->data)); + if (!first_pose_received_) { + RCLCPP_INFO(get_node()->get_logger(), + "First valid GPS fix received. Initializing filter state."); + if (nav_state.has("imu")) { + auto imu_data = nav_state.get(std::string("imu")); + pose->pose.pose.orientation = imu_data.data.orientation; + } + ukf_global_->setPoseCallback(pose); + first_pose_received_ = true; + continue; + } + ukf_global_->poseCallback( + pose, + gps_cb_arr[i], + tf_info.map_frame, + gps_data[i]->data.header.frame_id, + false + ); + } } } + + ukf_global_->periodicUpdate(); + + nav_msgs::msg::Odometry global_odom; + if (ukf_global_->getFilteredOdometryMessage(&global_odom)) { + nav_state.set("robot_pose", global_odom); + navsat_pub_->publish(odom_to_navsatfix(global_odom)); + } } - ukf_wrapper_->periodicUpdate(); + if (has_local_filter_) { + ukf_local_->periodicUpdate(); - nav_msgs::msg::Odometry current_odom; - if (ukf_wrapper_->getFilteredOdometryMessage(¤t_odom)) { - nav_state.set("robot_pose", current_odom); + nav_msgs::msg::Odometry local_odom; + if (ukf_local_->getFilteredOdometryMessage(&local_odom)) { + nav_state.set("robot_pose_local", local_odom); + } } } -// 3. Hook de actualización no-RT (baja frecuencia) void FusionLocalizer::update([[maybe_unused]] NavState & nav_state) { @@ -110,16 +227,12 @@ geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose const sensor_msgs::msg::NavSatFix & navsat_msg) { geometry_msgs::msg::PoseWithCovarianceStamped pose_msg; - const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); - // 1. Establecer el Header - // Usamos el mismo timestamp que el mensaje original - // y el world_frame_id que el filtro UKF espera (p.ej., "map" u "odom") pose_msg.header = navsat_msg.header; + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); pose_msg.header.frame_id = tf_info.map_frame; - // 2. Convertir coordenadas (Lat, Lon) a UTM (x, y) double utm_x, utm_y; int zone; bool northp; @@ -130,7 +243,8 @@ geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose zone, northp, utm_x, - utm_y); + utm_y, + UTM_zone_number_); pose_msg.pose.pose.position.x = utm_x - UTM_origin_x_; pose_msg.pose.pose.position.y = utm_y - UTM_origin_y_; @@ -143,23 +257,65 @@ geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose pose_msg.pose.covariance.fill(0.0); - pose_msg.pose.covariance[0] = navsat_msg.position_covariance[0]; // xx - pose_msg.pose.covariance[1] = navsat_msg.position_covariance[1]; // xy - pose_msg.pose.covariance[2] = navsat_msg.position_covariance[2]; // xz + double default_var = 1.0; // 1 meter variance standard + + if (navsat_msg.position_covariance_type == + sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN) + { + pose_msg.pose.covariance[0] = navsat_msg.position_covariance[0]; + pose_msg.pose.covariance[7] = navsat_msg.position_covariance[4]; + pose_msg.pose.covariance[14] = navsat_msg.position_covariance[8]; + + pose_msg.pose.covariance[21] = default_var; + pose_msg.pose.covariance[28] = default_var; + pose_msg.pose.covariance[35] = default_var; + } else { + // Fallback variances if GPS doesn't provide them + pose_msg.pose.covariance[0] = default_var; + pose_msg.pose.covariance[7] = default_var; + pose_msg.pose.covariance[14] = default_var; + + pose_msg.pose.covariance[21] = default_var; + pose_msg.pose.covariance[28] = default_var; + pose_msg.pose.covariance[35] = default_var; + + RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 5000, + "NavSatFix covariance type unknown or invalid. Using default covariance."); + } + + return pose_msg; +} + +sensor_msgs::msg::NavSatFix FusionLocalizer::odom_to_navsatfix( + const nav_msgs::msg::Odometry & odom_msg) +{ + sensor_msgs::msg::NavSatFix navsat_msg; - pose_msg.pose.covariance[6] = navsat_msg.position_covariance[3]; // yx - pose_msg.pose.covariance[7] = navsat_msg.position_covariance[4]; // yy - pose_msg.pose.covariance[8] = navsat_msg.position_covariance[5]; // yz + navsat_msg.header = odom_msg.header; + navsat_msg.header.frame_id = "gps_link"; + navsat_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX; + navsat_msg.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS; - pose_msg.pose.covariance[12] = navsat_msg.position_covariance[6]; // zx - pose_msg.pose.covariance[13] = navsat_msg.position_covariance[7]; // zy - pose_msg.pose.covariance[14] = navsat_msg.position_covariance[8]; // zz + const double utm_x = odom_msg.pose.pose.position.x + UTM_origin_x_; + const double utm_y = odom_msg.pose.pose.position.y + UTM_origin_y_; - pose_msg.pose.covariance[21] = 99999.0; - pose_msg.pose.covariance[28] = 99999.0; - pose_msg.pose.covariance[35] = 99999.0; + double latitude = 0.0; + double longitude = 0.0; + GeographicLib::UTMUPS::Reverse( + UTM_zone_number_, UTM_zone_northp_, utm_x, utm_y, latitude, longitude); - return pose_msg; + navsat_msg.latitude = latitude; + navsat_msg.longitude = longitude; + navsat_msg.altitude = odom_msg.pose.pose.position.z + UTM_origin_z_; + + navsat_msg.position_covariance[0] = odom_msg.pose.covariance[0]; + navsat_msg.position_covariance[4] = odom_msg.pose.covariance[7]; + navsat_msg.position_covariance[8] = odom_msg.pose.covariance[14]; + + navsat_msg.position_covariance_type = + sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; + + return navsat_msg; } } // namespace easynav diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp index 8bf14a01..5ce5bd30 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp @@ -75,7 +75,6 @@ #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include #include -#include #include "easynav_common/RTTFBuffer.hpp" @@ -86,7 +85,8 @@ using namespace std::chrono_literals; UkfWrapper::UkfWrapper( std::shared_ptr parent_node, const std::string & tf_prefix, - const std::string & plugin_name) + const std::string & plugin_name, + bool local_filter) : print_diagnostics_(true), publish_acceleration_(false), publish_transform_(true), @@ -114,14 +114,15 @@ UkfWrapper::UkfWrapper( last_set_pose_time_(0, 0, RCL_ROS_TIME), latest_control_time_(0, 0, RCL_ROS_TIME), tf_timeout_(0ns), - tf_time_offset_(0ns) + tf_time_offset_(0ns), + local_filter_(local_filter) { parent_node_ = parent_node; tf_prefix_ = tf_prefix; plugin_name_ = plugin_name; - tf_buffer_ = std::make_unique(parent_node_->get_clock()); - tf_listener_ = std::make_unique(*tf_buffer_); + // Use the shared RTTFBuffer singleton instead of creating a private tf2_ros::Buffer + tf_buffer_ = easynav::RTTFBuffer::getInstance(); state_variable_names_.push_back("X"); state_variable_names_.push_back("Y"); @@ -147,8 +148,6 @@ UkfWrapper::~UkfWrapper() set_pose_sub_.reset(); control_sub_.reset(); stamped_control_sub_.reset(); - tf_listener_.reset(); - tf_buffer_.reset(); diagnostic_updater_.reset(); world_transform_broadcaster_.reset(); set_pose_service_.reset(); @@ -184,8 +183,7 @@ void UkfWrapper::reset() last_diff_time_ = parent_node_->now().seconds(); - // clear tf buffer to avoid TF_OLD_DATA errors - tf_buffer_->clear(); + // Note: tf_buffer_ is the shared RTTFBuffer singleton, do not clear it here // clear last message timestamp, so older messages will be accepted last_message_times_.clear(); @@ -370,7 +368,7 @@ void UkfWrapper::enqueueMeasurement( const std::vector & update_vector, const double mahalanobis_thresh, const rclcpp::Time & time) { - MeasurementPtr meas = MeasurementPtr(new Measurement()); + MeasurementPtr meas = std::make_shared(); meas->topic_name_ = topic_name; meas->measurement_ = measurement; @@ -895,7 +893,11 @@ void UkfWrapper::loadParams() odom_frame_id_ = tf_info.odom_frame; base_link_frame_id_ = tf_info.robot_footprint_frame; // World frame comes from Easynav TFInfo configuration - world_frame_id_ = tf_info.map_frame; + if(local_filter_) { + world_frame_id_ = tf_info.odom_frame; + } else { + world_frame_id_ = tf_info.map_frame; + } base_link_output_frame_id_ = base_link_frame_id_; @@ -1172,30 +1174,39 @@ void UkfWrapper::loadParams() "\npermit_corrected_publication is " << permit_corrected_publication_ << "\nprint_diagnostics is " << print_diagnostics_ << "\n"); + // Determine mode prefix for service/topic names to avoid collisions + // between local and global filter instances + std::string mode_prefix; + if (local_filter_) { + mode_prefix = "local_"; + } else { + mode_prefix = "global_"; + } + // Create a subscriber for manually setting/resetting pose set_pose_sub_ = parent_node_->create_subscription( - "set_pose", rclcpp::QoS(1), + mode_prefix + "set_pose", rclcpp::QoS(1), std::bind(&UkfWrapper::setPoseCallback, this, std::placeholders::_1), options); // Create a service for manually setting/resetting pose set_pose_service_ = parent_node_->create_service( - "set_pose", std::bind( + mode_prefix + "set_pose", std::bind( &UkfWrapper::setPoseSrvCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); // Create a service for manually enabling the filter enable_filter_srv_ = parent_node_->create_service( - "~/enable", std::bind( + "~/" + mode_prefix + "enable", std::bind( &UkfWrapper::enableFilterSrvCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); // Create a service for manually setting/resetting pose reset_srv_ = parent_node_->create_service( - "reset", std::bind( + mode_prefix + "reset", std::bind( &UkfWrapper::resetSrvCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); @@ -1203,7 +1214,7 @@ void UkfWrapper::loadParams() // publishing toggle_filter_processing_srv_ = parent_node_->create_service( - "~/toggle", std::bind( + "~/" + mode_prefix + "toggle", std::bind( &UkfWrapper::toggleFilterProcessingCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); @@ -2090,6 +2101,7 @@ void UkfWrapper::odometryCallback( pos_ptr->header = msg->header; pos_ptr->pose = msg->pose; // Entire pose object, also copies covariance + if (pose_callback_data.pose_use_child_frame_) { poseCallback(pos_ptr, pose_callback_data, world_frame_id_, msg->child_frame_id, false); } else { @@ -2262,15 +2274,21 @@ void UkfWrapper::initialize() // Position publisher rclcpp::PublisherOptions publisher_options; publisher_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); + std::string mode_prefix; + if (local_filter_) { + mode_prefix = "local_"; + } else { + mode_prefix = "global_"; + } position_pub_ = parent_node_->create_publisher( - "odometry/filtered", rclcpp::QoS(10), publisher_options); + mode_prefix + "odometry/filtered", rclcpp::QoS(10), publisher_options); // Optional acceleration publisher if (publish_acceleration_) { accel_pub_ = parent_node_->create_publisher( - "accel/filtered", rclcpp::QoS(10), publisher_options); + mode_prefix + "accel/filtered", rclcpp::QoS(10), publisher_options); } // Block commented as it is handled in the update_rt @@ -2520,10 +2538,29 @@ void UkfWrapper::setPoseCallback( update_vector, measurement, measurement_covariance); // For the state + const auto now = parent_node_->now(); + + // Deterministically set the internal state to the requested pose. + // This ensures a /set_pose (or /initialpose) immediately changes outputs. filter_.setState(measurement); filter_.setEstimateErrorCovariance(measurement_covariance); + filter_.setLastMeasurementTime(now); - filter_.setLastMeasurementTime(parent_node_->now()); + // If the filter was not initialized yet, feed an equivalent measurement + // through the filter to flip its initialized flag. + if (!filter_.getInitializedStatus()) { + Measurement pose_measurement; + pose_measurement.topic_name_ = topic_name; + pose_measurement.measurement_ = measurement; + pose_measurement.covariance_ = measurement_covariance; + pose_measurement.update_vector_ = update_vector; + pose_measurement.time_ = now; + pose_measurement.mahalanobis_thresh_ = std::numeric_limits::max(); + pose_measurement.latest_control_ = latest_control_; + pose_measurement.latest_control_time_ = latest_control_time_; + + filter_.processMeasurement(pose_measurement); + } RF_DEBUG("\n------ /UkfWrapper::setPoseCallback ------\n"); } @@ -2864,7 +2901,7 @@ bool UkfWrapper::prepareAcceleration( // we have to handle the situation. tf2::Transform target_frame_trans; bool can_transform = ros_filter_utilities::lookupTransformSafe( - tf_buffer_.get(), target_frame, msg_frame, msg->header.stamp, tf_timeout_, + tf_buffer_, target_frame, msg_frame, msg->header.stamp, tf_timeout_, target_frame_trans); if (can_transform) { @@ -3093,7 +3130,7 @@ bool UkfWrapper::preparePose( // 2. Get the target frame transformation tf2::Transform target_frame_trans; bool can_transform = ros_filter_utilities::lookupTransformSafe( - tf_buffer_.get(), final_target_frame, pose_tmp.frame_id_, + tf_buffer_, final_target_frame, pose_tmp.frame_id_, rclcpp::Time(tf2::timeToSec(pose_tmp.stamp_)), tf_timeout_, target_frame_trans); @@ -3103,7 +3140,7 @@ bool UkfWrapper::preparePose( bool can_src_transform = false; if (source_frame != base_link_frame_id_) { can_src_transform = ros_filter_utilities::lookupTransformSafe( - tf_buffer_.get(), source_frame, base_link_frame_id_, + tf_buffer_, source_frame, base_link_frame_id_, rclcpp::Time(tf2::timeToSec(pose_tmp.stamp_)), tf_timeout_, source_frame_trans); } @@ -3525,7 +3562,7 @@ bool UkfWrapper::prepareTwist( // 4. We need to transform this into the target frame (probably base_link) tf2::Transform target_frame_trans; bool can_transform = ros_filter_utilities::lookupTransformSafe( - tf_buffer_.get(), target_frame, msg_frame, msg->header.stamp, tf_timeout_, + tf_buffer_, target_frame, msg_frame, msg->header.stamp, tf_timeout_, target_frame_trans); if (can_transform) { diff --git a/localizers/easynav_fusion_localizer/tests/CMakeLists.txt b/localizers/easynav_fusion_localizer/tests/CMakeLists.txt new file mode 100644 index 00000000..8d470925 --- /dev/null +++ b/localizers/easynav_fusion_localizer/tests/CMakeLists.txt @@ -0,0 +1,12 @@ +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(easynav_localizer REQUIRED) + +ament_add_gtest(fusion_localizer_tests fusion_localizer_tests.cpp) + +target_link_libraries(fusion_localizer_tests + fusion_localizer + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + easynav_localizer::easynav_localizer +) diff --git a/localizers/easynav_fusion_localizer/tests/fusion_localizer_tests.cpp b/localizers/easynav_fusion_localizer/tests/fusion_localizer_tests.cpp new file mode 100644 index 00000000..1cbac580 --- /dev/null +++ b/localizers/easynav_fusion_localizer/tests/fusion_localizer_tests.cpp @@ -0,0 +1,183 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include + +#include "easynav_fusion_localizer/FusionLocalizer.hpp" +#include "easynav_localizer/LocalizerNode.hpp" + +#include "easynav_common/types/NavState.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" + +#include "tf2/LinearMath/Quaternion.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace +{ + +double yaw_from_quat(const geometry_msgs::msg::Quaternion & q) +{ + tf2::Quaternion tf_q(q.x, q.y, q.z, q.w); + double roll = 0.0, pitch = 0.0, yaw = 0.0; + tf2::Matrix3x3(tf_q).getRPY(roll, pitch, yaw); + return yaw; +} + +class FriendFusionLocalizer : public easynav::FusionLocalizer +{ +public: + using easynav::FusionLocalizer::init_pose_sub_; + using easynav::FusionLocalizer::update_rt; +}; + +class FusionLocalizerInitialPoseTest : public ::testing::Test +{ +protected: + void SetUp() override {rclcpp::init(0, nullptr);} + void TearDown() override {rclcpp::shutdown();} +}; + +} // namespace + +TEST_F(FusionLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackGroup) +{ + const double x0 = 1.5; + const double y0 = -0.25; + const double yaw0 = 0.7; + + const double x1 = -0.8; + const double y1 = 2.1; + const double yaw1 = -1.2; + + // Provide at least one parameter override so FusionLocalizer considers a filter configured. + rclcpp::NodeOptions options; + options.parameter_overrides({ + rclcpp::Parameter("test.global_filter.frequency", 30.0), + rclcpp::Parameter("test.initial_pose.x", x0), + rclcpp::Parameter("test.initial_pose.y", y0), + rclcpp::Parameter("test.initial_pose.yaw", yaw0), + }); + + auto node = std::make_shared(options); + node->declare_parameter>("localizer_types", std::vector{}); + + auto localizer = std::make_shared(); + + ASSERT_NO_THROW(localizer->initialize(node, "test")); + + ASSERT_NE(localizer->init_pose_sub_, nullptr); + + const auto infos = node->get_subscriptions_info_by_topic("initialpose"); + ASSERT_FALSE(infos.empty()); + + const bool has_expected_type = std::any_of( + infos.begin(), infos.end(), + [](const rclcpp::TopicEndpointInfo & info) { + return info.topic_type() == "geometry_msgs/msg/PoseWithCovarianceStamped"; + }); + EXPECT_TRUE(has_expected_type); + + auto pub_node = std::make_shared("fusion_test_pub_node"); + auto initialpose_pub = pub_node->create_publisher( + "initialpose", 10); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(node->get_node_base_interface()); + exec.add_node(pub_node->get_node_base_interface()); + + { + const auto connect_deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500); + while (std::chrono::steady_clock::now() < connect_deadline && + initialpose_pub->get_subscription_count() == 0) + { + exec.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + + easynav::NavState nav_state; + { + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500); + while (std::chrono::steady_clock::now() < deadline) { + exec.spin_some(); + localizer->update_rt(nav_state); + if (nav_state.has("robot_pose")) { + const auto odom = nav_state.get("robot_pose"); + const bool ok = + std::abs(odom.pose.pose.position.x - x0) < 1e-6 && + std::abs(odom.pose.pose.position.y - y0) < 1e-6 && + std::abs(yaw_from_quat(odom.pose.pose.orientation) - yaw0) < 1e-3; + if (ok) { + break; + } + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + + ASSERT_TRUE(nav_state.has("robot_pose")); + { + const auto odom = nav_state.get("robot_pose"); + EXPECT_NEAR(odom.pose.pose.position.x, x0, 1e-6); + EXPECT_NEAR(odom.pose.pose.position.y, y0, 1e-6); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw0, 1e-3); + } + + geometry_msgs::msg::PoseWithCovarianceStamped init_pose_msg; + init_pose_msg.header.stamp = pub_node->now(); + init_pose_msg.header.frame_id = "map"; + init_pose_msg.pose.pose.position.x = x1; + init_pose_msg.pose.pose.position.y = y1; + init_pose_msg.pose.pose.position.z = 0.0; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, yaw1); + init_pose_msg.pose.pose.orientation = tf2::toMsg(q); + init_pose_msg.pose.covariance.fill(0.0); + initialpose_pub->publish(init_pose_msg); + + { + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500); + while (std::chrono::steady_clock::now() < deadline) { + exec.spin_some(); + localizer->update_rt(nav_state); + + const auto odom = nav_state.get("robot_pose"); + const bool ok = + std::abs(odom.pose.pose.position.x - x1) < 1e-6 && + std::abs(odom.pose.pose.position.y - y1) < 1e-6 && + std::abs(yaw_from_quat(odom.pose.pose.orientation) - yaw1) < 1e-3; + + if (ok) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + + { + const auto odom = nav_state.get("robot_pose"); + EXPECT_NEAR(odom.pose.pose.position.x, x1, 1e-6); + EXPECT_NEAR(odom.pose.pose.position.y, y1, 1e-6); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw1, 1e-3); + } +} diff --git a/localizers/easynav_gps_localizer/CMakeLists.txt b/localizers/easynav_gps_localizer/CMakeLists.txt index f18114bc..b44c29b4 100644 --- a/localizers/easynav_gps_localizer/CMakeLists.txt +++ b/localizers/easynav_gps_localizer/CMakeLists.txt @@ -59,6 +59,9 @@ if(BUILD_TESTING) set(ament_cmake_copyright_FOUND TRUE) set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(tests) endif() ament_export_include_directories(include) diff --git a/localizers/easynav_gps_localizer/README.md b/localizers/easynav_gps_localizer/README.md index 5306d1fb..97640b1f 100644 --- a/localizers/easynav_gps_localizer/README.md +++ b/localizers/easynav_gps_localizer/README.md @@ -28,7 +28,13 @@ GPS-based localizer that fuses NavSatFix and IMU to publish odometry in an odom- ## Parameters -This plugin does not declare configurable ROS parameters. +All parameters are declared under the plugin namespace, i.e., `//easynav_gps_localizer/GpsLocalizer/...`. + +| Name | Type | Default | Description | +|---|---|---:|---| +| `.initial_pose.x` | `double` | `0.0` | Initial X position (m). | +| `.initial_pose.y` | `double` | `0.0` | Initial Y position (m). | +| `.initial_pose.yaw` | `double` | `0.0` | Initial yaw (rad). | ## Interfaces (Topics and Services) @@ -38,6 +44,7 @@ This plugin does not declare configurable ROS parameters. |---|---|---|---|---| | Subscription | `robot/gps/fix` | `sensor_msgs/msg/NavSatFix` | Raw GPS fix. | SensorDataQoS (reliable) | | Subscription | `imu/data` | `sensor_msgs/msg/Imu` | IMU orientation for yaw fusion. | SensorDataQoS (reliable) | +| Subscription | `initialpose` | `geometry_msgs/msg/PoseWithCovarianceStamped` | Seed/reset the localizer pose from an external initial pose estimate. | depth=10 | | Publisher | `robot/odom_gps` | `nav_msgs/msg/Odometry` | Odometry fused from GPS + IMU (UTM-projected). | SensorDataQoS | ### Services diff --git a/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp b/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp index 03d1fcdc..2628d6a9 100644 --- a/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp +++ b/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp @@ -21,10 +21,12 @@ #include "nav_msgs/msg/odometry.hpp" #include "sensor_msgs/msg/nav_sat_fix.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" #include "easynav_core/LocalizerMethodBase.hpp" #include "tf2_ros/static_transform_broadcaster.hpp" #include "sensor_msgs/msg/imu.hpp" #include +#include namespace easynav { @@ -83,6 +85,16 @@ class GpsLocalizer : public easynav::LocalizerMethodBase */ virtual void update(NavState & nav_state) override; +protected: + /// Subscriber for the initial pose. + rclcpp::Subscription::SharedPtr init_pose_sub_; + + /// Initial pose received but not yet applied (e.g. waiting for a valid GPS fix). + std::optional pending_init_pose_; + + /// Callback for /initialpose. + void init_pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + private: /** * @brief Internal representation of the robot's current odometry. diff --git a/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp b/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp index c4eec332..a0ab2cf9 100644 --- a/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp +++ b/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp @@ -20,12 +20,16 @@ #include "easynav_common/RTTFBuffer.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/LinearMath/Quaternion.hpp" + namespace easynav { void GpsLocalizer::on_initialize() { auto node = get_node(); + const auto & plugin_name = get_plugin_name(); // Initialize the odometry message odom_.header.stamp = get_node()->now(); @@ -46,6 +50,38 @@ void GpsLocalizer::on_initialize() "imu/data", rclcpp::SensorDataQoS().reliable(), std::bind(&GpsLocalizer::imu_callback, this, std::placeholders::_1)); + // Subscribe to initial pose + init_pose_sub_ = node->create_subscription( + "initialpose", 10, + std::bind(&GpsLocalizer::init_pose_callback, this, std::placeholders::_1)); + + // Optional initial pose from parameters (kept consistent with AMCL parameter names) + node->declare_parameter(plugin_name + ".initial_pose.x", 0.0); + node->declare_parameter(plugin_name + ".initial_pose.y", 0.0); + node->declare_parameter(plugin_name + ".initial_pose.yaw", 0.0); + + double init_x = 0.0; + double init_y = 0.0; + double init_yaw = 0.0; + node->get_parameter(plugin_name + ".initial_pose.x", init_x); + node->get_parameter(plugin_name + ".initial_pose.y", init_y); + node->get_parameter(plugin_name + ".initial_pose.yaw", init_yaw); + + if (std::abs(init_x) > 1e-12 || std::abs(init_y) > 1e-12 || std::abs(init_yaw) > 1e-12) { + geometry_msgs::msg::PoseWithCovarianceStamped init_pose; + init_pose.header.stamp = node->now(); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + init_pose.header.frame_id = tf_info.map_frame; + init_pose.pose.pose.position.x = init_x; + init_pose.pose.pose.position.y = init_y; + init_pose.pose.pose.position.z = 0.0; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, init_yaw); + init_pose.pose.pose.orientation = tf2::toMsg(q); + init_pose.pose.covariance.fill(0.0); + pending_init_pose_ = init_pose; + } + // Create publisher odom_pub_ = node->create_publisher( "robot/odom_gps", rclcpp::SensorDataQoS().reliable()); @@ -81,6 +117,12 @@ void GpsLocalizer::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) imu_msg_ = std::move(*msg); } +void GpsLocalizer::init_pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) +{ + pending_init_pose_ = *msg; +} + void GpsLocalizer::update_rt([[maybe_unused]] NavState & nav_state) { @@ -98,6 +140,14 @@ void GpsLocalizer::update(NavState & nav_state) GeographicLib::UTMUPS::Forward(lat, lon, zone, northp, utm_x, utm_y); std::string utm_zone = std::to_string(zone) + (northp ? "N" : "S"); + // If an initial pose was provided, align the UTM origin so that the current + // GPS fix maps to that pose in the map frame. + if (pending_init_pose_.has_value() && gps_msg_ != sensor_msgs::msg::NavSatFix()) { + origin_utm_.x = utm_x - pending_init_pose_->pose.pose.position.x; + origin_utm_.y = utm_y - pending_init_pose_->pose.pose.position.y; + pending_init_pose_.reset(); + } + if (origin_utm_ == geometry_msgs::msg::Point() && gps_msg_ != sensor_msgs::msg::NavSatFix()) { diff --git a/localizers/easynav_gps_localizer/tests/CMakeLists.txt b/localizers/easynav_gps_localizer/tests/CMakeLists.txt new file mode 100644 index 00000000..0e8b0048 --- /dev/null +++ b/localizers/easynav_gps_localizer/tests/CMakeLists.txt @@ -0,0 +1,10 @@ +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) + +ament_add_gtest(gps_localizer_tests gps_localizer_tests.cpp) + +target_link_libraries(gps_localizer_tests + gps_localizer + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle +) diff --git a/localizers/easynav_gps_localizer/tests/gps_localizer_tests.cpp b/localizers/easynav_gps_localizer/tests/gps_localizer_tests.cpp new file mode 100644 index 00000000..b594eaee --- /dev/null +++ b/localizers/easynav_gps_localizer/tests/gps_localizer_tests.cpp @@ -0,0 +1,186 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include + +#include "easynav_gps_localizer/GpsLocalizer.hpp" + +#include "easynav_common/types/NavState.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/LinearMath/Quaternion.hpp" + +namespace +{ + +class FriendGpsLocalizer : public easynav::GpsLocalizer +{ +public: + using easynav::GpsLocalizer::init_pose_sub_; + using easynav::GpsLocalizer::update; +}; + +class GpsLocalizerInitialPoseTest : public ::testing::Test +{ +protected: + void SetUp() override {rclcpp::init(0, nullptr);} + void TearDown() override {rclcpp::shutdown();} +}; + +} // namespace + +TEST_F(GpsLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackGroup) +{ + const double x0 = 1.1; + const double y0 = -0.7; + + const double x1 = -2.3; + const double y1 = 0.4; + + rclcpp::NodeOptions options; + options.parameter_overrides({ + rclcpp::Parameter("test.initial_pose.x", x0), + rclcpp::Parameter("test.initial_pose.y", y0), + rclcpp::Parameter("test.initial_pose.yaw", 0.0), + }); + + auto node = std::make_shared("test_gps_localizer_node", options); + auto localizer = std::make_shared(); + + localizer->initialize(node, "test"); + + ASSERT_NE(localizer->init_pose_sub_, nullptr); + + const auto infos = node->get_subscriptions_info_by_topic("initialpose"); + ASSERT_FALSE(infos.empty()); + + const bool has_expected_type = std::any_of( + infos.begin(), infos.end(), + [](const rclcpp::TopicEndpointInfo & info) { + return info.topic_type() == "geometry_msgs/msg/PoseWithCovarianceStamped"; + }); + EXPECT_TRUE(has_expected_type); + + auto pub_node = std::make_shared("gps_test_pub_node"); + auto initialpose_pub = pub_node->create_publisher( + "initialpose", 10); + auto gps_pub = pub_node->create_publisher( + "robot/gps/fix", rclcpp::SensorDataQoS().reliable()); + auto imu_pub = pub_node->create_publisher( + "imu/data", rclcpp::SensorDataQoS().reliable()); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(node->get_node_base_interface()); + exec.add_node(pub_node->get_node_base_interface()); + + { + const auto connect_deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500); + while (std::chrono::steady_clock::now() < connect_deadline && + (initialpose_pub->get_subscription_count() == 0 || + gps_pub->get_subscription_count() == 0 || + imu_pub->get_subscription_count() == 0)) + { + exec.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + + sensor_msgs::msg::Imu imu_msg; + imu_msg.header.stamp = pub_node->now(); + imu_msg.header.frame_id = ""; + imu_msg.angular_velocity.z = 0.0; + imu_msg.orientation.w = 1.0; + imu_pub->publish(imu_msg); + + sensor_msgs::msg::NavSatFix gps_msg; + gps_msg.header.stamp = pub_node->now(); + gps_msg.header.frame_id = ""; + gps_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX; + gps_msg.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS; + gps_msg.latitude = 42.0; + gps_msg.longitude = -1.0; + gps_msg.altitude = 0.0; + gps_pub->publish(gps_msg); + + easynav::NavState nav_state; + + // Deliver GPS/IMU callbacks and run update() until the initial pose is applied. + { + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500); + while (std::chrono::steady_clock::now() < deadline) { + exec.spin_some(); + localizer->update(nav_state); + + if (nav_state.has("robot_pose")) { + const auto odom = nav_state.get("robot_pose"); + const bool ok = + std::abs(odom.pose.pose.position.x - x0) < 1e-9 && + std::abs(odom.pose.pose.position.y - y0) < 1e-9; + if (ok) { + break; + } + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + + ASSERT_TRUE(nav_state.has("robot_pose")); + { + const auto odom = nav_state.get("robot_pose"); + EXPECT_NEAR(odom.pose.pose.position.x, x0, 1e-9); + EXPECT_NEAR(odom.pose.pose.position.y, y0, 1e-9); + } + + geometry_msgs::msg::PoseWithCovarianceStamped init_pose_msg; + init_pose_msg.header.stamp = pub_node->now(); + init_pose_msg.header.frame_id = "map"; + init_pose_msg.pose.pose.position.x = x1; + init_pose_msg.pose.pose.position.y = y1; + init_pose_msg.pose.pose.position.z = 0.0; + init_pose_msg.pose.pose.orientation.w = 1.0; + init_pose_msg.pose.covariance.fill(0.0); + initialpose_pub->publish(init_pose_msg); + + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500); + while (std::chrono::steady_clock::now() < deadline) { + exec.spin_some(); + localizer->update(nav_state); + + const auto odom = nav_state.get("robot_pose"); + const bool ok = + std::abs(odom.pose.pose.position.x - x1) < 1e-9 && + std::abs(odom.pose.pose.position.y - y1) < 1e-9; + + if (ok) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + { + const auto odom = nav_state.get("robot_pose"); + EXPECT_NEAR(odom.pose.pose.position.x, x1, 1e-9); + EXPECT_NEAR(odom.pose.pose.position.y, y1, 1e-9); + } +} diff --git a/localizers/easynav_navmap_localizer/CMakeLists.txt b/localizers/easynav_navmap_localizer/CMakeLists.txt index 12e237f4..8bd6781e 100644 --- a/localizers/easynav_navmap_localizer/CMakeLists.txt +++ b/localizers/easynav_navmap_localizer/CMakeLists.txt @@ -64,7 +64,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) - # add_subdirectory(tests) + add_subdirectory(tests) endif() ament_export_include_directories("include/${PROJECT_NAME}") diff --git a/localizers/easynav_navmap_localizer/README.md b/localizers/easynav_navmap_localizer/README.md index b984b01b..827a6c1c 100644 --- a/localizers/easynav_navmap_localizer/README.md +++ b/localizers/easynav_navmap_localizer/README.md @@ -59,6 +59,7 @@ All parameters are declared under the plugin name namespace, i.e., `// | Direction | Topic | Type | Purpose | QoS | |---|---|---|---|---| | Subscription | `/odom` | `nav_msgs/msg/Odometry` | Used only when `.compute_odom_from_tf = false` | SensorDataQoS (reliable) | +| Subscription | `initialpose` | `geometry_msgs/msg/PoseWithCovarianceStamped` | Initialize particles pose to the received pose, using covariance | depth=10 | | Publisher | `//particles` | `geometry_msgs/msg/PoseArray` | Publishes the current particle set | depth=10 | | Publisher | `//pose` | `geometry_msgs/msg/PoseWithCovarianceStamped` | Publishes the estimated pose with covariance | depth=10 | diff --git a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp index c166f876..55a666d3 100644 --- a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp @@ -163,6 +163,9 @@ class AMCLLocalizer : public LocalizerMethodBase /// Subscriber for odometry messages. rclcpp::Subscription::SharedPtr odom_sub_; + /// Subscriber for the initial pose. + rclcpp::Subscription::SharedPtr init_pose_sub_; + /** * @brief Callback for receiving odometry updates. * @@ -170,6 +173,13 @@ class AMCLLocalizer : public LocalizerMethodBase */ void odom_callback(nav_msgs::msg::Odometry::UniquePtr msg); + /** + * @brief Callback for receiving the initial pose. + * + * @param msg The incoming initial pose with covariance. + */ + void init_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr msg); + /** * @brief Update odom from TFs instead of a odom topic * diff --git a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp index 79117a91..2b6dc641 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -34,8 +34,8 @@ #include "tf2/LinearMath/Vector3.hpp" #include "easynav_common/RTTFBuffer.hpp" -#include "easynav_common/types/PointPerception.hpp" -#include "easynav_common/types/IMUPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" +#include "easynav_sensors/types/IMUPerception.hpp" #include "navmap_core/NavMap.hpp" @@ -449,7 +449,8 @@ void AMCLLocalizer::on_initialize() tf_broadcaster_ = std::make_unique(get_node()); auto node_typed = std::dynamic_pointer_cast(get_node()); - auto rt_cbg = node_typed->get_real_time_cbg(); + auto rt_cbg = node_typed ? node_typed->get_real_time_cbg() : + get_node()->get_node_base_interface()->get_default_callback_group(); rclcpp::SubscriptionOptions options; options.callback_group = rt_cbg; if (!compute_odom_from_tf_) { @@ -463,6 +464,10 @@ void AMCLLocalizer::on_initialize() estimate_pub_ = get_node()->create_publisher( node->get_fully_qualified_name() + std::string("/") + plugin_name + "/pose", 10); + init_pose_sub_ = get_node()->create_subscription( + "initialpose", 10, + std::bind(&AMCLLocalizer::init_pose_callback, this, std::placeholders::_1)); + last_reseed_ = get_node()->now(); get_node()->get_logger().set_level(rclcpp::Logger::Level::Debug); } @@ -475,6 +480,153 @@ void AMCLLocalizer::odom_callback(nav_msgs::msg::Odometry::UniquePtr msg) if (!initialized_odom_) {last_odom_ = odom_; initialized_odom_ = true;} } +void +AMCLLocalizer::init_pose_callback( + geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr msg) +{ + if (particles_.empty()) { + return; + } + + auto logger = get_node()->get_logger(); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + + // Check expected frame (map) + const std::string expected_frame = tf_info.map_frame; + if (!msg->header.frame_id.empty() && msg->header.frame_id != expected_frame) { + RCLCPP_WARN( + logger, + "AMCLLocalizer::init_pose_callback: received initial pose in frame '%s' but expected '%s'. " + "Ignoring message.", + msg->header.frame_id.c_str(), expected_frame.c_str()); + return; + } + + last_input_time_ = msg->header.stamp; + + // Extract pose mean (x, y, yaw) expressed in map frame + const auto & pose = msg->pose.pose; + const double mean_x = pose.position.x; + const double mean_y = pose.position.y; + + tf2::Quaternion q( + pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + pose.orientation.w); + double roll, pitch, yaw; + tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); + const double mean_yaw = yaw; + + // Extract 6x6 covariance (ROS order: x, y, z, roll, pitch, yaw) + const auto & cov = msg->pose.covariance; + + // Positional covariance terms + double var_x = cov[0]; + double cov_xy = cov[1]; + double var_y = cov[7]; + + // Yaw variance + double var_yaw = cov[35]; + + // Ensure non-negative variances + var_x = std::max(var_x, 0.0); + var_y = std::max(var_y, 0.0); + var_yaw = std::max(var_yaw, 0.0); + + // 2D Cholesky decomposition of covariance(x,y): + // [ var_x cov_xy ] + // [ cov_xy var_y ] + double l00 = std::sqrt(var_x); + double l10 = 0.0; + double l11 = 0.0; + + if (l00 > 0.0) { + l10 = cov_xy / l00; + double tmp = var_y - l10 * l10; + if (tmp < 0.0) { + tmp = 0.0; + } + l11 = std::sqrt(tmp); + } else { + // Degenerate case: fallback to diagonal stddevs + l00 = std::sqrt(var_x); + l10 = 0.0; + l11 = std::sqrt(var_y); + } + + // Enforce minimum translational noise if covariance is too small + const double std_xy = std::sqrt(0.5 * std::max(0.0, var_x + var_y)); + if (std_xy < min_noise_xy_) { + const double safe_std_xy = (std_xy > 1e-6) ? std_xy : 1.0; + const double scale = min_noise_xy_ / safe_std_xy; + l00 *= scale; + l10 *= scale; + l11 *= scale; + } + + // Yaw noise with minimum enforced + double yaw_stddev = std::sqrt(var_yaw); + yaw_stddev = std::max(yaw_stddev, min_noise_yaw_); + + std::normal_distribution standard_normal(0.0, 1.0); + std::normal_distribution yaw_noise(0.0, yaw_stddev); + + const std::size_t N = particles_.size(); + for (std::size_t i = 0; i < N; ++i) { + const double z0 = standard_normal(rng_); + const double z1 = standard_normal(rng_); + + const double dx = l00 * z0; + const double dy = l10 * z0 + l11 * z1; + + const double new_x = mean_x + dx; + const double new_y = mean_y + dy; + const double new_yaw = mean_yaw + yaw_noise(rng_); + + tf2::Quaternion q_particle; + q_particle.setRPY(0.0, 0.0, new_yaw); + + particles_[i].pose.setOrigin(tf2::Vector3(new_x, new_y, 0.0)); + particles_[i].pose.setRotation(q_particle); + + particles_[i].hits = 0; + particles_[i].possible_hits = 0; + particles_[i].weight = 1.0 / static_cast(N); + } + + // Normalize particle weights (safety) + double total_weight = 0.0; + for (const auto & p : particles_) { + total_weight += p.weight; + } + if (total_weight > 0.0) { + for (auto & p : particles_) { + p.weight /= total_weight; + } + } + + // Prevent immediate reseed after initialization + last_reseed_ = get_node()->now(); + + // Compute map->basefootprint estimate + tf2::Transform map2bf = getEstimatedPose(); + + // Publish TF if odom transform is already known + if (initialized_odom_) { + tf2::Transform map2odom = map2bf * odom_.inverse(); + publishTF(map2odom); + } + + publishEstimatedPose(map2bf); + publishParticles(); + + RCLCPP_INFO( + logger, + "AMCLLocalizer::init_pose_callback: reinitialized %zu particles around (%.3f, %.3f, %.3f)", + N, mean_x, mean_y, mean_yaw); +} + void AMCLLocalizer::update_odom_from_tf() { const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); @@ -496,9 +648,8 @@ void AMCLLocalizer::update_odom_from_tf() std::optional get_latest_imu_quat(const NavState & nav_state) { if (!nav_state.has("imu")) {return std::nullopt;} - const auto & imus = nav_state.get("imu"); - if (imus.empty() || !imus.back()) {return std::nullopt;} - const auto & imu_msg = imus.back()->data; + const auto & imu = nav_state.get("imu"); + const auto & imu_msg = imu.data; tf2::Quaternion q(imu_msg.orientation.x, imu_msg.orientation.y, imu_msg.orientation.z, imu_msg.orientation.w); if (q.length2() < 1e-12) {return std::nullopt;} @@ -689,11 +840,11 @@ static ScoreAgg score_particle_sensor_cloud( // ---------- correct() ---------- void AMCLLocalizer::correct(NavState & nav_state) { - if (!nav_state.has("points")) { - RCLCPP_WARN(get_node()->get_logger(), "No points perceptions yet"); + const auto & perceptions = nav_state.get_no_group(); + if (perceptions.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions"); return; } - const auto & perceptions = nav_state.get("points"); if (!nav_state.has("map.bonxai")) { RCLCPP_WARN(get_node()->get_logger(), "No Bonxai map yet"); diff --git a/localizers/easynav_navmap_localizer/tests/CMakeLists.txt b/localizers/easynav_navmap_localizer/tests/CMakeLists.txt index b874c059..400b4523 100644 --- a/localizers/easynav_navmap_localizer/tests/CMakeLists.txt +++ b/localizers/easynav_navmap_localizer/tests/CMakeLists.txt @@ -2,8 +2,8 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(std_srvs REQUIRED) -ament_add_gtest(costmap_localizer_tests costmap_localizer_tests.cpp) -target_link_libraries(costmap_localizer_tests +ament_add_gtest(navmap_localizer_tests navmap_localizer_tests.cpp) +target_link_libraries(navmap_localizer_tests ${PROJECT_NAME} rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle diff --git a/localizers/easynav_navmap_localizer/tests/costmap_localizer_tests.cpp b/localizers/easynav_navmap_localizer/tests/costmap_localizer_tests.cpp deleted file mode 100644 index 7e9166a0..00000000 --- a/localizers/easynav_navmap_localizer/tests/costmap_localizer_tests.cpp +++ /dev/null @@ -1,157 +0,0 @@ -// Copyright 2025 Intelligent Robotics Lab -// -// This file is part of the project Easy Navigation (EasyNav in short) -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include "easynav_navmap_localizer/AMCLLocalizer.hpp" -#include "easynav_navmap_common/costmap_2d.hpp" - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" - -#include "std_srvs/srv/trigger.hpp" - -#include -#include - -/// \brief Fixture for AMCLLocalizer tests (minimal) -class AMCLLocalizerTest : public ::testing::Test -{ -protected: - void SetUp() override - { - rclcpp::init(0, nullptr); - } - - void TearDown() override - { - rclcpp::shutdown(); - } -}; - -TEST_F(AMCLLocalizerTest, BasicDynamicUpdate) -{ - auto node = std::make_shared("test_node"); - auto manager = std::make_shared(); - manager->initialize(node, "test"); - - auto static_map = std::make_shared<::navmap::NavMap>(); - static_map->resizeMap(30, 30, 0.1, -1.5, -1.5); - static_map->setCost(15, 15, 254); // Occupied cell - manager->set_static_map(static_map); - - easynav::NavState navstate; - auto perception = std::make_shared(); - perception->data.points.resize(2); - perception->data.points[0].x = 0.0; - perception->data.points[0].y = 0.0; - perception->data.points[0].z = 0.2; - perception->data.points[1].x = 1.5; - perception->data.points[1].y = 1.5; - perception->data.points[1].z = 0.2; - perception->stamp = rclcpp::Time(0); - perception->frame_id = "map"; - perception->valid = true; - - navstate.set("perceptions", easynav::Perceptions()); - navstate.get_mutable("perceptions")->push_back(perception); - navstate.set("map.static", *static_map); - - manager->update(navstate); - - auto map_ptr = std::dynamic_pointer_cast<::navmap::NavMap>(manager->get_static_map()); - ASSERT_TRUE(map_ptr != nullptr); - EXPECT_EQ(map_ptr->getCost(15, 15), 254u); // Cell (15, 15) -} - -TEST_F(AMCLLocalizerTest, IncomingOccupancyGridUpdatesMaps) -{ - auto node = std::make_shared("test_node2"); - auto manager = std::make_shared(); - manager->initialize(node, "test2"); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); - - auto pub = node->create_publisher( - "test_node2/test2/incoming_map", rclcpp::QoS(1).transient_local().reliable()); - - nav_msgs::msg::OccupancyGrid grid; - grid.header.frame_id = "map"; - grid.info.width = 10; - grid.info.height = 10; - grid.info.resolution = 0.2; - grid.info.origin.position.x = -1.0; - grid.info.origin.position.y = -0.6; - grid.data.assign(100, 0); - grid.data[55] = 100; - - pub->publish(grid); - - executor.spin_some(); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - - auto static_map = std::dynamic_pointer_cast<::navmap::NavMap>(manager->get_static_map()); - ASSERT_TRUE(static_map != nullptr); - EXPECT_EQ(static_map->getCost(5, 5), 254u); -} - -class FriendAMCLLocalizer : public easynav::AMCLLocalizer { -public: - void force_path(const std::string & path) {map_path_ = path;} -}; - -TEST_F(AMCLLocalizerTest, SavemapServiceWorks) -{ - auto node = std::make_shared("test_savemap_node"); - auto manager = std::make_shared(); - manager->initialize(node, "test_savemap"); - - auto static_map = std::make_shared<::navmap::NavMap>(); - static_map->resizeMap(4, 4, 0.5, -1.0, -1.0); - static_map->setCost(1, 1, 254); - static_map->setCost(2, 2, 254); - manager->set_static_map(static_map); - - const std::string test_map_file = "/tmp/savemap_test_map"; - std::static_pointer_cast(manager)->force_path(test_map_file); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); - - auto client = node->create_client( - "/test_savemap_node/test_savemap/savemap"); - - ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); - - auto request = std::make_shared(); - auto future = client->async_send_request(request); - executor.spin_until_future_complete(future); - - auto response = future.get(); - EXPECT_TRUE(response->success); - EXPECT_NE(response->message.find("saved"), std::string::npos); - - std::ifstream yamlfile(test_map_file + ".yaml"); - ASSERT_TRUE(yamlfile.is_open()); - - std::string line; - std::getline(yamlfile, line); - EXPECT_NE(line.find("image:"), std::string::npos); - yamlfile.close(); - - std::remove((test_map_file + ".yaml").c_str()); - std::remove((test_map_file + ".pgm").c_str()); -} diff --git a/localizers/easynav_navmap_localizer/tests/navmap_localizer_tests.cpp b/localizers/easynav_navmap_localizer/tests/navmap_localizer_tests.cpp new file mode 100644 index 00000000..89dfed8a --- /dev/null +++ b/localizers/easynav_navmap_localizer/tests/navmap_localizer_tests.cpp @@ -0,0 +1,182 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include + +#include "easynav_navmap_localizer/AMCLLocalizer.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace +{ + +double yaw_from_quat(const geometry_msgs::msg::Quaternion & q) +{ + tf2::Quaternion tf_q(q.x, q.y, q.z, q.w); + double roll = 0.0, pitch = 0.0, yaw = 0.0; + tf2::Matrix3x3(tf_q).getRPY(roll, pitch, yaw); + return yaw; +} + +double yaw_from_tf(const tf2::Transform & tf) +{ + double roll = 0.0, pitch = 0.0, yaw = 0.0; + tf2::Matrix3x3(tf.getRotation()).getRPY(roll, pitch, yaw); + return yaw; +} + +class FriendAMCLLocalizer : public easynav::navmap::AMCLLocalizer +{ +public: + using easynav::navmap::AMCLLocalizer::init_pose_sub_; +}; + +class AMCLLocalizerInitialPoseTest : public ::testing::Test +{ +protected: + void SetUp() override {rclcpp::init(0, nullptr);} + void TearDown() override {rclcpp::shutdown();} +}; + +} // namespace + +TEST_F(AMCLLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackGroup) +{ + const double x0 = -3.0; + const double y0 = 0.25; + const double yaw0 = 0.2; + + const double x1 = 0.8; + const double y1 = 3.3; + const double yaw1 = 2.0; + + rclcpp::NodeOptions options; + options.parameter_overrides({ + rclcpp::Parameter("test.num_particles", 100), + rclcpp::Parameter("test.initial_pose.x", x0), + rclcpp::Parameter("test.initial_pose.y", y0), + rclcpp::Parameter("test.initial_pose.yaw", yaw0), + rclcpp::Parameter("test.initial_pose.std_dev_xy", 0.0), + rclcpp::Parameter("test.initial_pose.std_dev_yaw", 0.0), + rclcpp::Parameter("test.min_noise_xy", 0.0), + rclcpp::Parameter("test.min_noise_yaw", 0.0), + rclcpp::Parameter("test.compute_odom_from_tf", true), + }); + + auto node = std::make_shared( + "test_navmap_localizer_node", options); + auto localizer = std::make_shared(); + + localizer->initialize(node, "test"); + + ASSERT_NE(localizer->init_pose_sub_, nullptr); + + { + const tf2::Transform tf = localizer->getEstimatedPose(); + EXPECT_NEAR(tf.getOrigin().x(), x0, 1e-9); + EXPECT_NEAR(tf.getOrigin().y(), y0, 1e-9); + EXPECT_NEAR(yaw_from_tf(tf), yaw0, 1e-9); + } + + { + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + EXPECT_NEAR(odom.pose.pose.position.x, x0, 1e-9); + EXPECT_NEAR(odom.pose.pose.position.y, y0, 1e-9); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw0, 1e-9); + } + + const auto infos = node->get_subscriptions_info_by_topic("initialpose"); + ASSERT_FALSE(infos.empty()); + + const bool has_expected_type = std::any_of( + infos.begin(), infos.end(), + [](const rclcpp::TopicEndpointInfo & info) { + return info.topic_type() == "geometry_msgs/msg/PoseWithCovarianceStamped"; + }); + EXPECT_TRUE(has_expected_type); + + auto pub_node = std::make_shared("initialpose_pub_node"); + auto pub = pub_node->create_publisher( + "initialpose", 10); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(node->get_node_base_interface()); + exec.add_node(pub_node->get_node_base_interface()); + + geometry_msgs::msg::PoseWithCovarianceStamped msg; + msg.header.stamp = pub_node->now(); + msg.header.frame_id = "map"; + msg.pose.pose.position.x = x1; + msg.pose.pose.position.y = y1; + msg.pose.pose.position.z = 0.0; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, yaw1); + msg.pose.pose.orientation = tf2::toMsg(q); + msg.pose.covariance.fill(0.0); + + { + const auto connect_deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(200); + while (std::chrono::steady_clock::now() < connect_deadline && + pub->get_subscription_count() == 0) + { + exec.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + pub->publish(msg); + + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500); + while (std::chrono::steady_clock::now() < deadline) { + exec.spin_some(); + const tf2::Transform tf = localizer->getEstimatedPose(); + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + + const bool pose_ok = + std::abs(tf.getOrigin().x() - x1) < 1e-9 && + std::abs(tf.getOrigin().y() - y1) < 1e-9 && + std::abs(yaw_from_tf(tf) - yaw1) < 1e-9 && + std::abs(odom.pose.pose.position.x - x1) < 1e-9 && + std::abs(odom.pose.pose.position.y - y1) < 1e-9 && + std::abs(yaw_from_quat(odom.pose.pose.orientation) - yaw1) < 1e-9; + + if (pose_ok) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + { + const tf2::Transform tf = localizer->getEstimatedPose(); + EXPECT_NEAR(tf.getOrigin().x(), x1, 1e-9); + EXPECT_NEAR(tf.getOrigin().y(), y1, 1e-9); + EXPECT_NEAR(yaw_from_tf(tf), yaw1, 1e-9); + } + + { + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + EXPECT_NEAR(odom.pose.pose.position.x, x1, 1e-9); + EXPECT_NEAR(odom.pose.pose.position.y, y1, 1e-9); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw1, 1e-9); + } +} diff --git a/localizers/easynav_simple_localizer/CMakeLists.txt b/localizers/easynav_simple_localizer/CMakeLists.txt index 5b61ef63..a26bb848 100644 --- a/localizers/easynav_simple_localizer/CMakeLists.txt +++ b/localizers/easynav_simple_localizer/CMakeLists.txt @@ -58,7 +58,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) - # add_subdirectory(tests) + add_subdirectory(tests) endif() ament_export_include_directories("include/${PROJECT_NAME}") diff --git a/localizers/easynav_simple_localizer/README.md b/localizers/easynav_simple_localizer/README.md index bee15a05..8a06b1be 100644 --- a/localizers/easynav_simple_localizer/README.md +++ b/localizers/easynav_simple_localizer/README.md @@ -52,6 +52,7 @@ All parameters are declared under the plugin namespace, i.e., `//easyn | Direction | Topic | Type | Purpose | QoS | |---|---|---|---|---| | Subscription | `/odom` | `nav_msgs/msg/Odometry` | Read odometry when compute_odom_from_tf=false (not present here). | SensorDataQoS (reliable) | +| Subscription | `initialpose` | `geometry_msgs/msg/PoseWithCovarianceStamped` | Initialize particles pose to the received pose, using covariance. | depth=10 | | Publisher | `//particles` | `geometry_msgs/msg/PoseArray` | Publishes the current particle set. | depth=10 | | Publisher | `//pose` | `geometry_msgs/msg/PoseWithCovarianceStamped` | Estimated pose with covariance. | depth=10 | diff --git a/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp b/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp index 9ba5ebab..caf6fdd7 100644 --- a/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp @@ -155,6 +155,9 @@ class AMCLLocalizer : public LocalizerMethodBase /// Subscriber for odometry messages. rclcpp::Subscription::SharedPtr odom_sub_; + /// Subscriber for the initial pose. + rclcpp::Subscription::SharedPtr init_pose_sub_; + /** * @brief Callback for receiving odometry updates. * @@ -162,6 +165,13 @@ class AMCLLocalizer : public LocalizerMethodBase */ void odom_callback(nav_msgs::msg::Odometry::UniquePtr msg); + /** + * @brief Callback for receiving the initial pose. + * + * @param msg The incoming initial pose with covariance. + */ + void init_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr msg); + /// List of particles representing the belief distribution. std::vector particles_; diff --git a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp index 6fc0b92a..d391cb4b 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -22,7 +22,7 @@ #include "tf2/LinearMath/Vector3.hpp" #include "easynav_common/RTTFBuffer.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "easynav_simple_common/SimpleMap.hpp" #include "easynav_simple_localizer/AMCLLocalizer.hpp" @@ -243,7 +243,8 @@ AMCLLocalizer::on_initialize() tf_broadcaster_ = std::make_unique(get_node()); auto node_typed = std::dynamic_pointer_cast(get_node()); - auto rt_cbg = node_typed->get_real_time_cbg(); + auto rt_cbg = node_typed ? node_typed->get_real_time_cbg() : + get_node()->get_node_base_interface()->get_default_callback_group(); rclcpp::SubscriptionOptions options; options.callback_group = rt_cbg; @@ -256,6 +257,10 @@ AMCLLocalizer::on_initialize() estimate_pub_ = get_node()->create_publisher( node->get_fully_qualified_name() + std::string("/") + plugin_name + "/pose", 10); + init_pose_sub_ = get_node()->create_subscription( + "initialpose", 10, + std::bind(&AMCLLocalizer::init_pose_callback, this, std::placeholders::_1)); + last_reseed_ = get_node()->now(); last_input_time_ = get_node()->now(); @@ -314,6 +319,154 @@ AMCLLocalizer::odom_callback(nav_msgs::msg::Odometry::UniquePtr msg) } } +void +AMCLLocalizer::init_pose_callback( + geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr msg) +{ + if (particles_.empty()) { + return; + } + + auto logger = get_node()->get_logger(); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + + // Check expected frame (map) + const std::string expected_frame = tf_info.map_frame; + if (!msg->header.frame_id.empty() && msg->header.frame_id != expected_frame) { + RCLCPP_WARN( + logger, + "AMCLLocalizer::init_pose_callback: received initial pose in frame '%s' but expected '%s'. " + "Ignoring message.", + msg->header.frame_id.c_str(), expected_frame.c_str()); + return; + } + + last_input_time_ = msg->header.stamp; + + // Extract pose mean (x, y, yaw) expressed in map frame + const auto & pose = msg->pose.pose; + const double mean_x = pose.position.x; + const double mean_y = pose.position.y; + + tf2::Quaternion q( + pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + pose.orientation.w); + + double roll, pitch, yaw; + tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); + const double mean_yaw = yaw; + + // Extract 6x6 covariance (ROS order: x, y, z, roll, pitch, yaw) + const auto & cov = msg->pose.covariance; + + // Positional covariance terms + double var_x = cov[0]; + double cov_xy = cov[1]; + double var_y = cov[7]; + + // Yaw variance + double var_yaw = cov[35]; + + // Ensure non-negative variances + var_x = std::max(var_x, 0.0); + var_y = std::max(var_y, 0.0); + var_yaw = std::max(var_yaw, 0.0); + + // 2D Cholesky decomposition of covariance(x,y): + // [ var_x cov_xy ] + // [ cov_xy var_y ] + double l00 = std::sqrt(var_x); + double l10 = 0.0; + double l11 = 0.0; + + if (l00 > 0.0) { + l10 = cov_xy / l00; + double tmp = var_y - l10 * l10; + if (tmp < 0.0) { + tmp = 0.0; + } + l11 = std::sqrt(tmp); + } else { + // Degenerate case: fallback to diagonal stddevs + l00 = std::sqrt(var_x); + l10 = 0.0; + l11 = std::sqrt(var_y); + } + + // Enforce minimum translational noise if covariance is too small + const double std_xy = std::sqrt(0.5 * std::max(0.0, var_x + var_y)); + if (std_xy < min_noise_xy_) { + const double safe_std_xy = (std_xy > 1e-6) ? std_xy : 1.0; + const double scale = min_noise_xy_ / safe_std_xy; + l00 *= scale; + l10 *= scale; + l11 *= scale; + } + + // Yaw noise with minimum enforced + double yaw_stddev = std::sqrt(var_yaw); + yaw_stddev = std::max(yaw_stddev, min_noise_yaw_); + + std::normal_distribution standard_normal(0.0, 1.0); + std::normal_distribution yaw_noise(0.0, yaw_stddev); + + const std::size_t N = particles_.size(); + for (std::size_t i = 0; i < N; ++i) { + const double z0 = standard_normal(rng_); + const double z1 = standard_normal(rng_); + + const double dx = l00 * z0; + const double dy = l10 * z0 + l11 * z1; + + const double new_x = mean_x + dx; + const double new_y = mean_y + dy; + const double new_yaw = mean_yaw + yaw_noise(rng_); + + tf2::Quaternion q_particle; + q_particle.setRPY(0.0, 0.0, new_yaw); + + particles_[i].pose.setOrigin(tf2::Vector3(new_x, new_y, 0.0)); + particles_[i].pose.setRotation(q_particle); + + particles_[i].hits = 0; + particles_[i].possible_hits = 0; + particles_[i].weight = 1.0 / static_cast(N); + } + + // Normalize particle weights (safety) + double total_weight = 0.0; + for (const auto & p : particles_) { + total_weight += p.weight; + } + if (total_weight > 0.0) { + for (auto & p : particles_) { + p.weight /= total_weight; + } + } + + // Prevent immediate reseed after initialization + last_reseed_ = get_node()->now(); + + // Compute map->basefootprint estimate + tf2::Transform map2bf = getEstimatedPose(); + + // Publish TF if odom transform is already known + if (initialized_odom_) { + tf2::Transform map2odom = map2bf * odom_.inverse(); + publishTF(map2odom); + } + + publishEstimatedPose(map2bf); + publishParticles(); + + RCLCPP_INFO( + logger, + "AMCLLocalizer::init_pose_callback: reinitialized %zu particles around (%.3f, %.3f, %.3f)", + N, mean_x, mean_y, mean_yaw); +} + void AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state) { @@ -368,13 +521,12 @@ AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state) void AMCLLocalizer::correct(NavState & nav_state) { - if (!nav_state.has("points")) { - RCLCPP_WARN(get_node()->get_logger(), "There is yet no points perceptions"); + const auto & perceptions = nav_state.get_no_group(); + if (perceptions.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions"); return; } - const auto & perceptions = nav_state.get("points"); - if (!nav_state.has("map.static")) { RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.static map"); return; diff --git a/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp b/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp index d353bbea..36efd008 100644 --- a/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp +++ b/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp @@ -15,173 +15,167 @@ #include -#include "easynav_simple_common/SimpleMap.hpp" +#include +#include +#include +#include + #include "easynav_simple_localizer/AMCLLocalizer.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "std_srvs/srv/trigger.hpp" - -#include -#include +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -/// \brief Fixture for AMCLLocalizer tests (minimal) -class AMCLLocalizerTest : public ::testing::Test +namespace { -protected: - void SetUp() override - { - rclcpp::init(0, nullptr); - } - void TearDown() override - { - rclcpp::shutdown(); - } -}; - - -/// \brief Dynamic map update tests -TEST_F(AMCLLocalizerTest, BasicDynamicUpdate) +double yaw_from_quat(const geometry_msgs::msg::Quaternion & q) { - auto node = std::make_shared("test_node"); - auto manager = std::make_shared(); - manager->initialize(node, "test"); - - auto static_map = std::make_shared(); - static_map->initialize(30, 30, 0.1, -1.5, -1.5, 0.0); - manager->set_static_map(static_map); - - easynav::NavState navstate; - auto perception = std::make_shared(); - - perception->data.points.resize(6); - perception->data.points[0].x = 1.0; - perception->data.points[0].y = 1.0; - perception->data.points[0].z = 0.2; - perception->data.points[1].x = -1.0; - perception->data.points[1].y = -1.0; - perception->data.points[1].z = 0.2; - perception->data.points[2].x = -10.0; - perception->data.points[2].y = -1.0; - perception->data.points[2].z = 0.2; - perception->data.points[3].x = 10.0; - perception->data.points[3].y = -1.0; - perception->data.points[3].z = 0.2; - perception->data.points[4].x = 1.0; - perception->data.points[4].y = -10.0; - perception->data.points[4].z = 0.2; - perception->data.points[5].x = 1.0; - perception->data.points[5].y = 10.0; - perception->data.points[5].z = 0.2; - - perception->stamp = rclcpp::Time(0); - perception->frame_id = "map"; - perception->valid = true; - - navstate.set("perceptions", Perceptions()); - navstate.get_mutable("perceptions")->push_back(perception); - - manager->update(navstate); - - auto map_ptr = std::dynamic_pointer_cast(manager->get_dynamyc_map()); - ASSERT_TRUE(map_ptr != nullptr); - - auto cell1 = map_ptr->metric_to_cell(1.0, 1.0); - EXPECT_TRUE(map_ptr->at(cell1.first, cell1.second)); - - auto cell2 = map_ptr->metric_to_cell(-1.0, -1.0); - EXPECT_TRUE(map_ptr->at(cell2.first, cell2.second)); + tf2::Quaternion tf_q(q.x, q.y, q.z, q.w); + double roll = 0.0, pitch = 0.0, yaw = 0.0; + tf2::Matrix3x3(tf_q).getRPY(roll, pitch, yaw); + return yaw; } -/// \brief Map loading via subscription test -TEST_F(AMCLLocalizerTest, IncomingOccupancyGridUpdatesMaps) +double yaw_from_tf(const tf2::Transform & tf) { - auto node = std::make_shared("test_node2"); - auto manager = std::make_shared(); - easynav::TFInfo tf_info; - tf_info.map_frame = "world_map"; - tf_info.odom_frame = "world_odom"; - tf_info.robot_frame = "world_base"; - tf_info.robot_footprint_frame = "world_footprint_base"; - easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); - - manager->initialize(node, "test2"); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); - - rclcpp::Publisher::SharedPtr pub = - node->create_publisher( - "test_node2/test2/incoming_map", rclcpp::QoS(1).transient_local().reliable()); - - nav_msgs::msg::OccupancyGrid grid; - grid.header.frame_id = tf_info.map_frame; - grid.info.width = 10; - grid.info.height = 10; - grid.info.resolution = 0.2; - grid.info.origin.position.x = -1.0; - grid.info.origin.position.y = -0.6; - grid.data.assign(100, 0); - grid.data[55] = 100; // Mark cell (5,5) as occupied - - pub->publish(grid); - - executor.spin_some(); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - - auto static_map = std::dynamic_pointer_cast(manager->get_static_map()); - ASSERT_TRUE(static_map != nullptr); - - EXPECT_EQ(static_map->at(5, 5), 1); - EXPECT_EQ(static_map->at(1, 1), 0); + double roll = 0.0, pitch = 0.0, yaw = 0.0; + tf2::Matrix3x3(tf.getRotation()).getRPY(roll, pitch, yaw); + return yaw; } -class FriendAMCLLocalizer : public easynav::AMCLLocalizer { +class FriendAMCLLocalizer : public easynav::AMCLLocalizer +{ public: - void force_path(const std::string & path) {map_path_ = path;} + using easynav::AMCLLocalizer::init_pose_sub_; }; -TEST_F(AMCLLocalizerTest, SavemapServiceWorks) +class AMCLLocalizerInitialPoseTest : public ::testing::Test { - auto node = std::make_shared("test_savemap_node"); - auto manager = std::make_shared(); - manager->initialize(node, "test_savemap"); - - auto static_map = std::make_shared(); - static_map->initialize(4, 4, 0.5, -1.0, -1.0, 0); - static_map->at(1, 1) = true; - static_map->at(2, 2) = true; - manager->set_static_map(static_map); - - const std::string test_map_file = "/tmp/savemap_test_map.txt"; - const std::string service_name = "/test_savemap_node/test_savemap/savemap"; +protected: + void SetUp() override {rclcpp::init(0, nullptr);} + void TearDown() override {rclcpp::shutdown();} +}; - std::static_pointer_cast(manager)->force_path(test_map_file); +} // namespace - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); +TEST_F(AMCLLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackGroup) +{ + const double x0 = 0.3; + const double y0 = 1.7; + const double yaw0 = -0.8; + + const double x1 = 2.2; + const double y1 = -0.4; + const double yaw1 = 1.1; + + rclcpp::NodeOptions options; + options.parameter_overrides({ + rclcpp::Parameter("test.num_particles", 100), + rclcpp::Parameter("test.initial_pose.x", x0), + rclcpp::Parameter("test.initial_pose.y", y0), + rclcpp::Parameter("test.initial_pose.yaw", yaw0), + rclcpp::Parameter("test.initial_pose.std_dev_xy", 0.0), + rclcpp::Parameter("test.initial_pose.std_dev_yaw", 0.0), + rclcpp::Parameter("test.min_noise_xy", 0.0), + rclcpp::Parameter("test.min_noise_yaw", 0.0), + }); + + auto node = std::make_shared( + "test_simple_localizer_node", options); + auto localizer = std::make_shared(); + + localizer->initialize(node, "test"); + + ASSERT_NE(localizer->init_pose_sub_, nullptr); - auto client = node->create_client(service_name); - ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); + { + const tf2::Transform tf = localizer->getEstimatedPose(); + EXPECT_NEAR(tf.getOrigin().x(), x0, 1e-9); + EXPECT_NEAR(tf.getOrigin().y(), y0, 1e-9); + EXPECT_NEAR(yaw_from_tf(tf), yaw0, 1e-9); + } - auto request = std::make_shared(); - auto future = client->async_send_request(request); - executor.spin_until_future_complete(future); + { + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + EXPECT_NEAR(odom.pose.pose.position.x, x0, 1e-9); + EXPECT_NEAR(odom.pose.pose.position.y, y0, 1e-9); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw0, 1e-9); + } - auto response = future.get(); - EXPECT_TRUE(response->success); - EXPECT_NE(response->message.find("saved"), std::string::npos); + const auto infos = node->get_subscriptions_info_by_topic("initialpose"); + ASSERT_FALSE(infos.empty()); + + const bool has_expected_type = std::any_of( + infos.begin(), infos.end(), + [](const rclcpp::TopicEndpointInfo & info) { + return info.topic_type() == "geometry_msgs/msg/PoseWithCovarianceStamped"; + }); + EXPECT_TRUE(has_expected_type); + + auto pub_node = std::make_shared("initialpose_pub_node"); + auto pub = pub_node->create_publisher( + "initialpose", 10); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(node->get_node_base_interface()); + exec.add_node(pub_node->get_node_base_interface()); + + geometry_msgs::msg::PoseWithCovarianceStamped msg; + msg.header.stamp = pub_node->now(); + msg.header.frame_id = "map"; + msg.pose.pose.position.x = x1; + msg.pose.pose.position.y = y1; + msg.pose.pose.position.z = 0.0; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, yaw1); + msg.pose.pose.orientation = tf2::toMsg(q); + msg.pose.covariance.fill(0.0); - std::ifstream infile(test_map_file); - ASSERT_TRUE(infile.is_open()); + { + const auto connect_deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(200); + while (std::chrono::steady_clock::now() < connect_deadline && + pub->get_subscription_count() == 0) + { + exec.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + pub->publish(msg); + + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500); + while (std::chrono::steady_clock::now() < deadline) { + exec.spin_some(); + const tf2::Transform tf = localizer->getEstimatedPose(); + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + + const bool pose_ok = + std::abs(tf.getOrigin().x() - x1) < 1e-9 && + std::abs(tf.getOrigin().y() - y1) < 1e-9 && + std::abs(yaw_from_tf(tf) - yaw1) < 1e-9 && + std::abs(odom.pose.pose.position.x - x1) < 1e-9 && + std::abs(odom.pose.pose.position.y - y1) < 1e-9 && + std::abs(yaw_from_quat(odom.pose.pose.orientation) - yaw1) < 1e-9; + + if (pose_ok) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } - std::string first_line; - std::getline(infile, first_line); - EXPECT_NE(first_line.find("4 4"), std::string::npos); - EXPECT_NE(first_line.find("0.5"), std::string::npos); + { + const tf2::Transform tf = localizer->getEstimatedPose(); + EXPECT_NEAR(tf.getOrigin().x(), x1, 1e-9); + EXPECT_NEAR(tf.getOrigin().y(), y1, 1e-9); + EXPECT_NEAR(yaw_from_tf(tf), yaw1, 1e-9); + } - infile.close(); - std::remove(test_map_file.c_str()); + { + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + EXPECT_NEAR(odom.pose.pose.position.x, x1, 1e-9); + EXPECT_NEAR(odom.pose.pose.position.y, y1, 1e-9); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw1, 1e-9); + } } diff --git a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp index b354f31e..69e41672 100644 --- a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp +++ b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp @@ -18,7 +18,7 @@ #include "easynav_costmap_common/costmap_2d.hpp" #include "easynav_common/types/NavState.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "easynav_costmap_common/costmap_2d.hpp" #include "easynav_costmap_common/cost_values.hpp" @@ -41,12 +41,12 @@ ObstacleFilter::on_initialize() void ObstacleFilter::update(NavState & nav_state) { - if (!nav_state.has("points")) { + const auto & perceptions = nav_state.get_no_group(); + if (perceptions.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions"); return; } - const auto & perceptions = nav_state.get("points"); - auto dynamic_map_ptr = nav_state.get_ptr("map.dynamic.filtered"); Costmap2D & dynamic_map = *dynamic_map_ptr; const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); diff --git a/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp b/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp index f14df242..8440f0c2 100644 --- a/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp +++ b/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp @@ -15,7 +15,7 @@ #include -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "easynav_common/types/NavState.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/maps_managers/easynav_navmap_maps_manager/README.md b/maps_managers/easynav_navmap_maps_manager/README.md index 9f925d2f..f019a211 100644 --- a/maps_managers/easynav_navmap_maps_manager/README.md +++ b/maps_managers/easynav_navmap_maps_manager/README.md @@ -59,7 +59,7 @@ All parameters are declared under the plugin namespace, i.e. | `.height_threshold` | `double` | `0.25` | Minimum vertical height (in meters) between max and min z to mark as an obstacle. | | `.downsample` | `double` | `0.3` | Voxel size used to downsample point clouds before obstacle detection. | | `.fuse_frame` | `string` | `"map"` | Frame in which points are fused before projection into NavMap. | -| **Input Key:** | | | Reads point clouds from `NavState` key `"points"`. | +| **Input Key:** | | | Reads list of point clouds from `NavState` group `"points"`. | | **Output Layer:** | | | Updates or creates NavMap layer `"obstacles"`. | #### **InflationFilter** diff --git a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp index aeeed492..87532a09 100644 --- a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp +++ b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp @@ -18,7 +18,7 @@ #include #include "easynav_common/types/NavState.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "easynav_common/RTTFBuffer.hpp" #include "navmap_core/NavMap.hpp" @@ -44,9 +44,13 @@ ObstacleFilter::on_initialize() void ObstacleFilter::update(::easynav::NavState & nav_state) { if (!nav_state.has("map.navmap")) {return;} - if (!nav_state.has("points")) {return;} - const auto & perceptions = nav_state.get("points"); + const auto & perceptions = nav_state.get_no_group(); + if (perceptions.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions"); + return; + } + navmap_ = nav_state.get<::navmap::NavMap>("map.navmap"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); diff --git a/maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_tests.cpp b/maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_tests.cpp index d06b8904..4c55b8fe 100644 --- a/maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_tests.cpp +++ b/maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_tests.cpp @@ -15,7 +15,7 @@ #include -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "easynav_common/types/NavState.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/InflationFilter.cpp b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/InflationFilter.cpp index 0703a15f..09a13363 100644 --- a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/InflationFilter.cpp +++ b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/InflationFilter.cpp @@ -42,8 +42,8 @@ #include #include "easynav_common/types/NavState.hpp" -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/Perceptions.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "octomap_core/Octomap.hpp" diff --git a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/ObstacleFilter.cpp b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/ObstacleFilter.cpp index 33a1abb9..105ab371 100644 --- a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/ObstacleFilter.cpp +++ b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/ObstacleFilter.cpp @@ -17,8 +17,8 @@ #include #include "easynav_common/types/NavState.hpp" -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/Perceptions.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "octomap_core/Octomap.hpp" @@ -45,11 +45,13 @@ ObstacleFilter::update(::easynav::NavState & nav_state) if (!nav_state.has("map")) { return; } - if (!nav_state.has("points")) { + + const auto & perceptions = nav_state.get_no_group(); + if (perceptions.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions"); return; } - const auto & perceptions = nav_state.get("points"); octomap_ = nav_state.get<::octomap::Octomap>("map"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); diff --git a/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/RoutesMapsManager.cpp b/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/RoutesMapsManager.cpp index 4af7b510..3c0645f3 100644 --- a/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/RoutesMapsManager.cpp +++ b/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/RoutesMapsManager.cpp @@ -74,11 +74,12 @@ void RoutesMapsManager::on_initialize() map_path_.clear(); if (package_name.empty() && map_path_file.empty()) { - // Accepted: we will use a default in-memory route in load_routes_from_yaml(). + // No path configured: default to /tmp so that save_routes still works. + map_path_ = "/tmp/routes.yaml"; RCLCPP_INFO( node->get_logger(), - "[%s] No package or map_path_file specified, using default in-memory route", - plugin_name.c_str()); + "[%s] No package or map_path_file specified, routes will be saved to %s", + plugin_name.c_str(), map_path_.c_str()); } else if (!map_path_file.empty() && map_path_file[0] == '/') { // Absolute path: ignore package_name. map_path_ = map_path_file; @@ -161,11 +162,16 @@ void RoutesMapsManager::on_initialize() out << YAML::EndMap; std::ofstream file(map_path_); + if (!file.is_open()) { + response->success = false; + response->message = "Could not open file for writing: " + map_path_; + return; + } file << out.c_str(); file.close(); response->success = true; - response->message = "Routes saved"; + response->message = "Routes saved to " + map_path_; } catch (const std::exception & e) { response->success = false; response->message = e.what(); diff --git a/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp b/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp index 9241f3ce..76751621 100644 --- a/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp +++ b/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp @@ -17,7 +17,7 @@ /// \brief Implementation of the SimpleMapsManager class. #include "easynav_simple_maps_manager/SimpleMapsManager.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" #include "ament_index_cpp/get_package_prefix.hpp" @@ -138,13 +138,17 @@ SimpleMapsManager::update(NavState & nav_state) dynamic_map_.deep_copy(static_map_); - if (!nav_state.has("points")) { + const auto & perceptions = nav_state.get_no_group(); + if (perceptions.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "There are no points perceptions"); + } + + if (perceptions.empty()) { nav_state.set("map.static", static_map_); nav_state.set("map.dynamic", dynamic_map_); return; } - const auto & perceptions = nav_state.get("points"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); rclcpp::Time stamp; diff --git a/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp b/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp index 82c67758..e6adb415 100644 --- a/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp +++ b/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp @@ -17,7 +17,7 @@ #include "easynav_simple_common/SimpleMap.hpp" #include "easynav_common/RTTFBuffer.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_sensors/types/PointPerception.hpp" #include "easynav_simple_maps_manager/SimpleMapsManager.hpp" #include "rclcpp/rclcpp.hpp" @@ -68,7 +68,7 @@ TEST_F(SimpleMapsManagerTest, BasicDynamicUpdate) easynav::NavState navstate; auto perception = std::make_shared(); - navstate.set("points", easynav::PointPerceptions()); + navstate.set("laser", easynav::PointPerception()); perception->data.points.resize(6); perception->data.points[0].x = 1.0; @@ -94,9 +94,8 @@ TEST_F(SimpleMapsManagerTest, BasicDynamicUpdate) perception->frame_id = "map"; perception->valid = true; - easynav::PointPerceptions perceptions; - perceptions.push_back(perception); - navstate.set("points", perceptions); + navstate.set("laser", perception); + // navstate.set_group("points", {"laser"}); manager->update(navstate); diff --git a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp index 4062a6d8..f6bfdf20 100644 --- a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp +++ b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp @@ -157,12 +157,12 @@ void CostmapPlanner::update(NavState & nav_state) rclcpp::Time latest_stamp = nav_state.get("map_time"); if (rclcpp::Time(robot_pose.header.stamp, latest_stamp.get_clock_type()) > latest_stamp) { - latest_stamp = robot_pose.header.stamp; + latest_stamp = rclcpp::Time(robot_pose.header.stamp, latest_stamp.get_clock_type()); } if (rclcpp::Time(goals.goals.front().header.stamp, latest_stamp.get_clock_type()) > latest_stamp) { - latest_stamp = goals.goals.front().header.stamp; + latest_stamp = rclcpp::Time(goals.goals.front().header.stamp, latest_stamp.get_clock_type()); } current_path_.header.stamp = latest_stamp; diff --git a/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp b/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp index a619ecc8..b869c68c 100644 --- a/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp +++ b/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp @@ -130,7 +130,8 @@ SimplePlanner::update(NavState & nav_state) const auto & goal = goals.goals.front().pose; const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); - rclcpp::Time latest_stamp = robot_pose.header.stamp; + const auto clock_type = get_node()->get_clock()->get_clock_type(); + rclcpp::Time latest_stamp(robot_pose.header.stamp, clock_type); if (rclcpp::Time(goals.goals.front().header.stamp, latest_stamp.get_clock_type()) > latest_stamp) {