diff --git a/.circleci/config.yml b/.circleci/config.yml index 841d60042d1..94e08c4f15e 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -33,12 +33,12 @@ _commands: - restore_cache: name: Restore Cache << parameters.key >> keys: - - "<< parameters.key >>-v39\ + - "<< parameters.key >>-v40\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" - - "<< parameters.key >>-v39\ + - "<< parameters.key >>-v40\ -{{ arch }}\ -main\ -\ @@ -532,7 +532,7 @@ jobs: _parameters: release_parameters: &release_parameters - packages_skip_regex: "'(nav2_system_tests|nav2_smac_planner|nav2_mppi_controller|nav2_route|nav2_rviz_plugins|nav2_rotation_shim_controller|nav2_waypoint_follower|nav2_smoother|opennav_docking|nav2_behaviors|nav2_bringup|navigation2)'" + packages_skip_regex: "'(nav2_system_tests|nav2_smac_planner|nav2_mppi_controller|nav2_route|nav2_rviz_plugins|nav2_rotation_shim_controller|nav2_waypoint_follower|nav2_smoother|opennav_docking|opennav_following|nav2_behaviors|nav2_bringup|navigation2)'" workflows: version: 2 diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 851a660ab66..1827fb5665b 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -261,6 +261,12 @@ list(APPEND plugin_libs nav2_compute_route_bt_node) add_library(nav2_toggle_collision_monitor_service_bt_node SHARED plugins/action/toggle_collision_monitor_service.cpp) list(APPEND plugin_libs nav2_toggle_collision_monitor_service_bt_node) +add_library(opennav_follow_action_bt_node SHARED plugins/action/follow_object_action.cpp) +list(APPEND plugin_libs opennav_follow_action_bt_node) + +add_library(opennav_follow_cancel_bt_node SHARED plugins/action/follow_object_cancel_node.cpp) +list(APPEND plugin_libs opennav_follow_cancel_bt_node) + foreach(bt_plugin ${plugin_libs}) target_include_directories(${bt_plugin} PRIVATE diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index b10e8c32040..09becc161fa 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -67,6 +67,7 @@ BtActionServer::BtActionServer( "compute_path", "dock_robot", "drive_on_heading", + "follow_object", "follow_path", "nav_thru_poses", "nav_to_pose", diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_object_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_object_action.hpp new file mode 100644 index 00000000000..f89fbfb7060 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_object_action.hpp @@ -0,0 +1,98 @@ +// Copyright (c) 2024 Open Navigation LLC +// Copyright (c) 2024 Alberto J. Tudela Roldán +// +// 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. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_OBJECT_ACTION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_OBJECT_ACTION_HPP_ + +#include +#include +#include + +#include "nav2_behavior_tree/bt_action_node.hpp" +#include "nav2_msgs/action/follow_object.hpp" + + +namespace nav2_behavior_tree +{ + +/** + * @brief nav2_behavior_tree::BtActionNode class that wraps nav2_msgs/FollowObject + */ +class FollowObjectAction + : public nav2_behavior_tree::BtActionNode +{ + using Action = nav2_msgs::action::FollowObject; + using ActionResult = Action::Result; + +public: + /** + * @brief A constructor for nav2_behavior_tree::FollowObjectAction + * @param xml_tag_name Name for the XML tag for this node + * @param action_name Action name this node creates a client for + * @param conf BT node configuration + */ + FollowObjectAction( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf); + + /** + * @brief Function to perform some user-defined operation on tick + */ + void on_tick() override; + + /** + * @brief Function to perform some user-defined operation upon successful completion of the action + */ + BT::NodeStatus on_success() override; + + /** + * @brief Function to perform some user-defined operation upon abortion of the action + */ + BT::NodeStatus on_aborted() override; + + /** + * @brief Function to perform some user-defined operation upon cancellation of the action + */ + BT::NodeStatus on_cancelled() override; + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ + static BT::PortsList providedPorts() + { + return providedBasicPorts( + { + BT::InputPort( + "pose_topic", "dynamic_pose", "Topic to publish the pose of the object to follow"), + BT::InputPort( + "tracked_frame", "Target frame to follow (Optional, used if pose_topic is not set)"), + BT::InputPort( + "max_duration", 0.0, "The maximum duration to follow the object (Optional)"), + + BT::OutputPort( + "total_elapsed_time", "Total elapsed time"), + BT::OutputPort( + "error_code_id", "Error code"), + BT::OutputPort( + "error_msg", "Error message"), + }); + } +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_OBJECT_ACTION_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_object_cancel_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_object_cancel_node.hpp new file mode 100644 index 00000000000..5503f65d7ba --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_object_cancel_node.hpp @@ -0,0 +1,61 @@ +// Copyright (c) 2024 Open Navigation LLC +// Copyright (c) 2024 Alberto J. Tudela Roldán +// +// 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. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_OBJECT_CANCEL_NODE_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_OBJECT_CANCEL_NODE_HPP_ + +#include +#include + +#include "nav2_behavior_tree/bt_cancel_action_node.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" +#include "nav2_msgs/action/follow_object.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::FollowObject + */ +class FollowObjectCancel + : public nav2_behavior_tree::BtCancelActionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::FollowObject + * @param xml_tag_name Name for the XML tag for this node + * @param action_name Action name this node creates a client for + * @param conf BT node configuration + */ + FollowObjectCancel( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf); + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ + static BT::PortsList providedPorts() + { + return providedBasicPorts( + { + }); + } +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_OBJECT_CANCEL_NODE_HPP_ diff --git a/nav2_behavior_tree/plugins/action/follow_object_action.cpp b/nav2_behavior_tree/plugins/action/follow_object_action.cpp new file mode 100644 index 00000000000..8b9ae19c5f8 --- /dev/null +++ b/nav2_behavior_tree/plugins/action/follow_object_action.cpp @@ -0,0 +1,82 @@ +// Copyright (c) 2024 Open Navigation LLC +// Copyright (c) 2024 Alberto J. Tudela Roldán +// +// 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 "nav2_behavior_tree/plugins/action/follow_object_action.hpp" + +namespace nav2_behavior_tree +{ + +FollowObjectAction::FollowObjectAction( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) +: BtActionNode(xml_tag_name, action_name, conf) +{ +} + +void FollowObjectAction::on_tick() +{ + // Get core inputs about what to perform + getInput("pose_topic", goal_.pose_topic); + getInput("tracked_frame", goal_.tracked_frame); + double max_duration; + getInput("max_duration", max_duration); + + // Populate the input message + goal_.max_duration = rclcpp::Duration::from_seconds(max_duration); +} + +BT::NodeStatus FollowObjectAction::on_success() +{ + setOutput("total_elapsed_time", result_.result->total_elapsed_time); + setOutput("error_code_id", ActionResult::NONE); + setOutput("error_msg", ""); + return BT::NodeStatus::SUCCESS; +} + +BT::NodeStatus FollowObjectAction::on_aborted() +{ + setOutput("total_elapsed_time", result_.result->total_elapsed_time); + setOutput("error_code_id", result_.result->error_code); + setOutput("error_msg", result_.result->error_msg); + return BT::NodeStatus::FAILURE; +} + +BT::NodeStatus FollowObjectAction::on_cancelled() +{ + setOutput("total_elapsed_time", result_.result->total_elapsed_time); + setOutput("error_code_id", ActionResult::NONE); + setOutput("error_msg", ""); + return BT::NodeStatus::SUCCESS; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "follow_object", config); + }; + + factory.registerBuilder( + "FollowObject", builder); +} diff --git a/nav2_behavior_tree/plugins/action/follow_object_cancel_node.cpp b/nav2_behavior_tree/plugins/action/follow_object_cancel_node.cpp new file mode 100644 index 00000000000..62b6862e41d --- /dev/null +++ b/nav2_behavior_tree/plugins/action/follow_object_cancel_node.cpp @@ -0,0 +1,47 @@ +// Copyright (c) 2024 Open Navigation LLC +// Copyright (c) 2024 Alberto J. Tudela Roldán +// +// 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 "nav2_behavior_tree/plugins/action/follow_object_cancel_node.hpp" + +namespace nav2_behavior_tree +{ + +FollowObjectCancel::FollowObjectCancel( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) +: nav2_behavior_tree::BtCancelActionNode( + xml_tag_name, action_name, conf) +{ +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "follow_object", config); + }; + + factory.registerBuilder( + "CancelFollowObject", builder); +} diff --git a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt index 5bd9e2c69e6..56f144fc4f1 100644 --- a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt @@ -94,3 +94,7 @@ plugin_add_test(test_progress_checker_selector_node test_progress_checker_select plugin_add_test(test_toggle_collision_monitor_service test_toggle_collision_monitor_service.cpp nav2_toggle_collision_monitor_service_bt_node) + +plugin_add_test(test_follow_object_action test_follow_object_action.cpp opennav_follow_action_bt_node) + +plugin_add_test(test_follow_object_cancel_node test_follow_object_cancel_node.cpp opennav_follow_cancel_bt_node) diff --git a/nav2_behavior_tree/test/plugins/action/test_follow_object_action.cpp b/nav2_behavior_tree/test/plugins/action/test_follow_object_action.cpp new file mode 100644 index 00000000000..e945f03d082 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_follow_object_action.cpp @@ -0,0 +1,186 @@ +// Copyright (c) 2024 Open Navigation LLC +// Copyright (c) 2024 Alberto J. Tudela Roldán +// +// 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 "behaviortree_cpp/bt_factory.h" + +#include "nav2_behavior_tree/utils/test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/follow_object_action.hpp" + +class FollowObjectActionServer + : public TestActionServer +{ +public: + FollowObjectActionServer() + : TestActionServer("follow_object") + {} + +protected: + void execute( + const typename std::shared_ptr< + rclcpp_action::ServerGoalHandle> goal_handle) + override + { + const auto goal = goal_handle->get_goal(); + auto result = std::make_shared(); + goal_handle->succeed(result); + } +}; + +class FollowObjectActionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("follow_object_action_test_fixture"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set( + "node", node_); + config_->blackboard->set( + "server_timeout", std::chrono::milliseconds(20)); + config_->blackboard->set( + "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", std::chrono::milliseconds(1000)); + config_->blackboard->set("initial_pose_received", false); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "follow_object", config); + }; + + factory_->registerBuilder("FollowObject", builder); + } + + static void TearDownTestCase() + { + factory_.reset(); + action_server_.reset(); + delete config_; + config_ = nullptr; + node_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + + static std::shared_ptr action_server_; + +protected: + static nav2::LifecycleNode::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +nav2::LifecycleNode::SharedPtr FollowObjectActionTestFixture::node_ = nullptr; +std::shared_ptr +FollowObjectActionTestFixture::action_server_ = nullptr; +BT::NodeConfiguration * FollowObjectActionTestFixture::config_ = nullptr; +std::shared_ptr FollowObjectActionTestFixture::factory_ = nullptr; +std::shared_ptr FollowObjectActionTestFixture::tree_ = nullptr; + +TEST_F(FollowObjectActionTestFixture, test_ports) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + EXPECT_EQ(tree_->rootNode()->getInput("max_duration"), 0.0); + + xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + EXPECT_EQ(tree_->rootNode()->getInput("pose_topic"), "test_topic"); + EXPECT_EQ(tree_->rootNode()->getInput("max_duration"), 20.0); + EXPECT_EQ(tree_->rootNode()->getInput("tracked_frame"), "test_frame"); +} + +TEST_F(FollowObjectActionTestFixture, test_tick) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + geometry_msgs::msg::PoseStamped pose; + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // the goal should have reached our server + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + + // halt node so another goal can be sent + tree_->haltTree(); + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize action server and spin on new thread + FollowObjectActionTestFixture::action_server_ = + std::make_shared(); + + std::thread server_thread([]() { + rclcpp::spin(FollowObjectActionTestFixture::action_server_); + }); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + server_thread.join(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/plugins/action/test_follow_object_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_follow_object_cancel_node.cpp new file mode 100644 index 00000000000..59dcf486f62 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_follow_object_cancel_node.cpp @@ -0,0 +1,173 @@ +// Copyright (c) 2022 Joshua Wallace +// Copyright (c) 2024 Alberto J. Tudela Roldán +// +// 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 "behaviortree_cpp/bt_factory.h" + +#include "lifecycle_msgs/srv/change_state.hpp" +#include "nav2_behavior_tree/utils/test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/follow_object_cancel_node.hpp" + +class CancelFollowObjectServer + : public TestActionServer +{ +public: + CancelFollowObjectServer() + : TestActionServer("follow_object") + {} + +protected: + void execute( + const typename std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle) + { + while (!goal_handle->is_canceling()) { + // Assisted Teleop here until goal cancels + std::this_thread::sleep_for(std::chrono::milliseconds(15)); + } + } +}; + +class CancelFollowObjectActionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("cancel_follow_object_action_test_fixture"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set("node", node_); + config_->blackboard->set( + "server_timeout", std::chrono::milliseconds(20)); + config_->blackboard->set( + "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", std::chrono::milliseconds(1000)); + client_ = rclcpp_action::create_client( + node_, "follow_object"); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "follow_object", config); + }; + + factory_->registerBuilder( + "CancelFollowObject", builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + action_server_.reset(); + client_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + + static std::shared_ptr action_server_; + static std::shared_ptr> client_; + +protected: + static nav2::LifecycleNode::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +nav2::LifecycleNode::SharedPtr CancelFollowObjectActionTestFixture::node_ = nullptr; +std::shared_ptr +CancelFollowObjectActionTestFixture::action_server_ = nullptr; +std::shared_ptr> +CancelFollowObjectActionTestFixture::client_ = nullptr; + +BT::NodeConfiguration * CancelFollowObjectActionTestFixture::config_ = nullptr; +std::shared_ptr +CancelFollowObjectActionTestFixture::factory_ = nullptr; +std::shared_ptr CancelFollowObjectActionTestFixture::tree_ = nullptr; + +TEST_F(CancelFollowObjectActionTestFixture, test_ports) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + auto send_goal_options = nav2::ActionClient< + nav2_msgs::action::FollowObject>::SendGoalOptions(); + + // Creating a dummy goal_msg + auto goal_msg = nav2_msgs::action::FollowObject::Goal(); + + // BackUping for server and sending a goal + client_->wait_for_action_server(); + client_->async_send_goal(goal_msg, send_goal_options); + + // Adding a sleep so that the goal is indeed older than 10ms as described in our abstract class + std::this_thread::sleep_for(std::chrono::milliseconds(15)); + + // Executing tick + tree_->rootNode()->executeTick(); + + // BT node should return success, once when the goal is cancelled + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + + // Adding another test case to check if the goal is in fact cancelling + EXPECT_EQ(action_server_->isGoalCancelled(), true); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize action server and back_up on new thread + CancelFollowObjectActionTestFixture::action_server_ = + std::make_shared(); + std::thread server_thread([]() { + rclcpp::spin(CancelFollowObjectActionTestFixture::action_server_); + }); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + server_thread.join(); + + return all_successful; +} diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py index 2aa605617e4..9aafe73c349 100644 --- a/nav2_bringup/launch/navigation_launch.py +++ b/nav2_bringup/launch/navigation_launch.py @@ -52,6 +52,7 @@ def generate_launch_description() -> LaunchDescription: 'bt_navigator', 'waypoint_follower', 'docking_server', + 'following_server', ] # Map fully qualified names to relative ones so the node's namespace can be prepended. @@ -261,6 +262,17 @@ def generate_launch_description() -> LaunchDescription: arguments=['--ros-args', '--log-level', log_level], remappings=remappings, ), + Node( + package='opennav_following', + executable='opennav_following', + name='following_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings, + ), Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', @@ -350,6 +362,13 @@ def generate_launch_description() -> LaunchDescription: parameters=[configured_params], remappings=remappings, ), + ComposableNode( + package='opennav_following', + plugin='opennav_following::FollowingServer', + name='following_server', + parameters=[configured_params], + remappings=remappings, + ), ComposableNode( package='nav2_lifecycle_manager', plugin='nav2_lifecycle_manager::LifecycleManager', diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index f1d959f4873..e6521258ffc 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -75,6 +75,7 @@ bt_navigator: - compute_path - dock_robot - drive_on_heading + - follow_object - follow_path - nav_thru_poses - nav_to_pose diff --git a/nav2_following/opennav_following/CMakeLists.txt b/nav2_following/opennav_following/CMakeLists.txt new file mode 100644 index 00000000000..d30d4f60bc9 --- /dev/null +++ b/nav2_following/opennav_following/CMakeLists.txt @@ -0,0 +1,104 @@ +cmake_minimum_required(VERSION 3.5) +project(opennav_following) + +find_package(ament_cmake REQUIRED) +find_package(angles REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav2_common REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(nav2_ros_common REQUIRED) +find_package(nav2_util REQUIRED) +find_package(opennav_docking REQUIRED) +find_package(opennav_docking_core REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) + +nav2_package() + +set(executable_name opennav_following) +set(library_name ${executable_name}_core) + +add_library(${library_name} SHARED + src/following_server.cpp +) +target_include_directories(${library_name} + PUBLIC + "$" + "$" + "$" +) +target_link_libraries(${library_name} PUBLIC + ${geometry_msgs_TARGETS} + nav2_util::nav2_util_core + ${nav2_msgs_TARGETS} + opennav_docking::controller + opennav_docking::pose_filter + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + tf2_ros::tf2_ros + PRIVATE + angles::angles + opennav_docking_core::opennav_docking_core +) +target_link_libraries(${library_name} PRIVATE + rclcpp_components::component + tf2_geometry_msgs::tf2_geometry_msgs +) + +add_executable(${executable_name} + src/main.cpp +) +target_include_directories(${executable_name} + PUBLIC + "$" + "$" +) +target_link_libraries(${executable_name} PRIVATE ${library_name} rclcpp::rclcpp) + +rclcpp_components_register_nodes(${library_name} "opennav_following::FollowingServer") + +install(TARGETS ${library_name} + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(TARGETS ${executable_name} + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_pytest REQUIRED) + find_package(ament_index_cpp REQUIRED) + + ament_lint_auto_find_test_dependencies() + ament_find_gtest() + add_subdirectory(test) +endif() + +ament_export_include_directories(include/${PROJECT_NAME}) +ament_export_libraries(${library_name}) +ament_export_dependencies( + geometry_msgs + nav2_msgs + nav2_ros_common + nav2_util + opennav_docking + opennav_docking_core + rclcpp + rclcpp_lifecycle + tf2_ros +) +ament_export_targets(${PROJECT_NAME}) +ament_package() diff --git a/nav2_following/opennav_following/include/opennav_following/following_server.hpp b/nav2_following/opennav_following/include/opennav_following/following_server.hpp new file mode 100644 index 00000000000..5ff2d0b214b --- /dev/null +++ b/nav2_following/opennav_following/include/opennav_following/following_server.hpp @@ -0,0 +1,285 @@ +// Copyright (c) 2024 Open Navigation LLC +// Copyright (c) 2024 Alberto J. Tudela Roldán +// +// 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. + +#ifndef OPENNAV_FOLLOWING__FOLLOWING_SERVER_HPP_ +#define OPENNAV_FOLLOWING__FOLLOWING_SERVER_HPP_ + +#include +#include +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_publisher.hpp" +#include "nav2_msgs/action/follow_object.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" +#include "nav2_ros_common/node_utils.hpp" +#include "nav2_ros_common/simple_action_server.hpp" +#include "nav2_util/twist_publisher.hpp" +#include "nav2_util/odometry_utils.hpp" +#include "opennav_docking/controller.hpp" +#include "opennav_docking/pose_filter.hpp" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" + +namespace opennav_following +{ +/** + * @class opennav_following::FollowingServer + * @brief An action server which implements a dynamic following behavior + */ +class FollowingServer : public nav2::LifecycleNode +{ +public: + using FollowObject = nav2_msgs::action::FollowObject; + using FollowingActionServer = nav2::SimpleActionServer; + + /** + * @brief A constructor for opennav_following::FollowingServer + * @param options Additional options to control creation of the node. + */ + explicit FollowingServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + + /** + * @brief A destructor for opennav_following::FollowingServer + */ + ~FollowingServer() = default; + + /** + * @brief Publish feedback from a following action. + * @param state Current state - should be one of those defined in message. + */ + void publishFollowingFeedback(uint16_t state); + + /** + * @brief Use control law and perception to approach the object. + * @param object_pose Initial object pose, will be refined by perception. + * @param target_frame The frame to be tracked instead of the pose. + * @returns True if successfully approached, False if cancelled. For + * any internal error, will throw. + */ + virtual bool approachObject( + geometry_msgs::msg::PoseStamped & object_pose, + const std::string & target_frame = std::string("")); + + /** + * @brief Rotate the robot to find the object again. + * @param object_pose The last known object pose. + * @param target_frame The frame to be tracked instead of the pose. + * @returns True if successful. + */ + virtual bool rotateToObject( + geometry_msgs::msg::PoseStamped & object_pose, + const std::string & target_frame = std::string("")); + + /** + * @brief Gets a preempted goal if immediately requested + * @param Goal goal to check or replace if required with preemption + * @param action_server Action server to check for preemptions on + * @return SUCCESS or FAILURE + */ + template + void getPreemptedGoalIfRequested( + typename std::shared_ptr goal, + const typename nav2::SimpleActionServer::SharedPtr & action_server); + + /** + * @brief Checks and logs warning if action canceled + * @param action_server Action server to check for cancellation on + * @param name Name of action to put in warning message + * @return True if action has been cancelled + */ + template + bool checkAndWarnIfCancelled( + typename nav2::SimpleActionServer::SharedPtr & action_server, + const std::string & name); + + /** + * @brief Checks and logs warning if action preempted + * @param action_server Action server to check for preemption on + * @param name Name of action to put in warning message + * @return True if action has been preempted + */ + template + bool checkAndWarnIfPreempted( + typename nav2::SimpleActionServer::SharedPtr & action_server, + const std::string & name); + + /** + * @brief Configure member variables + * @param state Reference to LifeCycle node state + * @return SUCCESS or FAILURE + */ + nav2::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Activate member variables + * @param state Reference to LifeCycle node state + * @return SUCCESS or FAILURE + */ + nav2::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Deactivate member variables + * @param state Reference to LifeCycle node state + * @return SUCCESS or FAILURE + */ + nav2::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Reset member variables + * @param state Reference to LifeCycle node state + * @return SUCCESS or FAILURE + */ + nav2::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Called when in shutdown state + * @param state Reference to LifeCycle node state + * @return SUCCESS or FAILURE + */ + nav2::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Publish zero velocity at terminal condition + */ + void publishZeroVelocity(); + +protected: + /** + * @brief Main action callback method to complete following request + */ + void followObject(); + + /** + * @brief Method to obtain the refined dynamic pose. + * @param pose The initial estimate of the dynamic pose + * which will be updated with the refined pose. + * @return true if successful, false otherwise + */ + virtual bool getRefinedPose(geometry_msgs::msg::PoseStamped & pose); + + /** + * @brief Get the pose of a specific frame in the fixed frame. + * @param pose The output pose. + * @param frame_id The frame to get the pose for. + * @return true if successful, false otherwise + */ + virtual bool getFramePose(geometry_msgs::msg::PoseStamped & pose, const std::string & frame_id); + + /** + * @brief Get the tracking pose based on the current tracking mode. + * @param pose The output pose. + * @param frame_id The frame to get the pose for. + * @return true if successful, false otherwise. + */ + virtual bool getTrackingPose( + geometry_msgs::msg::PoseStamped & pose, const std::string & frame_id); + + /** + * @brief Get the pose at a distance in front of the input pose + * + * @param pose Input pose + * @param distance Distance to move (in meters) + * @return Pose distance meters in front of the input pose + */ + geometry_msgs::msg::PoseStamped getPoseAtDistance( + const geometry_msgs::msg::PoseStamped & pose, double distance); + + /** + * @brief Check if the goal has been reached. + * + * @param goal_pose The goal pose to check + * @return true If the goal has been reached + */ + bool isGoalReached(const geometry_msgs::msg::PoseStamped & goal_pose); + + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); + + // Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + + // Mutex for dynamic parameters + std::mutex dynamic_params_lock_; + + // Frequency to run control loops + double controller_frequency_; + // Timeout to detect the object while rotating to it + double rotate_to_object_timeout_; + // Timeout after which a static object is considered as goal reached + double static_object_timeout_; + // Time when object became static + rclcpp::Time static_object_start_time_; + // Flag to track if we've initialized the static timer + bool static_timer_initialized_; + // Tolerance for transforming coordinates + double transform_tolerance_; + // Tolerances for arriving at the safe_distance pose + double linear_tolerance_, angular_tolerance_; + // Maximum number of times the robot will retry to approach the object + int max_retries_, num_retries_; + // This is the root frame of the robot - typically "base_link" + std::string base_frame_; + // This is our fixed frame for controlling - typically "odom" + std::string fixed_frame_; + // Desired distance to keep from the object + double desired_distance_; + // Skip perception orientation + bool skip_orientation_; + // Should the robot search for the object by rotating or go to last known heading + bool search_by_rotating_; + // Search angle relative to current robot orientation when rotating to find objects + double search_angle_; + + // Timestamp of the last time a iteration was started + rclcpp::Time iteration_start_time_; + + // This is a class member so it can be accessed in publish feedback + rclcpp::Time action_start_time_; + + // Subscribe to the dynamic pose + nav2::Subscription::SharedPtr dynamic_pose_sub_; + + // Publish the filtered dynamic pose + nav2::Publisher::SharedPtr + filtered_dynamic_pose_pub_; + + // Latest message + geometry_msgs::msg::PoseStamped detected_dynamic_pose_; + + // Filtering of detected poses + std::unique_ptr filter_; + double detection_timeout_; + + std::unique_ptr vel_publisher_; + std::unique_ptr odom_sub_; + typename FollowingActionServer::SharedPtr following_action_server_; + + std::unique_ptr controller_; + + std::shared_ptr tf2_buffer_; + std::unique_ptr tf2_listener_; +}; + +} // namespace opennav_following + +#endif // OPENNAV_FOLLOWING__FOLLOWING_SERVER_HPP_ diff --git a/nav2_following/opennav_following/package.xml b/nav2_following/opennav_following/package.xml new file mode 100644 index 00000000000..0cb29905fb6 --- /dev/null +++ b/nav2_following/opennav_following/package.xml @@ -0,0 +1,36 @@ + + + + opennav_following + 1.4.0 + A Task Server for dynamic following object + Alberto Tudela + Apache-2.0 + + ament_cmake + nav2_common + + angles + geometry_msgs + nav2_msgs + nav2_ros_common + nav2_util + opennav_docking + opennav_docking_core + rclcpp + rclcpp_components + rclcpp_lifecycle + tf2 + tf2_geometry_msgs + tf2_ros + + ament_cmake_gtest + ament_cmake_pytest + ament_index_cpp + ament_lint_common + ament_lint_auto + + + ament_cmake + + diff --git a/nav2_following/opennav_following/src/following_server.cpp b/nav2_following/opennav_following/src/following_server.cpp new file mode 100644 index 00000000000..b2d46c7349e --- /dev/null +++ b/nav2_following/opennav_following/src/following_server.cpp @@ -0,0 +1,765 @@ +// Copyright (c) 2024 Open Navigation LLC +// Copyright (c) 2024 Alberto J. Tudela Roldán +// +// 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 "angles/angles.h" +#include "opennav_docking_core/docking_exceptions.hpp" +#include "opennav_following/following_server.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/robot_utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/utils.hpp" + +using namespace std::chrono_literals; +using rcl_interfaces::msg::ParameterType; +using std::placeholders::_1; + +namespace opennav_following +{ + +FollowingServer::FollowingServer(const rclcpp::NodeOptions & options) +: nav2::LifecycleNode("following_server", "", options) +{ + RCLCPP_INFO(get_logger(), "Creating %s", get_name()); + + declare_parameter("controller_frequency", 50.0); + declare_parameter("detection_timeout", 2.0); + declare_parameter("rotate_to_object_timeout", 10.0); + declare_parameter("static_object_timeout", -1.0); + declare_parameter("linear_tolerance", 0.15); + declare_parameter("angular_tolerance", 0.15); + declare_parameter("max_retries", 3); + declare_parameter("base_frame", "base_link"); + declare_parameter("fixed_frame", "odom"); + declare_parameter("filter_coef", 0.1); + declare_parameter("desired_distance", 1.0); + declare_parameter("skip_orientation", true); + declare_parameter("search_by_rotating", false); + declare_parameter("search_angle", M_PI_2); + declare_parameter("odom_topic", "odom"); + declare_parameter("odom_duration", 0.3); + declare_parameter("transform_tolerance", 0.1); +} + +nav2::CallbackReturn +FollowingServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Configuring %s", get_name()); + auto node = shared_from_this(); + + get_parameter("controller_frequency", controller_frequency_); + get_parameter("detection_timeout", detection_timeout_); + get_parameter("rotate_to_object_timeout", rotate_to_object_timeout_); + get_parameter("static_object_timeout", static_object_timeout_); + get_parameter("linear_tolerance", linear_tolerance_); + get_parameter("angular_tolerance", angular_tolerance_); + get_parameter("max_retries", max_retries_); + get_parameter("base_frame", base_frame_); + get_parameter("fixed_frame", fixed_frame_); + get_parameter("desired_distance", desired_distance_); + get_parameter("skip_orientation", skip_orientation_); + get_parameter("search_by_rotating", search_by_rotating_); + get_parameter("search_angle", search_angle_); + get_parameter("transform_tolerance", transform_tolerance_); + RCLCPP_INFO(get_logger(), "Controller frequency set to %.4fHz", controller_frequency_); + + vel_publisher_ = std::make_unique(node, "cmd_vel"); + tf2_buffer_ = std::make_shared(node->get_clock()); + + // Create odom subscriber for backward blind docking + std::string odom_topic; + get_parameter("odom_topic", odom_topic); + double odom_duration; + get_parameter("odom_duration", odom_duration); + odom_sub_ = std::make_unique(node, odom_duration, odom_topic); + + // Create the action server for dynamic following + following_action_server_ = node->create_action_server( + "follow_object", + std::bind(&FollowingServer::followObject, this), + nullptr, std::chrono::milliseconds(500), + true); + + // Create the controller + // Note: Collision detection is not supported in following server so we force it off + // and warn if the user has it enabled (from launch file or parameter file) + declare_parameter("controller.use_collision_detection", false); + controller_ = + std::make_unique(node, tf2_buffer_, fixed_frame_, base_frame_); + + auto get_use_collision_detection = false; + get_parameter("controller.use_collision_detection", get_use_collision_detection); + if (get_use_collision_detection) { + RCLCPP_ERROR(get_logger(), + "Collision detection is not supported in the following server. Please disable " + "the controller.use_collision_detection parameter."); + return nav2::CallbackReturn::FAILURE; + } + + // Setup filter + double filter_coef; + get_parameter("filter_coef", filter_coef); + filter_ = std::make_unique(filter_coef, detection_timeout_); + + // And publish the filtered pose + filtered_dynamic_pose_pub_ = + create_publisher("filtered_dynamic_pose"); + + // Initialize static object detection variables + static_timer_initialized_ = false; + static_object_start_time_ = rclcpp::Time(0); + + return nav2::CallbackReturn::SUCCESS; +} + +nav2::CallbackReturn +FollowingServer::on_activate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Activating %s", get_name()); + + auto node = shared_from_this(); + + tf2_listener_ = std::make_unique(*tf2_buffer_, this, true); + vel_publisher_->on_activate(); + filtered_dynamic_pose_pub_->on_activate(); + following_action_server_->activate(); + + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&FollowingServer::dynamicParametersCallback, this, _1)); + + // Create bond connection + createBond(); + + return nav2::CallbackReturn::SUCCESS; +} + +nav2::CallbackReturn +FollowingServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Deactivating %s", get_name()); + + following_action_server_->deactivate(); + vel_publisher_->on_deactivate(); + filtered_dynamic_pose_pub_->on_deactivate(); + + remove_on_set_parameters_callback(dyn_params_handler_.get()); + dyn_params_handler_.reset(); + tf2_listener_.reset(); + + // Destroy bond connection + destroyBond(); + + return nav2::CallbackReturn::SUCCESS; +} + +nav2::CallbackReturn +FollowingServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Cleaning up %s", get_name()); + tf2_buffer_.reset(); + following_action_server_.reset(); + controller_.reset(); + vel_publisher_.reset(); + filtered_dynamic_pose_pub_.reset(); + odom_sub_.reset(); + return nav2::CallbackReturn::SUCCESS; +} + +nav2::CallbackReturn +FollowingServer::on_shutdown(const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO(get_logger(), "Shutting down %s", get_name()); + return nav2::CallbackReturn::SUCCESS; +} + +template +void FollowingServer::getPreemptedGoalIfRequested( + typename std::shared_ptr goal, + const typename nav2::SimpleActionServer::SharedPtr & action_server) +{ + if (action_server->is_preempt_requested()) { + goal = action_server->accept_pending_goal(); + } +} + +template +bool FollowingServer::checkAndWarnIfCancelled( + typename nav2::SimpleActionServer::SharedPtr & action_server, + const std::string & name) +{ + if (action_server->is_cancel_requested()) { + RCLCPP_WARN(get_logger(), "Goal was cancelled. Cancelling %s action", name.c_str()); + return true; + } + return false; +} + +template +bool FollowingServer::checkAndWarnIfPreempted( + typename nav2::SimpleActionServer::SharedPtr & action_server, + const std::string & name) +{ + if (action_server->is_preempt_requested()) { + RCLCPP_WARN(get_logger(), "Goal was preempted. Cancelling %s action", name.c_str()); + return true; + } + return false; +} + +void FollowingServer::followObject() +{ + std::unique_lock lock(dynamic_params_lock_); + action_start_time_ = this->now(); + rclcpp::Rate loop_rate(controller_frequency_); + + auto goal = following_action_server_->get_current_goal(); + auto result = std::make_shared(); + + if (!following_action_server_ || !following_action_server_->is_server_active()) { + RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping."); + return; + } + + if (checkAndWarnIfCancelled(following_action_server_, "follow_object")) { + following_action_server_->terminate_all(); + return; + } + + getPreemptedGoalIfRequested(goal, following_action_server_); + num_retries_ = 0; + static_timer_initialized_ = false; + + // Reset the last detected dynamic pose timestamp so we start fresh for this action + detected_dynamic_pose_.header.stamp = rclcpp::Time(0); + + try { + auto pose_topic = goal->pose_topic; + auto target_frame = goal->tracked_frame; + if (target_frame.empty()) { + if (pose_topic.empty()) { + RCLCPP_ERROR(get_logger(), + "Both pose topic and target frame are empty. Cannot follow object."); + result->error_code = FollowObject::Result::FAILED_TO_DETECT_OBJECT; + result->error_msg = "No pose topic or target frame provided."; + following_action_server_->terminate_all(result); + return; + } else { + lock.unlock(); + RCLCPP_INFO(get_logger(), "Subscribing to pose topic: %s", pose_topic.c_str()); + dynamic_pose_sub_ = create_subscription( + pose_topic, + [this](const geometry_msgs::msg::PoseStamped::SharedPtr pose) { + detected_dynamic_pose_ = *pose; + }, + nav2::qos::StandardTopicQoS(1)); // Only want the most recent pose + lock.lock(); + } + } else { + RCLCPP_INFO(get_logger(), "Following frame: %s instead of pose", target_frame.c_str()); + } + + // Following control loop: while not timeout, run controller + geometry_msgs::msg::PoseStamped object_pose; + rclcpp::Duration max_duration = goal->max_duration; + while (rclcpp::ok()) { + try { + // Check if we have run out of time + if (this->now() - action_start_time_ > max_duration && max_duration.seconds() > 0.0) { + RCLCPP_INFO(get_logger(), "Exceeded max duration. Stopping."); + result->total_elapsed_time = this->now() - action_start_time_; + result->num_retries = num_retries_; + publishZeroVelocity(); + following_action_server_->succeeded_current(result); + dynamic_pose_sub_.reset(); + return; + } + + // Approach the object using control law + if (approachObject(object_pose, target_frame)) { + // Initialize static timer on first entry + if (!static_timer_initialized_) { + static_object_start_time_ = this->now(); + static_timer_initialized_ = true; + } + + // We have reached the object, maintain position + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 1000, + "Reached object. Stopping until goal is moved again."); + publishFollowingFeedback(FollowObject::Feedback::STOPPING); + publishZeroVelocity(); + + // Stop if the object has been static for some time + if (static_object_timeout_ > 0.0) { + auto static_duration = this->now() - static_object_start_time_; + if (static_duration.seconds() > static_object_timeout_) { + RCLCPP_INFO(get_logger(), + "Object has been static for %.2f seconds (timeout: %.2f), stopping.", + static_duration.seconds(), static_object_timeout_); + result->total_elapsed_time = this->now() - action_start_time_; + result->num_retries = num_retries_; + publishZeroVelocity(); + following_action_server_->succeeded_current(result); + return; + } + } + } else { + // Cancelled, preempted, or shutting down (recoverable errors throw DockingException) + static_timer_initialized_ = false; + result->total_elapsed_time = this->now() - action_start_time_; + publishZeroVelocity(); + following_action_server_->terminate_all(result); + dynamic_pose_sub_.reset(); + return; + } + } catch (opennav_docking_core::DockingException & e) { + if (++num_retries_ > max_retries_) { + RCLCPP_ERROR(get_logger(), "Failed to follow, all retries have been used"); + throw; + } + RCLCPP_WARN(get_logger(), "Following failed, will retry: %s", e.what()); + + // Perform an in-place rotation to find the object again + if (search_by_rotating_) { + RCLCPP_INFO(get_logger(), "Rotating to find object again"); + if (!rotateToObject(object_pose, target_frame)) { + // Cancelled, preempted, or shutting down + publishZeroVelocity(); + following_action_server_->terminate_all(result); + return; + } + } else { + RCLCPP_INFO(get_logger(), "Using last known heading to find object again"); + } + } + loop_rate.sleep(); + } + } catch (const tf2::TransformException & e) { + result->error_msg = std::string("Transform error: ") + e.what(); + RCLCPP_ERROR(get_logger(), result->error_msg.c_str()); + result->error_code = FollowObject::Result::TF_ERROR; + } catch (opennav_docking_core::FailedToDetectDock & e) { + result->error_msg = e.what(); + RCLCPP_ERROR(get_logger(), result->error_msg.c_str()); + result->error_code = FollowObject::Result::FAILED_TO_DETECT_OBJECT; + } catch (opennav_docking_core::FailedToControl & e) { + result->error_msg = e.what(); + RCLCPP_ERROR(get_logger(), result->error_msg.c_str()); + result->error_code = FollowObject::Result::FAILED_TO_CONTROL; + } catch (opennav_docking_core::DockingException & e) { + result->error_msg = e.what(); + RCLCPP_ERROR(get_logger(), result->error_msg.c_str()); + result->error_code = FollowObject::Result::UNKNOWN; + } catch (std::exception & e) { + result->error_msg = e.what(); + RCLCPP_ERROR(get_logger(), result->error_msg.c_str()); + result->error_code = FollowObject::Result::UNKNOWN; + } + + // Stop the robot and report + result->total_elapsed_time = this->now() - action_start_time_; + result->num_retries = num_retries_; + publishZeroVelocity(); + following_action_server_->terminate_current(result); + dynamic_pose_sub_.reset(); +} + +bool FollowingServer::approachObject( + geometry_msgs::msg::PoseStamped & object_pose, const std::string & target_frame) +{ + rclcpp::Rate loop_rate(controller_frequency_); + while (rclcpp::ok()) { + // Update the iteration start time, used for get robot position, transformation and control + iteration_start_time_ = this->now(); + + publishFollowingFeedback(FollowObject::Feedback::CONTROLLING); + + // Stop if cancelled/preempted + if (checkAndWarnIfCancelled(following_action_server_, "follow_object") || + checkAndWarnIfPreempted(following_action_server_, "follow_object")) + { + return false; + } + + // Get the tracking pose from topic or frame + getTrackingPose(object_pose, target_frame); + + // Get the pose at the distance we want to maintain from the object + // Stop and report success if goal is reached + auto target_pose = getPoseAtDistance(object_pose, desired_distance_); + if (isGoalReached(target_pose)) { + return true; + } + + // The control law can get jittery when close to the end when atan2's can explode. + // Thus, we reduce the desired distance by a small amount so that the robot never + // gets to the end of the spiral before its at the desired distance to stop the + // following procedure. + const double backward_projection = 0.25; + const double effective_distance = desired_distance_ - backward_projection; + target_pose = getPoseAtDistance(object_pose, effective_distance); + + // ... and transform the target_pose into base_frame + try { + tf2_buffer_->transform( + target_pose, target_pose, base_frame_, tf2::durationFromSec(transform_tolerance_)); + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN(get_logger(), "Failed to transform target pose: %s", ex.what()); + return false; + } + + // If the object is behind the robot, we reverse the control + geometry_msgs::msg::PoseStamped robot_pose; + if (!nav2_util::getCurrentPose( + robot_pose, *tf2_buffer_, target_pose.header.frame_id, base_frame_, transform_tolerance_, + iteration_start_time_)) + { + RCLCPP_WARN(get_logger(), "Failed to get current robot pose"); + return false; + } + + // Compute and publish controls + auto command = std::make_unique(); + command->header.stamp = now(); + if (!controller_->computeVelocityCommand(target_pose.pose, command->twist, true, false)) { + throw opennav_docking_core::FailedToControl("Failed to get control"); + } + vel_publisher_->publish(std::move(command)); + + loop_rate.sleep(); + } + return false; +} + +bool FollowingServer::rotateToObject( + geometry_msgs::msg::PoseStamped & object_pose, const std::string & target_frame) +{ + const double dt = 1.0 / controller_frequency_; + + // Compute initial robot heading + geometry_msgs::msg::PoseStamped robot_pose; + if (!nav2_util::getCurrentPose( + robot_pose, *tf2_buffer_, object_pose.header.frame_id, base_frame_, transform_tolerance_, + iteration_start_time_)) + { + RCLCPP_WARN(get_logger(), "Failed to get current robot pose"); + return false; + } + double initial_yaw = tf2::getYaw(robot_pose.pose.orientation); + + // Search angles: left offset, then right offset from initial heading + std::vector angles = {initial_yaw + search_angle_, initial_yaw - search_angle_}; + + rclcpp::Rate loop_rate(controller_frequency_); + auto start = this->now(); + auto timeout = rclcpp::Duration::from_seconds(rotate_to_object_timeout_); + + // Iterate over target angles + for (const double & target_angle : angles) { + // Create a target pose oriented at target_angle + auto target_pose = object_pose; + target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(target_angle); + + // Rotate towards target_angle while checking for detection + while (rclcpp::ok()) { + // Update the iteration start time, used for get robot position, transformation and control + iteration_start_time_ = this->now(); + + publishFollowingFeedback(FollowObject::Feedback::RETRY); + + // Stop if cancelled/preempted + if (checkAndWarnIfCancelled(following_action_server_, "follow_object") || + checkAndWarnIfPreempted(following_action_server_, "follow_object")) + { + return false; + } + + // Get current robot pose + if (!nav2_util::getCurrentPose( + robot_pose, *tf2_buffer_, object_pose.header.frame_id, base_frame_, transform_tolerance_, + iteration_start_time_)) + { + RCLCPP_WARN(get_logger(), "Failed to get current robot pose"); + return false; + } + + double angular_distance_to_heading = angles::shortest_angular_distance( + tf2::getYaw(robot_pose.pose.orientation), target_angle); + + // If we are close enough to the target orientation, break and try next angle + if (fabs(angular_distance_to_heading) < angular_tolerance_) { + break; + } + + // While rotating, check if we can get the tracking pose (object detected) + try { + if (getTrackingPose(object_pose, target_frame)) { + return true; + } + } catch (opennav_docking_core::FailedToDetectDock & e) { + // No detection yet, continue rotating + } + + geometry_msgs::msg::Twist current_vel; + current_vel.angular.z = odom_sub_->getRawTwist().angular.z; + + auto command = std::make_unique(); + command->header = robot_pose.header; + command->twist = controller_->computeRotateToHeadingCommand( + angular_distance_to_heading, current_vel, dt); + + vel_publisher_->publish(std::move(command)); + + if (this->now() - start > timeout) { + throw opennav_docking_core::FailedToControl("Timed out rotating to object"); + } + + loop_rate.sleep(); + } + } + + // If we exhausted all search angles and did not detect the object, fail + throw opennav_docking_core::FailedToControl("Failed to rotate to object"); +} + +void FollowingServer::publishZeroVelocity() +{ + auto cmd_vel = std::make_unique(); + cmd_vel->header.frame_id = base_frame_; + cmd_vel->header.stamp = now(); + vel_publisher_->publish(std::move(cmd_vel)); +} + +void FollowingServer::publishFollowingFeedback(uint16_t state) +{ + auto feedback = std::make_shared(); + feedback->state = state; + feedback->following_time = iteration_start_time_ - action_start_time_; + feedback->num_retries = num_retries_; + following_action_server_->publish_feedback(feedback); +} + +bool FollowingServer::getRefinedPose(geometry_msgs::msg::PoseStamped & pose) +{ + // Get current detections and transform to frame + geometry_msgs::msg::PoseStamped detected = detected_dynamic_pose_; + + // If we haven't received any detection yet, wait up to detection_timeout_ for one to arrive. + if (detected.header.stamp == rclcpp::Time(0)) { + auto start = this->now(); + auto timeout = rclcpp::Duration::from_seconds(detection_timeout_); + rclcpp::Rate wait_rate(controller_frequency_); + while (this->now() - start < timeout) { + // Check if a new detection arrived + if (detected_dynamic_pose_.header.stamp != rclcpp::Time(0)) { + detected = detected_dynamic_pose_; + break; + } + wait_rate.sleep(); + } + if (detected.header.stamp == rclcpp::Time(0)) { + RCLCPP_WARN(this->get_logger(), "No detection received within timeout period"); + return false; + } + } + + // Validate that external pose is new enough + auto timeout = rclcpp::Duration::from_seconds(detection_timeout_); + if (this->now() - detected.header.stamp > timeout) { + RCLCPP_WARN(this->get_logger(), "Lost detection or did not detect: timeout exceeded"); + return false; + } + + // Transform detected pose into fixed frame + if (detected.header.frame_id != fixed_frame_) { + try { + tf2_buffer_->transform( + detected, detected, fixed_frame_, tf2::durationFromSec(transform_tolerance_)); + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN(this->get_logger(), "Failed to transform detected object pose"); + return false; + } + } + + // The control law can oscillate if the orientation in the perception + // is not set correctly or has a lot of noise. + // Then, we skip the target orientation by pointing it + // in the same orientation than the vector from the robot to the object. + if (skip_orientation_) { + geometry_msgs::msg::PoseStamped robot_pose; + if (!nav2_util::getCurrentPose( + robot_pose, *tf2_buffer_, detected.header.frame_id, base_frame_, transform_tolerance_, + iteration_start_time_)) + { + RCLCPP_WARN(get_logger(), "Failed to get current robot pose"); + return false; + } + double dx = detected.pose.position.x - robot_pose.pose.position.x; + double dy = detected.pose.position.y - robot_pose.pose.position.y; + double angle_to_target = std::atan2(dy, dx); + detected.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(angle_to_target); + } + + // Filter the detected pose + auto pose_filtered = filter_->update(detected); + filtered_dynamic_pose_pub_->publish(pose_filtered); + + pose = pose_filtered; + return true; +} + +bool FollowingServer::getFramePose( + geometry_msgs::msg::PoseStamped & pose, const std::string & frame_id) +{ + try { + // Get the transform from the target frame to the fixed frame + auto transform = tf2_buffer_->lookupTransform( + fixed_frame_, frame_id, iteration_start_time_, tf2::durationFromSec(transform_tolerance_)); + + // Convert transform to pose + pose.header.frame_id = fixed_frame_; + pose.header.stamp = transform.header.stamp; + pose.pose.position.x = transform.transform.translation.x; + pose.pose.position.y = transform.transform.translation.y; + pose.pose.position.z = transform.transform.translation.z; + pose.pose.orientation = transform.transform.rotation; + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN(get_logger(), + "Failed to get transform for frame %s: %s", frame_id.c_str(), ex.what()); + return false; + } + + // Filter the detected pose + auto filtered_pose = filter_->update(pose); + filtered_dynamic_pose_pub_->publish(filtered_pose); + + pose = filtered_pose; + return true; +} + +bool FollowingServer::getTrackingPose( + geometry_msgs::msg::PoseStamped & pose, const std::string & frame_id) +{ + // Use frame tracking if we have a target frame, otherwise use topic tracking + if (!frame_id.empty()) { + if (!getFramePose(pose, frame_id)) { + throw opennav_docking_core::FailedToDetectDock( + "Failed to get pose in target frame: " + frame_id); + } + } else { + // Use the traditional pose detection from topic + if (!getRefinedPose(pose)) { + throw opennav_docking_core::FailedToDetectDock("Failed object detection"); + } + } + return true; +} + +geometry_msgs::msg::PoseStamped FollowingServer::getPoseAtDistance( + const geometry_msgs::msg::PoseStamped & pose, double distance) +{ + geometry_msgs::msg::PoseStamped robot_pose; + if (!nav2_util::getCurrentPose( + robot_pose, *tf2_buffer_, pose.header.frame_id, base_frame_, transform_tolerance_, + iteration_start_time_)) + { + RCLCPP_WARN(get_logger(), "Failed to get current robot pose"); + // Return original pose as fallback + return pose; + } + double dx = pose.pose.position.x - robot_pose.pose.position.x; + double dy = pose.pose.position.y - robot_pose.pose.position.y; + const double dist = std::hypot(dx, dy); + geometry_msgs::msg::PoseStamped forward_pose = pose; + forward_pose.pose.position.x -= distance * (dx / dist); + forward_pose.pose.position.y -= distance * (dy / dist); + return forward_pose; +} + +bool FollowingServer::isGoalReached(const geometry_msgs::msg::PoseStamped & goal_pose) +{ + geometry_msgs::msg::PoseStamped robot_pose; + if (!nav2_util::getCurrentPose( + robot_pose, *tf2_buffer_, goal_pose.header.frame_id, base_frame_, transform_tolerance_, + iteration_start_time_)) + { + RCLCPP_WARN(get_logger(), "Failed to get current robot pose"); + return false; + } + const double dist = std::hypot( + robot_pose.pose.position.x - goal_pose.pose.position.x, + robot_pose.pose.position.y - goal_pose.pose.position.y); + const double yaw = angles::shortest_angular_distance( + tf2::getYaw(robot_pose.pose.orientation), tf2::getYaw(goal_pose.pose.orientation)); + return dist < linear_tolerance_ && abs(yaw) < angular_tolerance_; +} + +rcl_interfaces::msg::SetParametersResult +FollowingServer::dynamicParametersCallback(std::vector parameters) +{ + std::lock_guard lock(dynamic_params_lock_); + + rcl_interfaces::msg::SetParametersResult result; + for (auto parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == "controller_frequency") { + controller_frequency_ = parameter.as_double(); + } else if (name == "detection_timeout") { + detection_timeout_ = parameter.as_double(); + } else if (name == "rotate_to_object_timeout") { + rotate_to_object_timeout_ = parameter.as_double(); + } else if (name == "static_object_timeout") { + static_object_timeout_ = parameter.as_double(); + } else if (name == "linear_tolerance") { + linear_tolerance_ = parameter.as_double(); + } else if (name == "angular_tolerance") { + angular_tolerance_ = parameter.as_double(); + } else if (name == "desired_distance") { + desired_distance_ = parameter.as_double(); + } else if (name == "transform_tolerance") { + transform_tolerance_ = parameter.as_double(); + } else if (name == "search_angle") { + search_angle_ = parameter.as_double(); + } + } else if (type == ParameterType::PARAMETER_STRING) { + if (name == "base_frame") { + base_frame_ = parameter.as_string(); + } else if (name == "fixed_frame") { + fixed_frame_ = parameter.as_string(); + } + } else if (type == ParameterType::PARAMETER_BOOL) { + if (name == "skip_orientation") { + skip_orientation_ = parameter.as_bool(); + } else if (name == "search_by_rotating") { + search_by_rotating_ = parameter.as_bool(); + } + } + } + + result.successful = true; + return result; +} + +} // namespace opennav_following + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(opennav_following::FollowingServer) diff --git a/nav2_following/opennav_following/src/main.cpp b/nav2_following/opennav_following/src/main.cpp new file mode 100644 index 00000000000..0986268065d --- /dev/null +++ b/nav2_following/opennav_following/src/main.cpp @@ -0,0 +1,28 @@ +// Copyright (c) 2024 Alberto J. Tudela Roldán +// +// 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 "opennav_following/following_server.hpp" +#include "rclcpp/rclcpp.hpp" + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node->get_node_base_interface()); + rclcpp::shutdown(); + + return 0; +} diff --git a/nav2_following/opennav_following/test/CMakeLists.txt b/nav2_following/opennav_following/test/CMakeLists.txt new file mode 100644 index 00000000000..8d2f34023b1 --- /dev/null +++ b/nav2_following/opennav_following/test/CMakeLists.txt @@ -0,0 +1,33 @@ +# Test following server (unit) +ament_add_gtest(test_following_server_unit + test_following_server_unit.cpp +) +target_link_libraries(test_following_server_unit + ${geometry_msgs_TARGETS} + ${library_name} + nav2_util::nav2_util_core + opennav_docking_core::opennav_docking_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle +) + +# Test following server (smoke) +ament_add_pytest_test(test_following_server_topic test_following_server.py + ENV + FOLLOWING_MODE=topic +) + +ament_add_pytest_test(test_following_server_frame test_following_server.py + ENV + FOLLOWING_MODE=frame +) + +ament_add_pytest_test(test_following_server_skip_pose test_following_server.py + ENV + FOLLOWING_MODE=skip_pose +) + +ament_add_pytest_test(test_following_server_search test_following_server.py + ENV + FOLLOWING_MODE=search +) diff --git a/nav2_following/opennav_following/test/test_following_server.py b/nav2_following/opennav_following/test/test_following_server.py new file mode 100644 index 00000000000..421582d017f --- /dev/null +++ b/nav2_following/opennav_following/test/test_following_server.py @@ -0,0 +1,423 @@ +# Copyright (c) 2024 Open Navigation LLC +# Copyright (c) 2024 Alberto J. Tudela Roldán +# +# 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. + +from math import cos, sin +import os +import threading +import time +from typing import Callable +import unittest + +from action_msgs.msg import GoalStatus +from geometry_msgs.msg import PoseStamped, TransformStamped, Twist, TwistStamped +from launch import LaunchDescription +from launch_ros.actions import Node +import launch_testing +import launch_testing.actions +from lifecycle_msgs.srv import GetState +from nav2_msgs.action import FollowObject +import pytest +import rclpy +from rclpy.action import ActionClient +from rclpy.client import Client +from rclpy.duration import Duration +from rclpy.executors import SingleThreadedExecutor +import tf2_ros +from tf2_ros import TransformBroadcaster + +# This test can be run standalone with: +# python3 -u -m pytest test_following_server.py -s + +# If python3-flaky is installed, you can run the test multiple times to +# try to identify flaky ness. +# python3 -u -m pytest --force-flaky --min-passes 3 --max-runs 5 -s -v test_following_server.py + + +@pytest.mark.rostest +# @pytest.mark.flaky +# @pytest.mark.flaky(max_runs=5, min_passes=3) +def generate_test_description() -> LaunchDescription: + + return LaunchDescription([ + # SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + # SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + Node( + package='opennav_following', + executable='opennav_following', + name='following_server', + parameters=[{'desired_distance': 0.5, + 'detection_timeout': 0.75, + 'linear_tolerance': 0.05, + 'angular_tolerance': 0.05, + 'transform_tolerance': 0.5, + 'static_object_timeout': 0.5, + 'search_by_rotating': True, + 'controller': { + 'use_collision_detection': False, + 'rotate_to_heading_max_angular_accel': 25.0, + 'transform_tolerance': 0.5, + }}], + output='screen', + ), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_navigation', + output='screen', + parameters=[{'autostart': True}, + {'node_names': ['following_server']}] + ), + launch_testing.actions.ReadyToTest(), # type: ignore[no-untyped-call] + ]) + + +class ObjectPublisher: + """ + A class that continuously publishes the tested object pose or frame at a fixed rate. + + The publisher runs in a separate thread and stops publishing when the provided + at_distance_getter callable returns True. + """ + + def __init__( + self, + topic_name: str, + frame_name: str, + rate_hz: float, + at_distance_getter: Callable[[], bool], + mode: str = 'topic', + ): + # Create a dedicated node and executor so timers and clocks behave correctly + self._node = rclpy.create_node('test_object_pose_publisher') + self._pub = self._node.create_publisher(PoseStamped, topic_name, 10) + self._at_distance_getter = at_distance_getter + self._mode = mode + self._frame_name = frame_name + + # Timer will drive publishing for both modes + self._timer = self._node.create_timer(1.0 / rate_hz, self._timer_cb) + + # Use a single-threaded executor to spin this node in the background + self._executor = SingleThreadedExecutor() + self._executor.add_node(self._node) + + # If in frame mode, create a TransformBroadcaster for publishing TF + self._tf_broadcaster: TransformBroadcaster | None = None + if self._mode == 'frame': + self._tf_broadcaster = TransformBroadcaster(self._node) + + self._stop_event = threading.Event() + self._thread = threading.Thread(target=self._spin, daemon=True) + self._thread.start() + + def _timer_cb(self) -> None: + # Called in executor context at the configured rate + if self._at_distance_getter(): + return + if self._mode == 'topic': + p = PoseStamped() + p.header.stamp = self._node.get_clock().now().to_msg() + p.header.frame_id = 'map' + p.pose.position.x = 1.75 + p.pose.position.y = 0.0 + self._pub.publish(p) + self._node.get_logger().debug('Publishing pose') + elif self._mode == 'frame': + # Publish a TF map -> object_frame + t = TransformStamped() + t.header.stamp = self._node.get_clock().now().to_msg() + t.header.frame_id = 'map' + t.child_frame_id = self._frame_name + t.transform.translation.x = 1.75 + t.transform.translation.y = 0.0 + t.transform.rotation.x = 0.0 + t.transform.rotation.y = 0.0 + t.transform.rotation.z = 0.0 + t.transform.rotation.w = 1.0 + if self._tf_broadcaster is not None: + self._tf_broadcaster.sendTransform(t) + + def _spin(self) -> None: + # Spin until stop event is set + try: + while not self._stop_event.is_set(): + self._executor.spin_once(timeout_sec=0.1) + except Exception: + # Ensure we don't crash the main test thread + pass + + def shutdown(self) -> None: + self._stop_event.set() + # Allow the executor loop to finish + self._thread.join(timeout=1.0) + try: + self._executor.remove_node(self._node) + except Exception: + pass + try: + self._node.destroy_node() + except Exception: + pass + + +class TestFollowingServer(unittest.TestCase): + + @classmethod + def setUpClass(cls) -> None: + rclpy.init() + + @classmethod + def tearDownClass(cls) -> None: + rclpy.shutdown() + + def setUp(self) -> None: + # Create a ROS node for tests + # Latest odom -> base_link + self.x: float = 0.0 + self.y: float = 0.0 + self.theta: float = 0.0 + # Track states + self.at_distance: bool = False + self.retry_state: bool = False + # Latest command velocity + self.command: Twist = Twist() + self.node = rclpy.create_node('test_following_server') + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self.node) + # Determine test mode from environment: 'topic, 'frame' or 'search' + mode_env = os.getenv('FOLLOWING_MODE', 'topic') + # In 'search' mode stop publishing once the robot reaches 0.75m so the server must + # perform its recovery behavior (object lost) + if mode_env == 'search': + def at_distance_getter() -> bool: + return bool(((self.x ** 2 + self.y ** 2) ** 0.5) >= 0.75) + pub_mode = 'topic' + else: + def at_distance_getter() -> bool: + return self.at_distance + pub_mode = mode_env + + self.object_publisher = ObjectPublisher( + 'tested_pose', + 'object_frame', + 20.0, + at_distance_getter, + pub_mode, + ) + + def wait_for_node_to_be_active(self, node_name: str, timeout_sec: float = 10.0) -> None: + """Wait for a managed node to become active.""" + client: Client[GetState.Request, GetState.Response] = ( # type: ignore[name-defined] + self.node.create_client(GetState, f'{node_name}/get_state') # type: ignore[arg-type] + ) + if not client.wait_for_service(timeout_sec=2.0): + self.fail(f'Service get_state for {node_name} not available.') + + start_time = time.time() + while time.time() - start_time < timeout_sec: + req = GetState.Request() # empty request + future = client.call_async(req) + rclpy.spin_until_future_complete(self.node, future, timeout_sec=1.0) + result = future.result() + if result is not None and result.current_state.id == 3: # 3 = ACTIVE + self.node.get_logger().info(f'Node {node_name} is active.') + return + time.sleep(0.5) + # raises AssertionError + self.fail(f'Node {node_name} did not become active within {timeout_sec} seconds.') + + def tearDown(self) -> None: + self.object_publisher.shutdown() + self.node.destroy_node() + + def command_velocity_callback(self, msg: TwistStamped) -> None: + self.node.get_logger().info(f'Command: {msg.twist.linear.x:f} {msg.twist.angular.z:f}') + self.command = msg.twist + + def timer_callback(self) -> None: + # Propagate command + period = 0.05 + self.x += cos(self.theta) * self.command.linear.x * period + self.y += sin(self.theta) * self.command.linear.x * period + self.theta += self.command.angular.z * period + # Need to publish updated TF + self.publish() + + def publish(self) -> None: + # Publish base->odom transform + t = TransformStamped() + t.header.stamp = self.node.get_clock().now().to_msg() + t.header.frame_id = 'odom' + t.child_frame_id = 'base_link' + t.transform.translation.x = self.x + t.transform.translation.y = self.y + t.transform.rotation.z = sin(self.theta / 2.0) + t.transform.rotation.w = cos(self.theta / 2.0) + self.tf_broadcaster.sendTransform(t) + # Also publish map->odom transform so object pose in 'map' can be transformed + m = TransformStamped() + m.header.stamp = self.node.get_clock().now().to_msg() + m.header.frame_id = 'map' + m.child_frame_id = 'odom' + m.transform.translation.x = 0.0 + m.transform.translation.y = 0.0 + m.transform.rotation.x = 0.0 + m.transform.rotation.y = 0.0 + m.transform.rotation.z = 0.0 + m.transform.rotation.w = 1.0 + self.tf_broadcaster.sendTransform(m) + + def action_feedback_callback( + self, + msg: FollowObject.Feedback # type: ignore[name-defined] + ) -> None: + # Force the following action to run a full recovery loop when + # the robot is at distance + if msg.feedback.state == msg.feedback.STOPPING: + self.at_distance = True + elif msg.feedback.state == msg.feedback.RETRY: + self.at_distance = False + self.retry_state = True + + def test_following_server(self) -> None: + # Publish TF for odometry + self.tf_broadcaster = TransformBroadcaster(self.node) + time.sleep(0.5) + + # Create a timer to run "control loop" at 20hz + self.timer = self.node.create_timer(0.05, self.timer_callback) + + # Create action client + self.follow_action_client: ActionClient[ # type: ignore[name-defined] + FollowObject.Goal, FollowObject.Result, FollowObject.Feedback + ] = ActionClient(self.node, FollowObject, 'follow_object') # type: ignore[arg-type] + + # Subscribe to command velocity + self.node.create_subscription( + TwistStamped, + 'cmd_vel', + self.command_velocity_callback, + 10 + ) + + # Publish transform + self.publish() + + # Wait until the transform is available. + self.node.get_logger().info('Waiting for TF odom->base_link to be available...') + start_time = time.time() + timeout = 10.0 + while not self.tf_buffer.can_transform('odom', 'base_link', rclpy.time.Time()): + if time.time() - start_time > timeout: + self.fail('TF transform odom->base_link not available after 10s') + rclpy.spin_once(self.node, timeout_sec=0.1) + time.sleep(0.1) + self.node.get_logger().info('TF is ready, proceeding with test.') + + # Wait until the following server is active. + self.wait_for_node_to_be_active('following_server') + + # Test follow action with an object at 1.75m in front of the robot + self.action_result = [] + assert self.follow_action_client.wait_for_server(timeout_sec=5.0), \ + 'follow_object service not available' + + # Create the goal + goal = FollowObject.Goal() + if os.getenv('FOLLOWING_MODE') == 'topic' or os.getenv('FOLLOWING_MODE') == 'search': + goal.pose_topic = 'tested_pose' + goal.max_duration = Duration(seconds=10.0).to_msg() + elif os.getenv('FOLLOWING_MODE') == 'frame': + goal.tracked_frame = 'object_frame' + goal.max_duration = Duration(seconds=4.0).to_msg() + + # Send a goal + self.node.get_logger().info('Sending first goal') + future = self.follow_action_client.send_goal_async( + goal, feedback_callback=self.action_feedback_callback) + rclpy.spin_until_future_complete(self.node, future) + self.goal_handle = future.result() + assert self.goal_handle is not None, 'goal_handle should not be None' + assert self.goal_handle.accepted, 'goal_handle not accepted' + result_future_original = self.goal_handle.get_result_async() + + # Run for 2 seconds + for _ in range(20): + rclpy.spin_once(self.node, timeout_sec=0.1) + time.sleep(0.1) + + # Send another goal to preempt the first + self.node.get_logger().info('Preempting with a new goal') + future = self.follow_action_client.send_goal_async( + goal, feedback_callback=self.action_feedback_callback) + rclpy.spin_until_future_complete(self.node, future) + self.goal_handle = future.result() + assert self.goal_handle is not None, 'goal_handle should not be None' + assert self.goal_handle.accepted, 'goal_handle not accepted' + result_future = self.goal_handle.get_result_async() + rclpy.spin_until_future_complete(self.node, result_future) + self.action_result.append(result_future.result()) + + rclpy.spin_until_future_complete(self.node, result_future_original) + self.action_result.append(result_future_original.result()) + + # First is aborted due to preemption + self.assertIsNotNone(self.action_result[0]) + if self.action_result[0] is not None: + self.assertEqual(self.action_result[0].status, GoalStatus.STATUS_ABORTED) + self.assertTrue(self.action_result[0].result, FollowObject.Result.NONE) + self.assertFalse(self.at_distance) + + self.node.get_logger().info('Goal preempted') + + # Run for 0.5 seconds + for _ in range(5): + rclpy.spin_once(self.node, timeout_sec=0.1) + time.sleep(0.1) + + # Second is aborted due to preemption during main loop (takes down all actions) + self.assertIsNotNone(self.action_result[1]) + if self.action_result[1] is not None: + self.assertEqual(self.action_result[1].status, GoalStatus.STATUS_ABORTED) + self.assertTrue(self.action_result[1].result, FollowObject.Result.NONE) + self.assertFalse(self.at_distance) + + # Resend the goal + self.node.get_logger().info('Sending goal again') + future = self.follow_action_client.send_goal_async( + goal, feedback_callback=self.action_feedback_callback) + rclpy.spin_until_future_complete(self.node, future) + self.goal_handle = future.result() + assert self.goal_handle is not None, 'goal_handle should not be None' + assert self.goal_handle.accepted, 'goal_handle not accepted' + result_future = self.goal_handle.get_result_async() + rclpy.spin_until_future_complete(self.node, result_future) + self.action_result.append(result_future.result()) + + self.assertIsNotNone(self.action_result[2]) + if self.action_result[2] is not None: + mode = os.getenv('FOLLOWING_MODE') + if mode != 'skip_pose' and mode != 'search': + self.assertEqual(self.action_result[2].status, GoalStatus.STATUS_SUCCEEDED) + self.assertEqual(self.action_result[2].result.num_retries, 0) + self.assertTrue(self.at_distance) + + +@launch_testing.post_shutdown_test() # type: ignore[no-untyped-call] +class TestProcessOutput(unittest.TestCase): + + def test_exit_code(self, proc_info: launch_testing.ProcInfoHandler) -> None: + # Check that all processes in the launch exit with code 0 + launch_testing.asserts.assertExitCodes(proc_info) # type: ignore[no-untyped-call] diff --git a/nav2_following/opennav_following/test/test_following_server_unit.cpp b/nav2_following/opennav_following/test/test_following_server_unit.cpp new file mode 100644 index 00000000000..ceb6a888f9e --- /dev/null +++ b/nav2_following/opennav_following/test/test_following_server_unit.cpp @@ -0,0 +1,375 @@ +// Copyright (c) 2024 Open Navigation LLC +// Copyright (c) 2024 Alberto J. Tudela Roldán +// +// 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 "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "opennav_docking_core/docking_exceptions.hpp" +#include "opennav_following/following_server.hpp" +#include "nav2_ros_common/node_thread.hpp" +#include "tf2/utils.hpp" +#include "tf2_ros/transform_broadcaster.hpp" + +// Testing unit functions in following server, smoke/system tests in python file + +using namespace std::chrono_literals; // NOLINT + +class RosLockGuard +{ +public: + RosLockGuard() {rclcpp::init(0, nullptr);} + ~RosLockGuard() {rclcpp::shutdown();} +}; +RosLockGuard g_rclcpp; + +namespace opennav_following +{ + +using FollowObject = nav2_msgs::action::FollowObject; + +class FollowingServerShim : public FollowingServer +{ +public: + FollowingServerShim() + : FollowingServer() {} + + void setUsingDedicatedThread() + { + tf2_buffer_->setUsingDedicatedThread(true); // One-thread broadcasting-listening model + } + + virtual bool approachObject(geometry_msgs::msg::PoseStamped &, const std::string &) + { + std::string exception; + this->get_parameter("exception_to_throw", exception); + if (exception == "TransformException") { + throw tf2::TransformException("TransformException"); + } else if (exception == "FailedToDetectObject") { + throw opennav_docking_core::FailedToDetectDock("FailedToDetectObject"); + } else if (exception == "FailedToControl") { + throw opennav_docking_core::FailedToControl("FailedToControl"); + } else if (exception == "DockingException") { + throw opennav_docking_core::DockingException("DockingException"); + } else if (exception == "exception") { + throw std::exception(); + } + return true; + } + + virtual bool getRefinedPose(geometry_msgs::msg::PoseStamped & pose) + { + return FollowingServer::getRefinedPose(pose); + } + + virtual bool getFramePose(geometry_msgs::msg::PoseStamped & pose, const std::string & frame_id) + { + return FollowingServer::getFramePose(pose, frame_id); + } + + virtual bool getTrackingPose(geometry_msgs::msg::PoseStamped & pose, const std::string & frame_id) + { + return FollowingServer::getTrackingPose(pose, frame_id); + } + + virtual bool rotateToObject(geometry_msgs::msg::PoseStamped &) + { + return true; + } + + geometry_msgs::msg::PoseStamped getPoseAtDistance( + const geometry_msgs::msg::PoseStamped & pose, double distance) + { + return FollowingServer::getPoseAtDistance(pose, distance); + } + + bool isGoalReached(const geometry_msgs::msg::PoseStamped & goal_pose) + { + return FollowingServer::isGoalReached(goal_pose); + } + + void setDynamicPose(const geometry_msgs::msg::PoseStamped & pose) + { + detected_dynamic_pose_ = pose; + } + + void setSkipOrientation(bool skip_orientation) + { + skip_orientation_ = skip_orientation; + } + + void setFixedFrame(const std::string & fixed_frame) + { + fixed_frame_ = fixed_frame; + } +}; + +TEST(FollowingServerTests, ObjectLifecycle) +{ + auto node = std::make_shared(); + node->configure(); + node->activate(); + node->deactivate(); + node->cleanup(); + node->shutdown(); + node.reset(); +} + +TEST(FollowingServerTests, ErrorExceptions) +{ + auto node = std::make_shared(); + auto node_thread = nav2::NodeThread(node); + auto node2 = std::make_shared("client_node"); + + auto pub = node2->create_publisher( + "dynamic_pose", rclcpp::QoS(1)); + + geometry_msgs::msg::PoseStamped detected_pose; + + node->on_configure(rclcpp_lifecycle::State()); + node->on_activate(rclcpp_lifecycle::State()); + + node->declare_parameter("exception_to_throw", rclcpp::ParameterValue("")); + node->declare_parameter("follow_action_called", rclcpp::ParameterValue(false)); + node->set_parameter(rclcpp::Parameter("fixed_frame", rclcpp::ParameterValue("test_frame"))); + + // Error codes following + std::vector error_ids{ + "TransformException", "FailedToDetectObject", "FailedToControl", + "DockingException", "exception"}; + std::vector error_codes{901, 902, 903, 999, 999}; + + // Call action, check error code + for (unsigned int i = 0; i != error_ids.size(); i++) { + node->set_parameter( + rclcpp::Parameter("exception_to_throw", rclcpp::ParameterValue(error_ids[i]))); + + auto client = rclcpp_action::create_client(node2, "follow_object"); + if (!client->wait_for_action_server(1s)) { + RCLCPP_ERROR(node2->get_logger(), "Action server not available after waiting"); + } + auto goal_msg = FollowObject::Goal(); + goal_msg.pose_topic = "dynamic_pose"; + auto future_goal_handle = client->async_send_goal(goal_msg); + pub->publish(detected_pose); + + if (rclcpp::spin_until_future_complete( + node2, future_goal_handle, 2s) == rclcpp::FutureReturnCode::SUCCESS) + { + auto future_result = client->async_get_result(future_goal_handle.get()); + if (rclcpp::spin_until_future_complete( + node2, future_result, 5s) == rclcpp::FutureReturnCode::SUCCESS) + { + auto result = future_result.get(); + EXPECT_EQ(result.result->error_code, error_codes[i]); + } else { + EXPECT_TRUE(false); + } + } else { + EXPECT_TRUE(false); + } + } + + // Set follow_action_called to true to simulate robot following object + node->set_parameter(rclcpp::Parameter("follow_action_called", true)); + + node->on_deactivate(rclcpp_lifecycle::State()); + node->on_cleanup(rclcpp_lifecycle::State()); + node->on_shutdown(rclcpp_lifecycle::State()); + node.reset(); +} + +TEST(FollowingServerTests, GetPoseAtDistance) +{ + auto node = std::make_shared(); + node->set_parameter(rclcpp::Parameter("base_frame", rclcpp::ParameterValue("my_frame"))); + node->on_configure(rclcpp_lifecycle::State()); + node->setUsingDedicatedThread(); + + geometry_msgs::msg::PoseStamped pose; + pose.header.stamp = node->now(); + pose.header.frame_id = "my_frame"; + pose.pose.position.x = 1.0; + pose.pose.position.y = -1.0; + + auto new_pose = node->getPoseAtDistance(pose, 0.2); + EXPECT_NEAR(new_pose.pose.position.x, 0.8585, 0.01); + EXPECT_NEAR(new_pose.pose.position.y, -0.8585, 0.01); + + node->on_cleanup(rclcpp_lifecycle::State()); + node->on_shutdown(rclcpp_lifecycle::State()); + node.reset(); +} + +TEST(FollowingServerTests, IsGoalReached) +{ + auto node = std::make_shared(); + node->set_parameter(rclcpp::Parameter("base_frame", rclcpp::ParameterValue("my_frame"))); + node->on_configure(rclcpp_lifecycle::State()); + node->setUsingDedicatedThread(); + + geometry_msgs::msg::PoseStamped pose; + pose.header.stamp = node->now(); + pose.header.frame_id = "my_frame"; + pose.pose.position.x = 1.0; + pose.pose.position.y = -1.0; + + EXPECT_FALSE(node->isGoalReached(pose)); + + // Set the pose below the tolerance + pose.pose.position.x = 0.1; + pose.pose.position.y = 0.1; + EXPECT_TRUE(node->isGoalReached(pose)); + + node->on_cleanup(rclcpp_lifecycle::State()); + node->on_shutdown(rclcpp_lifecycle::State()); + node.reset(); +} + +TEST(FollowingServerTests, RefinedPose) +{ + auto node = std::make_shared(); + + // Set filter coefficient to 0, so no filtering is done + node->set_parameter(rclcpp::Parameter("filter_coef", rclcpp::ParameterValue(0.0))); + node->set_parameter(rclcpp::Parameter("base_frame", rclcpp::ParameterValue("my_frame"))); + node->on_configure(rclcpp_lifecycle::State()); + node->on_activate(rclcpp_lifecycle::State()); + + // Timestamps are outdated; this is after timeout + geometry_msgs::msg::PoseStamped pose; + EXPECT_FALSE(node->getRefinedPose(pose)); + + // Set skip orientation to false + node->setSkipOrientation(false); + + // Set the detected pose + geometry_msgs::msg::PoseStamped detected_pose; + detected_pose.header.stamp = node->now(); + detected_pose.header.frame_id = "my_frame"; + detected_pose.pose.position.x = 0.1; + detected_pose.pose.position.y = -0.1; + node->setDynamicPose(detected_pose); + + node->setFixedFrame("my_frame"); + EXPECT_TRUE(node->getRefinedPose(pose)); + EXPECT_NEAR(pose.pose.position.x, 0.1, 0.01); + EXPECT_NEAR(pose.pose.position.y, -0.1, 0.01); + + // Now, set skip orientation to true + node->setSkipOrientation(true); + + detected_pose.header.stamp = node->now(); + node->setDynamicPose(detected_pose); + + EXPECT_TRUE(node->getRefinedPose(pose)); + EXPECT_NEAR(pose.pose.position.x, 0.1, 0.01); + EXPECT_NEAR(pose.pose.position.y, -0.1, 0.01); + + node->on_deactivate(rclcpp_lifecycle::State()); + node->on_cleanup(rclcpp_lifecycle::State()); + node->on_shutdown(rclcpp_lifecycle::State()); + node.reset(); +} + +TEST(FollowingServerTests, GetFramePose) +{ + auto node = std::make_shared(); + + // Set filter coefficient to 0, so no filtering is done + node->set_parameter(rclcpp::Parameter("filter_coef", rclcpp::ParameterValue(0.0))); + + node->on_configure(rclcpp_lifecycle::State()); + node->on_activate(rclcpp_lifecycle::State()); + + geometry_msgs::msg::PoseStamped pose; + + // Not frame set, should return false + auto frame_test = std::string("my_frame"); + node->setFixedFrame("fixed_frame_test"); + EXPECT_FALSE(node->getFramePose(pose, frame_test)); + + // Set transform between my_frame and fixed_frame_test + auto tf_broadcaster = std::make_unique(node); + geometry_msgs::msg::TransformStamped frame_to_fixed; + frame_to_fixed.header.frame_id = "fixed_frame_test"; + frame_to_fixed.header.stamp = node->get_clock()->now(); + frame_to_fixed.child_frame_id = "my_frame"; + frame_to_fixed.transform.translation.x = 1.0; + frame_to_fixed.transform.translation.y = 2.0; + frame_to_fixed.transform.translation.z = 3.0; + tf_broadcaster->sendTransform(frame_to_fixed); + + // Now, we should be able to get the pose in my_frame + EXPECT_TRUE(node->getFramePose(pose, frame_test)); + EXPECT_EQ(pose.pose.position.x, 1.0); + EXPECT_EQ(pose.pose.position.y, 2.0); + EXPECT_EQ(pose.pose.position.z, 3.0); + + node->on_deactivate(rclcpp_lifecycle::State()); + node->on_cleanup(rclcpp_lifecycle::State()); + node->on_shutdown(rclcpp_lifecycle::State()); + node.reset(); +} + +TEST(FollowingServerTests, DynamicParams) +{ + auto node = std::make_shared(); + node->on_configure(rclcpp_lifecycle::State()); + node->on_activate(rclcpp_lifecycle::State()); + + auto params = std::make_shared( + node->get_node_base_interface(), node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); + + // Set parameters + auto results = params->set_parameters_atomically( + {rclcpp::Parameter("controller_frequency", 1.0), + rclcpp::Parameter("detection_timeout", 2.0), + rclcpp::Parameter("rotate_to_object_timeout", 3.0), + rclcpp::Parameter("static_object_timeout", 4.0), + rclcpp::Parameter("desired_distance", 5.0), + rclcpp::Parameter("linear_tolerance", 6.0), + rclcpp::Parameter("angular_tolerance", 7.0), + rclcpp::Parameter("base_frame", std::string("test_base_frame")), + rclcpp::Parameter("fixed_frame", std::string("test_fixed_frame")), + rclcpp::Parameter("skip_orientation", false), + rclcpp::Parameter("search_by_rotating", true), + rclcpp::Parameter("search_angle", 8.0), + rclcpp::Parameter("transform_tolerance", 9.0) + }); + + // Spin + rclcpp::spin_until_future_complete(node->get_node_base_interface(), results); + + // Check parameters + EXPECT_EQ(node->get_parameter("controller_frequency").as_double(), 1.0); + EXPECT_EQ(node->get_parameter("detection_timeout").as_double(), 2.0); + EXPECT_EQ(node->get_parameter("rotate_to_object_timeout").as_double(), 3.0); + EXPECT_EQ(node->get_parameter("static_object_timeout").as_double(), 4.0); + EXPECT_EQ(node->get_parameter("desired_distance").as_double(), 5.0); + EXPECT_EQ(node->get_parameter("linear_tolerance").as_double(), 6.0); + EXPECT_EQ(node->get_parameter("angular_tolerance").as_double(), 7.0); + EXPECT_EQ(node->get_parameter("base_frame").as_string(), "test_base_frame"); + EXPECT_EQ(node->get_parameter("fixed_frame").as_string(), "test_fixed_frame"); + EXPECT_EQ(node->get_parameter("skip_orientation").as_bool(), false); + EXPECT_EQ(node->get_parameter("search_by_rotating").as_bool(), true); + EXPECT_EQ(node->get_parameter("search_angle").as_double(), 8.0); + EXPECT_EQ(node->get_parameter("transform_tolerance").as_double(), 9.0); +} + +} // namespace opennav_following diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index b44e08ccb3a..7a21503c948 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -79,6 +79,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "action/UndockRobot.action" "action/ComputeRoute.action" "action/ComputeAndTrackRoute.action" + "action/FollowObject.action" DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs geographic_msgs unique_identifier_msgs ) diff --git a/nav2_msgs/action/FollowObject.action b/nav2_msgs/action/FollowObject.action new file mode 100644 index 00000000000..c1b204e5c97 --- /dev/null +++ b/nav2_msgs/action/FollowObject.action @@ -0,0 +1,33 @@ +#goal definition + +string pose_topic # Topic to publish the pose of the object to follow +string tracked_frame # Target frame to follow (Optional, used if pose_topic is not set) +builtin_interfaces/Duration max_duration + +--- +#result definition + +# Error codes +# Note: The expected priority order of the errors should match the message order +uint16 NONE=0 +uint16 TF_ERROR=901 +uint16 FAILED_TO_DETECT_OBJECT=902 +uint16 FAILED_TO_CONTROL=903 +uint16 UNKNOWN=999 + +builtin_interfaces/Duration total_elapsed_time +uint16 error_code 0 # Contextual error code, if any +uint16 num_retries 0 # Number of retries attempted +string error_msg # Error message, if any +--- +#feedback definition + +uint16 NONE=0 +uint16 INITIAL_PERCEPTION=1 +uint16 CONTROLLING=2 +uint16 STOPPING=3 +uint16 RETRY=4 + +uint16 state # Current following state +builtin_interfaces/Duration following_time +uint16 num_retries 0 # Number of retries attempted diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 5b30fd6d7a8..e907ceb3305 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -24,10 +24,11 @@ from geographic_msgs.msg import GeoPose from geometry_msgs.msg import Point, PoseStamped, PoseWithCovarianceStamped from lifecycle_msgs.srv import GetState -from nav2_msgs.action import (AssistedTeleop, BackUp, ComputeAndTrackRoute, - ComputePathThroughPoses, ComputePathToPose, ComputeRoute, DockRobot, - DriveOnHeading, FollowGPSWaypoints, FollowPath, FollowWaypoints, - NavigateThroughPoses, NavigateToPose, SmoothPath, Spin, UndockRobot) +from nav2_msgs.action import (AssistedTeleop, BackUp, # type: ignore[attr-defined] + ComputeAndTrackRoute, ComputePathThroughPoses, ComputePathToPose, + ComputeRoute, DockRobot, DriveOnHeading, FollowGPSWaypoints, + FollowObject, FollowPath, FollowWaypoints, NavigateThroughPoses, + NavigateToPose, SmoothPath, Spin, UndockRobot) from nav2_msgs.msg import Costmap, Route from nav2_msgs.srv import (ClearCostmapAroundPose, ClearCostmapAroundRobot, ClearCostmapExceptRegion, ClearEntireCostmap, GetCostmap, LoadMap, @@ -69,6 +70,7 @@ class RunningTask(Enum): DOCK_ROBOT = 10 UNDOCK_ROBOT = 11 COMPUTE_AND_TRACK_ROUTE = 12 + FOLLOW_OBJECT = 13 class BasicNavigator(Node): @@ -199,6 +201,11 @@ def __init__(self, node_name: str = 'basic_navigator', namespace: str = '') -> N UndockRobot.Result, UndockRobot.Feedback ] = ActionClient(self, UndockRobot, 'undock_robot') + self.following_client: ActionClient[ + FollowObject.Goal, + FollowObject.Result, + FollowObject.Feedback + ] = ActionClient(self, FollowObject, 'follow_object') self.localization_pose_sub = self.create_subscription( PoseWithCovarianceStamped, @@ -561,8 +568,8 @@ def dockRobotByPose(self, dock_pose: PoseStamped, goal_msg.navigate_to_staging_pose = nav_to_dock # if want to navigate before staging self.info('Docking at pose: ' + str(dock_pose) + '...') - send_goal_future = self.docking_client.send_goal_async(goal_msg, - self._feedbackCallback) + send_goal_future = self.docking_client.send_goal_async( + goal_msg, self._feedbackCallback) rclpy.spin_until_future_complete(self, send_goal_future) self.goal_handle = send_goal_future.result() @@ -588,8 +595,8 @@ def dockRobotByID(self, dock_id: str, nav_to_dock: bool = True) -> Optional[Runn goal_msg.navigate_to_staging_pose = nav_to_dock # if want to navigate before staging self.info('Docking at dock ID: ' + str(dock_id) + '...') - send_goal_future = self.docking_client.send_goal_async(goal_msg, - self._feedbackCallback) + send_goal_future = self.docking_client.send_goal_async( + goal_msg, self._feedbackCallback) rclpy.spin_until_future_complete(self, send_goal_future) self.goal_handle = send_goal_future.result() @@ -613,8 +620,8 @@ def undockRobot(self, dock_type: str = '') -> Optional[RunningTask]: goal_msg.dock_type = dock_type self.info('Undocking from dock of type: ' + str(dock_type) + '...') - send_goal_future = self.undocking_client.send_goal_async(goal_msg, - self._feedbackCallback) + send_goal_future = self.undocking_client.send_goal_async( + goal_msg, self._feedbackCallback) rclpy.spin_until_future_complete(self, send_goal_future) self.goal_handle = send_goal_future.result() @@ -627,6 +634,58 @@ def undockRobot(self, dock_type: str = '') -> Optional[RunningTask]: self.result_future = self.goal_handle.get_result_async() return RunningTask.UNDOCK_ROBOT + def followObjectByTopic(self, topic: str, max_duration: int = 0) -> Optional[RunningTask]: + """Send a `FollowObject` action request.""" + self.clearTaskError() + self.info("Waiting for 'FollowObject' action server") + while not self.following_client.wait_for_server(timeout_sec=1.0): + self.info('"FollowObject" action server not available, waiting...') + + goal_msg = FollowObject.Goal() + goal_msg.pose_topic = topic + goal_msg.max_duration = Duration(sec=max_duration) + + self.info('Following object on topic: ' + str(topic) + '...') + send_goal_future = self.following_client.send_goal_async( + goal_msg, self._feedbackCallback) + rclpy.spin_until_future_complete(self, send_goal_future) + self.goal_handle = send_goal_future.result() + + if not self.goal_handle or not self.goal_handle.accepted: + msg = 'FollowObject request was rejected!' + self.setTaskError(FollowObject.Result.UNKNOWN, msg) + self.error(msg) + return None + + self.result_future = self.goal_handle.get_result_async() + return RunningTask.FOLLOW_OBJECT + + def followObjectByFrame(self, frame: str, max_duration: int = 0) -> Optional[RunningTask]: + """Send a `FollowObject` action request.""" + self.clearTaskError() + self.info("Waiting for 'FollowObject' action server") + while not self.following_client.wait_for_server(timeout_sec=1.0): + self.info('"FollowObject" action server not available, waiting...') + + goal_msg = FollowObject.Goal() + goal_msg.tracked_frame = frame + goal_msg.max_duration = Duration(sec=max_duration) + + self.info('Following object in frame: ' + str(frame) + '...') + send_goal_future = self.following_client.send_goal_async( + goal_msg, self._feedbackCallback) + rclpy.spin_until_future_complete(self, send_goal_future) + self.goal_handle = send_goal_future.result() + + if not self.goal_handle or not self.goal_handle.accepted: + msg = 'FollowObject request was rejected!' + self.setTaskError(FollowObject.Result.UNKNOWN, msg) + self.error(msg) + return None + + self.result_future = self.goal_handle.get_result_async() + return RunningTask.FOLLOW_OBJECT + def cancelTask(self) -> None: """Cancel pending task request of any type.""" self.info('Canceling current task.') diff --git a/nav2_system_tests/src/system/nav2_system_params.yaml b/nav2_system_tests/src/system/nav2_system_params.yaml index 3ee779d90a0..46d678e08c3 100644 --- a/nav2_system_tests/src/system/nav2_system_params.yaml +++ b/nav2_system_tests/src/system/nav2_system_params.yaml @@ -59,6 +59,7 @@ bt_navigator: - compute_path - dock_robot - drive_on_heading + - follow_object - follow_path - nav_thru_poses - nav_to_pose diff --git a/navigation2/package.xml b/navigation2/package.xml index 225861a4ef4..9d1003ab2c6 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -48,6 +48,7 @@ opennav_docking opennav_docking_bt opennav_docking_core + opennav_following ament_cmake