diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 0569c58bc9..224e8498e1 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -69,6 +69,9 @@ list(APPEND plugin_libs nav2_spin_cancel_bt_node) add_library(nav2_back_up_cancel_bt_node SHARED plugins/action/back_up_cancel_node.cpp) list(APPEND plugin_libs nav2_back_up_cancel_bt_node) +add_library(nav2_assisted_teleop_cancel_bt_node SHARED plugins/action/assisted_teleop_cancel_node.cpp) +list(APPEND plugin_libs nav2_assisted_teleop_cancel_bt_node) + add_library(nav2_drive_on_heading_cancel_bt_node SHARED plugins/action/drive_on_heading_cancel_node.cpp) list(APPEND plugin_libs nav2_drive_on_heading_cancel_bt_node) @@ -90,6 +93,9 @@ list(APPEND plugin_libs nav2_spin_action_bt_node) add_library(nav2_wait_action_bt_node SHARED plugins/action/wait_action.cpp) list(APPEND plugin_libs nav2_wait_action_bt_node) +add_library(nav2_assisted_teleop_action_bt_node SHARED plugins/action/assisted_teleop_action.cpp) +list(APPEND plugin_libs nav2_assisted_teleop_action_bt_node) + add_library(nav2_clear_costmap_service_bt_node SHARED plugins/action/clear_costmap_service.cpp) list(APPEND plugin_libs nav2_clear_costmap_service_bt_node) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp new file mode 100644 index 0000000000..22cde94c35 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp @@ -0,0 +1,70 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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__ASSISTED_TELEOP_ACTION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__ASSISTED_TELEOP_ACTION_HPP_ + +#include + +#include "nav2_behavior_tree/bt_action_node.hpp" +#include "nav2_msgs/action/assisted_teleop.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::AssistedTeleop + */ +class AssistedTeleopAction : public BtActionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::nav2_msgs::action::AssistedTeleop + * @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 + */ + AssistedTeleopAction( + 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; + + BT::NodeStatus on_aborted() 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("time_allowance", 10.0, "Allowed time for running assisted teleop"), + BT::InputPort("is_recovery", false, "If true the recovery count will be incremented") + }); + } + +private: + bool is_recovery_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__ASSISTED_TELEOP_ACTION_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.hpp new file mode 100644 index 0000000000..55fe337fe8 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.hpp @@ -0,0 +1,59 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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__ASSISTED_TELEOP_CANCEL_NODE_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__ASSISTED_TELEOP_CANCEL_NODE_HPP_ + +#include +#include + +#include "nav2_msgs/action/assisted_teleop.hpp" + +#include "nav2_behavior_tree/bt_cancel_action_node.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::BackUp + */ +class AssistedTeleopCancel : public BtCancelActionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::BackUpAction + * @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 + */ + AssistedTeleopCancel( + 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__ASSISTED_TELEOP_CANCEL_NODE_HPP_ diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 3f5fa4b522..c34a171fc8 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -43,6 +43,11 @@ Server timeout + + Service name to cancel the assisted teleop behavior + Server timeout + + Service name to cancel the wait behavior Server timeout @@ -164,6 +169,13 @@ Server timeout + + Allowed time for spinning + If true recovery count will be incremented + Service name + Server timeout + + Destination diff --git a/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp new file mode 100644 index 0000000000..025f4786fc --- /dev/null +++ b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp @@ -0,0 +1,62 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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/assisted_teleop_action.hpp" + +namespace nav2_behavior_tree +{ + +AssistedTeleopAction::AssistedTeleopAction( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) +: BtActionNode(xml_tag_name, action_name, conf) +{ + double time_allowance; + getInput("time_allowance", time_allowance); + getInput("is_recovery", is_recovery_); + + // Populate the input message + goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); +} + +void AssistedTeleopAction::on_tick() +{ + if (is_recovery_) { + increment_recovery_count(); + } +} + +BT::NodeStatus AssistedTeleopAction::on_aborted() +{ + return is_recovery_ ? BT::NodeStatus::FAILURE : BT::NodeStatus::SUCCESS; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "assisted_teleop", config); + }; + + factory.registerBuilder("AssistedTeleop", builder); +} diff --git a/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.cpp b/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.cpp new file mode 100644 index 0000000000..e226ba1975 --- /dev/null +++ b/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.cpp @@ -0,0 +1,47 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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 "std_msgs/msg/string.hpp" + +#include "nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.hpp" + +namespace nav2_behavior_tree +{ + +AssistedTeleopCancel::AssistedTeleopCancel( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) +: BtCancelActionNode(xml_tag_name, action_name, conf) +{ +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "assisted_teleop", config); + }; + + factory.registerBuilder( + "CancelAssistedTeleop", builder); +} diff --git a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt index 3c6e595ba7..0b7cae1a6c 100644 --- a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt @@ -19,6 +19,10 @@ ament_add_gtest(test_action_wait_action test_wait_action.cpp) target_link_libraries(test_action_wait_action nav2_wait_action_bt_node) ament_target_dependencies(test_action_wait_action ${dependencies}) +ament_add_gtest(test_action_assisted_teleop_action test_assisted_teleop_action.cpp) +target_link_libraries(test_action_assisted_teleop_action nav2_assisted_teleop_action_bt_node) +ament_target_dependencies(test_action_assisted_teleop_action ${dependencies}) + ament_add_gtest(test_action_controller_cancel_action test_controller_cancel_node.cpp) target_link_libraries(test_action_controller_cancel_action nav2_controller_cancel_bt_node) ament_target_dependencies(test_action_controller_cancel_action ${dependencies}) @@ -36,6 +40,10 @@ ament_add_gtest(test_action_back_up_cancel_action test_back_up_cancel_node.cpp) target_link_libraries(test_action_back_up_cancel_action nav2_back_up_cancel_bt_node) ament_target_dependencies(test_action_back_up_cancel_action ${dependencies}) +ament_add_gtest(test_action_assisted_teleop_cancel_action test_assisted_teleop_cancel_node.cpp) +target_link_libraries(test_action_assisted_teleop_cancel_action nav2_assisted_teleop_cancel_bt_node) +ament_target_dependencies(test_action_assisted_teleop_cancel_action ${dependencies}) + ament_add_gtest(test_action_drive_on_heading_cancel_action test_drive_on_heading_cancel_node.cpp) target_link_libraries(test_action_drive_on_heading_cancel_action nav2_drive_on_heading_cancel_bt_node) ament_target_dependencies(test_action_drive_on_heading_cancel_action ${dependencies}) diff --git a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp new file mode 100644 index 0000000000..d5b033184b --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp @@ -0,0 +1,215 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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_v3/bt_factory.h" + +#include "../../test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp" + +class AssistedTeleopActionServer : public TestActionServer +{ +public: + AssistedTeleopActionServer() + : TestActionServer("assisted_teleop") + {} + +protected: + void execute( + const typename std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle) + override + { + nav2_msgs::action::AssistedTeleop::Result::SharedPtr result = + std::make_shared(); + bool return_success = getReturnSuccess(); + if (return_success) { + goal_handle->succeed(result); + } else { + goal_handle->abort(result); + } + } +}; + +class AssistedTeleopActionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("backup_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("initial_pose_received", false); + config_->blackboard->set("number_recoveries", 0); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "assisted_teleop", config); + }; + + factory_->registerBuilder("AssistedTeleop", builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + action_server_.reset(); + factory_.reset(); + } + + void SetUp() override + { + config_->blackboard->set("number_recoveries", 0); + } + + void TearDown() override + { + tree_.reset(); + } + + static std::shared_ptr action_server_; + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr AssistedTeleopActionTestFixture::node_ = nullptr; +std::shared_ptr +AssistedTeleopActionTestFixture::action_server_ = nullptr; +BT::NodeConfiguration * AssistedTeleopActionTestFixture::config_ = nullptr; +std::shared_ptr AssistedTeleopActionTestFixture::factory_ = nullptr; +std::shared_ptr AssistedTeleopActionTestFixture::tree_ = nullptr; + +TEST_F(AssistedTeleopActionTestFixture, test_ports) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + EXPECT_EQ(tree_->rootNode()->getInput("time_allowance"), 10.0); + + xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + EXPECT_EQ(tree_->rootNode()->getInput("time_allowance"), 20.0); +} + +TEST_F(AssistedTeleopActionTestFixture, test_tick) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + EXPECT_EQ(config_->blackboard->get("number_recoveries"), 0); + + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(config_->blackboard->get("number_recoveries"), 1); + + auto goal = action_server_->getCurrentGoal(); + EXPECT_EQ(goal->time_allowance.sec, 10.0); +} + +TEST_F(AssistedTeleopActionTestFixture, test_failure) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + action_server_->setReturnSuccess(false); + EXPECT_EQ(config_->blackboard->get("number_recoveries"), 0); + + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS && + tree_->rootNode()->status() != BT::NodeStatus::FAILURE) + { + tree_->rootNode()->executeTick(); + } + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE); + EXPECT_EQ(config_->blackboard->get("number_recoveries"), 1); + + auto goal = action_server_->getCurrentGoal(); + EXPECT_EQ(goal->time_allowance.sec, 10.0); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize action server and spin on new thread + AssistedTeleopActionTestFixture::action_server_ = std::make_shared(); + std::thread server_thread([]() { + rclcpp::spin(AssistedTeleopActionTestFixture::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_assisted_teleop_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp new file mode 100644 index 0000000000..006370b697 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp @@ -0,0 +1,173 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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_v3/bt_factory.h" + +#include "../../test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.hpp" +#include "lifecycle_msgs/srv/change_state.hpp" + +class CancelAssistedTeleopServer : public TestActionServer +{ +public: + CancelAssistedTeleopServer() + : TestActionServer("assisted_teleop") + {} + +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 CancelAssistedTeleopActionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("cancel_back_up_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)); + client_ = rclcpp_action::create_client( + node_, "assisted_teleop"); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "assisted_teleop", config); + }; + + factory_->registerBuilder( + "CancelAssistedTeleop", 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 rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr CancelAssistedTeleopActionTestFixture::node_ = nullptr; +std::shared_ptr +CancelAssistedTeleopActionTestFixture::action_server_ = nullptr; +std::shared_ptr> +CancelAssistedTeleopActionTestFixture::client_ = nullptr; + +BT::NodeConfiguration * CancelAssistedTeleopActionTestFixture::config_ = nullptr; +std::shared_ptr +CancelAssistedTeleopActionTestFixture::factory_ = nullptr; +std::shared_ptr CancelAssistedTeleopActionTestFixture::tree_ = nullptr; + +TEST_F(CancelAssistedTeleopActionTestFixture, test_ports) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + auto send_goal_options = rclcpp_action::Client< + nav2_msgs::action::AssistedTeleop>::SendGoalOptions(); + + // Creating a dummy goal_msg + auto goal_msg = nav2_msgs::action::AssistedTeleop::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 infact 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 + CancelAssistedTeleopActionTestFixture::action_server_ = + std::make_shared(); + std::thread server_thread([]() { + rclcpp::spin(CancelAssistedTeleopActionTestFixture::action_server_); + }); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + server_thread.join(); + + return all_successful; +} diff --git a/nav2_behaviors/CMakeLists.txt b/nav2_behaviors/CMakeLists.txt index 273c5fe664..df200309d9 100644 --- a/nav2_behaviors/CMakeLists.txt +++ b/nav2_behaviors/CMakeLists.txt @@ -79,6 +79,14 @@ ament_target_dependencies(nav2_back_up_behavior ${dependencies} ) +add_library(nav2_assisted_teleop_behavior SHARED + plugins/assisted_teleop.cpp +) + +ament_target_dependencies(nav2_assisted_teleop_behavior + ${dependencies} +) + pluginlib_export_plugin_description_file(nav2_core behavior_plugin.xml) # Library @@ -106,6 +114,7 @@ rclcpp_components_register_nodes(${library_name} "behavior_server::BehaviorServe install(TARGETS ${library_name} nav2_spin_behavior nav2_wait_behavior + nav2_assisted_teleop_behavior nav2_drive_on_heading_behavior nav2_back_up_behavior ARCHIVE DESTINATION lib @@ -140,6 +149,7 @@ ament_export_include_directories(include) ament_export_libraries(${library_name} nav2_spin_behavior nav2_wait_behavior + nav2_assisted_teleop_behavior nav2_drive_on_heading_behavior nav2_back_up_behavior ) diff --git a/nav2_behaviors/behavior_plugin.xml b/nav2_behaviors/behavior_plugin.xml index 5989f566a9..cd20ae7aa9 100644 --- a/nav2_behaviors/behavior_plugin.xml +++ b/nav2_behaviors/behavior_plugin.xml @@ -22,4 +22,10 @@ + + + + + + diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp index 06528be887..7d38f38167 100644 --- a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp +++ b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp @@ -95,6 +95,11 @@ class TimedBehavior : public nav2_core::Behavior { } + // an opportunity for a derived class to do something on action completion + virtual void onActionCompletion() + { + } + // configure the server on lifecycle setup void configure( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, @@ -167,6 +172,7 @@ class TimedBehavior : public nav2_core::Behavior std::string global_frame_; std::string robot_base_frame_; double transform_tolerance_; + rclcpp::Duration elasped_time_{0, 0}; // Clock rclcpp::Clock steady_clock_{RCL_STEADY_TIME}; @@ -203,11 +209,13 @@ class TimedBehavior : public nav2_core::Behavior rclcpp::WallRate loop_rate(cycle_frequency_); while (rclcpp::ok()) { + elasped_time_ = steady_clock_.now() - start_time; if (action_server_->is_cancel_requested()) { RCLCPP_INFO(logger_, "Canceling %s", behavior_name_.c_str()); stopRobot(); - result->total_elapsed_time = steady_clock_.now() - start_time; + result->total_elapsed_time = elasped_time_; action_server_->terminate_all(result); + onActionCompletion(); return; } @@ -220,6 +228,7 @@ class TimedBehavior : public nav2_core::Behavior stopRobot(); result->total_elapsed_time = steady_clock_.now() - start_time; action_server_->terminate_current(result); + onActionCompletion(); return; } @@ -230,12 +239,14 @@ class TimedBehavior : public nav2_core::Behavior "%s completed successfully", behavior_name_.c_str()); result->total_elapsed_time = steady_clock_.now() - start_time; action_server_->succeeded_current(result); + onActionCompletion(); return; case Status::FAILED: RCLCPP_WARN(logger_, "%s failed", behavior_name_.c_str()); result->total_elapsed_time = steady_clock_.now() - start_time; action_server_->terminate_current(result); + onActionCompletion(); return; case Status::RUNNING: diff --git a/nav2_behaviors/plugins/assisted_teleop.cpp b/nav2_behaviors/plugins/assisted_teleop.cpp new file mode 100644 index 0000000000..defdfaf5a7 --- /dev/null +++ b/nav2_behaviors/plugins/assisted_teleop.cpp @@ -0,0 +1,186 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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 "assisted_teleop.hpp" +#include "nav2_util/node_utils.hpp" + +namespace nav2_behaviors +{ +AssistedTeleop::AssistedTeleop() +: TimedBehavior(), + feedback_(std::make_shared()) +{} + +void AssistedTeleop::onConfigure() +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + // set up parameters + nav2_util::declare_parameter_if_not_declared( + node, + "projection_time", rclcpp::ParameterValue(1.0)); + + nav2_util::declare_parameter_if_not_declared( + node, + "simulation_time_step", rclcpp::ParameterValue(0.1)); + + nav2_util::declare_parameter_if_not_declared( + node, + "cmd_vel_teleop", rclcpp::ParameterValue(std::string("cmd_vel_teleop"))); + + node->get_parameter("projection_time", projection_time_); + node->get_parameter("simulation_time_step", simulation_time_step_); + + std::string cmd_vel_teleop; + node->get_parameter("cmd_vel_teleop", cmd_vel_teleop); + + vel_sub_ = node->create_subscription( + cmd_vel_teleop, rclcpp::SystemDefaultsQoS(), + std::bind( + &AssistedTeleop::teleopVelocityCallback, + this, std::placeholders::_1)); + + preempt_teleop_sub_ = node->create_subscription( + "preempt_teleop", rclcpp::SystemDefaultsQoS(), + std::bind( + &AssistedTeleop::preemptTeleopCallback, + this, std::placeholders::_1)); +} + +Status AssistedTeleop::onRun(const std::shared_ptr command) +{ + preempt_teleop_ = false; + command_time_allowance_ = command->time_allowance; + end_time_ = steady_clock_.now() + command_time_allowance_; + return Status::SUCCEEDED; +} + +void AssistedTeleop::onActionCompletion() +{ + teleop_twist_ = geometry_msgs::msg::Twist(); + preempt_teleop_ = false; +} + +Status AssistedTeleop::onCycleUpdate() +{ + feedback_->current_teleop_duration = elasped_time_; + action_server_->publish_feedback(feedback_); + + rclcpp::Duration time_remaining = end_time_ - steady_clock_.now(); + if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) { + stopRobot(); + RCLCPP_WARN_STREAM( + logger_, + "Exceeded time allowance before reaching the " << behavior_name_.c_str() << + "goal - Exiting " << behavior_name_.c_str()); + return Status::FAILED; + } + + // user states that teleop was successful + if (preempt_teleop_) { + stopRobot(); + return Status::SUCCEEDED; + } + + geometry_msgs::msg::PoseStamped current_pose; + if (!nav2_util::getCurrentPose( + current_pose, *tf_, global_frame_, robot_base_frame_, + transform_tolerance_)) + { + RCLCPP_ERROR_STREAM( + logger_, + "Current robot pose is not available for " << + behavior_name_.c_str()); + return Status::FAILED; + } + geometry_msgs::msg::Pose2D projected_pose; + projected_pose.x = current_pose.pose.position.x; + projected_pose.y = current_pose.pose.position.y; + projected_pose.theta = tf2::getYaw(current_pose.pose.orientation); + + geometry_msgs::msg::Twist scaled_twist = teleop_twist_; + for (double time = simulation_time_step_; time < projection_time_; + time += simulation_time_step_) + { + projected_pose = projectPose(projected_pose, teleop_twist_, simulation_time_step_); + + if (!collision_checker_->isCollisionFree(projected_pose)) { + if (time == simulation_time_step_) { + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, + steady_clock_, + 1000, + behavior_name_.c_str() << " collided on first time step, setting velocity to zero"); + scaled_twist.linear.x = 0.0f; + scaled_twist.linear.y = 0.0f; + scaled_twist.angular.z = 0.0f; + break; + } else { + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, + steady_clock_, + 1000, + behavior_name_.c_str() << " collision approaching in " << time << " seconds"); + double scale_factor = time / projection_time_; + scaled_twist.linear.x *= scale_factor; + scaled_twist.linear.y *= scale_factor; + scaled_twist.angular.z *= scale_factor; + break; + } + } + } + vel_pub_->publish(std::move(scaled_twist)); + + return Status::RUNNING; +} + +geometry_msgs::msg::Pose2D AssistedTeleop::projectPose( + const geometry_msgs::msg::Pose2D & pose, + const geometry_msgs::msg::Twist & twist, + double projection_time) +{ + geometry_msgs::msg::Pose2D projected_pose = pose; + + projected_pose.x += projection_time * ( + twist.linear.x * cos(pose.theta) + + twist.linear.y * sin(pose.theta)); + + projected_pose.y += projection_time * ( + twist.linear.x * sin(pose.theta) - + twist.linear.y * cos(pose.theta)); + + projected_pose.theta += projection_time * twist.angular.z; + + return projected_pose; +} + +void AssistedTeleop::teleopVelocityCallback(const geometry_msgs::msg::Twist::SharedPtr msg) +{ + teleop_twist_ = *msg; +} + +void AssistedTeleop::preemptTeleopCallback(const std_msgs::msg::Empty::SharedPtr) +{ + preempt_teleop_ = true; +} + +} // namespace nav2_behaviors + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_behaviors::AssistedTeleop, nav2_core::Behavior) diff --git a/nav2_behaviors/plugins/assisted_teleop.hpp b/nav2_behaviors/plugins/assisted_teleop.hpp new file mode 100644 index 0000000000..12a85f327c --- /dev/null +++ b/nav2_behaviors/plugins/assisted_teleop.hpp @@ -0,0 +1,105 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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_BEHAVIORS__PLUGINS__ASSISTED_TELEOP_HPP_ +#define NAV2_BEHAVIORS__PLUGINS__ASSISTED_TELEOP_HPP_ + +#include +#include +#include + +#include "geometry_msgs/msg/twist.hpp" +#include "std_msgs/msg/empty.hpp" +#include "nav2_behaviors/timed_behavior.hpp" +#include "nav2_msgs/action/assisted_teleop.hpp" + +namespace nav2_behaviors +{ +using AssistedTeleopAction = nav2_msgs::action::AssistedTeleop; + +/** + * @class nav2_behaviors::AssistedTeleop + * @brief An action server behavior for assisted teleop + */ +class AssistedTeleop : public TimedBehavior +{ +public: + AssistedTeleop(); + + /** + * @brief Initialization to run behavior + * @param command Goal to execute + * @return Status of behavior + */ + Status onRun(const std::shared_ptr command) override; + + /** + * @brief func to run at the completion of the action + */ + void onActionCompletion() override; + + /** + * @brief Loop function to run behavior + * @return Status of behavior + */ + Status onCycleUpdate() override; + +protected: + /** + * @brief Configuration of behavior action + */ + void onConfigure() override; + + /** + * @brief project a position + * @param pose initial pose to project + * @param twist velocity to project pose by + * @param projection_time time to project by + */ + geometry_msgs::msg::Pose2D projectPose( + const geometry_msgs::msg::Pose2D & pose, + const geometry_msgs::msg::Twist & twist, + double projection_time); + + /** + * @brief Callback function for velocity subscriber + * @param msg received Twist message + */ + void teleopVelocityCallback(const geometry_msgs::msg::Twist::SharedPtr msg); + + /** + * @brief Callback function to preempt assisted teleop + * @param msg empty message + */ + void preemptTeleopCallback(const std_msgs::msg::Empty::SharedPtr msg); + + AssistedTeleopAction::Feedback::SharedPtr feedback_; + + // parameters + double projection_time_; + double simulation_time_step_; + + geometry_msgs::msg::Twist teleop_twist_; + bool preempt_teleop_{false}; + + // subscribers + rclcpp::Subscription::SharedPtr vel_sub_; + rclcpp::Subscription::SharedPtr preempt_teleop_sub_; + + rclcpp::Duration command_time_allowance_{0, 0}; + rclcpp::Time end_time_; +}; +} // namespace nav2_behaviors + +#endif // NAV2_BEHAVIORS__PLUGINS__ASSISTED_TELEOP_HPP_ diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml index 11e5502246..0e674039cf 100644 --- a/nav2_bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml @@ -56,6 +56,7 @@ bt_navigator: - nav2_follow_path_action_bt_node - nav2_spin_action_bt_node - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node - nav2_back_up_action_bt_node - nav2_drive_on_heading_bt_node - nav2_clear_costmap_service_bt_node @@ -93,6 +94,7 @@ bt_navigator: - nav2_wait_cancel_bt_node - nav2_spin_cancel_bt_node - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node - nav2_drive_on_heading_cancel_bt_node controller_server: diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml index 7e2b3551c8..6a8e6400a7 100644 --- a/nav2_bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml @@ -56,6 +56,7 @@ bt_navigator: - nav2_follow_path_action_bt_node - nav2_spin_action_bt_node - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node - nav2_back_up_action_bt_node - nav2_drive_on_heading_bt_node - nav2_clear_costmap_service_bt_node @@ -93,6 +94,7 @@ bt_navigator: - nav2_wait_cancel_bt_node - nav2_spin_cancel_bt_node - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node - nav2_drive_on_heading_cancel_bt_node controller_server: diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index c97ed6a2e1..8593587499 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -56,6 +56,7 @@ bt_navigator: - nav2_follow_path_action_bt_node - nav2_spin_action_bt_node - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node - nav2_back_up_action_bt_node - nav2_drive_on_heading_bt_node - nav2_clear_costmap_service_bt_node @@ -93,6 +94,7 @@ bt_navigator: - nav2_wait_cancel_bt_node - nav2_spin_cancel_bt_node - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node - nav2_drive_on_heading_cancel_bt_node controller_server: @@ -279,7 +281,7 @@ behavior_server: costmap_topic: local_costmap/costmap_raw footprint_topic: local_costmap/published_footprint cycle_frequency: 10.0 - behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"] + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] spin: plugin: "nav2_behaviors/Spin" backup: @@ -288,6 +290,8 @@ behavior_server: plugin: "nav2_behaviors/DriveOnHeading" wait: plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" global_frame: odom robot_base_frame: base_link transform_tolerance: 0.1 diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 5002c6f264..7c83ff9d9a 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -40,6 +40,7 @@ BtNavigator::BtNavigator(const rclcpp::NodeOptions & options) "nav2_follow_path_action_bt_node", "nav2_spin_action_bt_node", "nav2_wait_action_bt_node", + "nav2_assisted_teleop_action_bt_node", "nav2_back_up_action_bt_node", "nav2_drive_on_heading_bt_node", "nav2_clear_costmap_service_bt_node", @@ -76,6 +77,8 @@ BtNavigator::BtNavigator(const rclcpp::NodeOptions & options) "nav2_path_longer_on_approach_bt_node", "nav2_wait_cancel_bt_node", "nav2_spin_cancel_bt_node", + "nav2_assisted_teleop_cancel_bt_node", + "nav2_back_up_cancel_bt_node", "nav2_back_up_cancel_bt_node", "nav2_drive_on_heading_cancel_bt_node" }; diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index ce7376cebb..2be81db0a7 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -30,6 +30,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/ManageLifecycleNodes.srv" "srv/LoadMap.srv" "srv/SaveMap.srv" + "action/AssistedTeleop.action" "action/BackUp.action" "action/ComputePathToPose.action" "action/ComputePathThroughPoses.action" diff --git a/nav2_msgs/action/AssistedTeleop.action b/nav2_msgs/action/AssistedTeleop.action new file mode 100644 index 0000000000..ba830ebc12 --- /dev/null +++ b/nav2_msgs/action/AssistedTeleop.action @@ -0,0 +1,8 @@ +#goal definition +builtin_interfaces/Duration time_allowance +--- +#result definition +builtin_interfaces/Duration total_elapsed_time +--- +#feedback +builtin_interfaces/Duration current_teleop_duration diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 6cc6a32e49..6d56df1ef7 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -65,6 +65,7 @@ if(BUILD_TESTING) add_subdirectory(src/behaviors/wait) add_subdirectory(src/behaviors/backup) add_subdirectory(src/behaviors/drive_on_heading) + add_subdirectory(src/behaviors/assisted_teleop) add_subdirectory(src/costmap_filters) install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) diff --git a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp index b5bb56a6f1..d7ede55cd1 100644 --- a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp +++ b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp @@ -58,6 +58,7 @@ class BehaviorTreeHandler "nav2_follow_path_action_bt_node", "nav2_spin_action_bt_node", "nav2_wait_action_bt_node", + "nav2_assisted_teleop_action_bt_node", "nav2_back_up_action_bt_node", "nav2_drive_on_heading_bt_node", "nav2_clear_costmap_service_bt_node", @@ -91,6 +92,7 @@ class BehaviorTreeHandler "nav2_goal_checker_selector_bt_node", "nav2_controller_cancel_bt_node", "nav2_path_longer_on_approach_bt_node", + "nav2_assisted_teleop_cancel_bt_node", "nav2_wait_cancel_bt_node", "nav2_spin_cancel_bt_node", "nav2_back_up_cancel_bt_node", diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/CMakeLists.txt b/nav2_system_tests/src/behaviors/assisted_teleop/CMakeLists.txt new file mode 100644 index 0000000000..c2210b6fcb --- /dev/null +++ b/nav2_system_tests/src/behaviors/assisted_teleop/CMakeLists.txt @@ -0,0 +1,23 @@ +set(test_assisted_teleop_behavior test_assisted_teleop_behavior_node) + +ament_add_gtest_executable(${test_assisted_teleop_behavior} + test_assisted_teleop_behavior_node.cpp + assisted_teleop_behavior_tester.cpp +) + +ament_target_dependencies(${test_assisted_teleop_behavior} + ${dependencies} +) + +ament_add_test(test_assisted_teleop_behavior + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_assisted_teleop_behavior_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml + TEST_EXECUTABLE=$ + TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world + GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models + BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml +) diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp new file mode 100644 index 0000000000..bcc1950bde --- /dev/null +++ b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp @@ -0,0 +1,277 @@ +// Copyright (c) 2020 Sarthak Mittal +// Copyright (c) 2018 Intel Corporation +// +// 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. Reserved. + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "assisted_teleop_behavior_tester.hpp" +#include "nav2_util/geometry_utils.hpp" + +using namespace std::chrono_literals; +using namespace std::chrono; // NOLINT + +namespace nav2_system_tests +{ + +AssistedTeleopBehaviorTester::AssistedTeleopBehaviorTester() +: is_active_(false), + initial_pose_received_(false) +{ + node_ = rclcpp::Node::make_shared("assisted_teleop_behavior_test"); + + tf_buffer_ = std::make_shared(node_->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + client_ptr_ = rclcpp_action::create_client( + node_->get_node_base_interface(), + node_->get_node_graph_interface(), + node_->get_node_logging_interface(), + node_->get_node_waitables_interface(), + "assisted_teleop"); + + initial_pose_pub_ = + node_->create_publisher("initialpose", 10); + + preempt_pub_ = + node_->create_publisher("preempt_teleop", 10); + + cmd_vel_pub_ = + node_->create_publisher("cmd_vel_teleop", 10); + + subscription_ = node_->create_subscription( + "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&AssistedTeleopBehaviorTester::amclPoseCallback, this, std::placeholders::_1)); + + filtered_vel_sub_ = node_->create_subscription( + "cmd_vel", + rclcpp::SystemDefaultsQoS(), + std::bind(&AssistedTeleopBehaviorTester::filteredVelCallback, this, std::placeholders::_1)); + + std::string costmap_topic = "/local_costmap/costmap_raw"; + std::string footprint_topic = "/local_costmap/published_footprint"; + + costmap_sub_ = std::make_shared( + node_, + costmap_topic); + + footprint_sub_ = std::make_shared( + node_, + footprint_topic, + *tf_buffer_); + + collision_checker_ = std::make_unique( + *costmap_sub_, + *footprint_sub_ + ); + + stamp_ = node_->now(); +} + +AssistedTeleopBehaviorTester::~AssistedTeleopBehaviorTester() +{ + if (is_active_) { + deactivate(); + } +} + +void AssistedTeleopBehaviorTester::activate() +{ + if (is_active_) { + throw std::runtime_error("Trying to activate while already active"); + return; + } + + while (!initial_pose_received_) { + RCLCPP_WARN(node_->get_logger(), "Initial pose not received"); + sendInitialPose(); + std::this_thread::sleep_for(100ms); + rclcpp::spin_some(node_); + } + + // Wait for lifecycle_manager_navigation to activate behavior_server + std::this_thread::sleep_for(10s); + + if (!client_ptr_) { + RCLCPP_ERROR(node_->get_logger(), "Action client not initialized"); + is_active_ = false; + return; + } + + if (!client_ptr_->wait_for_action_server(10s)) { + RCLCPP_ERROR(node_->get_logger(), "Action server not available after waiting"); + is_active_ = false; + return; + } + + RCLCPP_INFO(this->node_->get_logger(), "Assisted Teleop action server is ready"); + is_active_ = true; +} + +void AssistedTeleopBehaviorTester::deactivate() +{ + if (!is_active_) { + throw std::runtime_error("Trying to deactivate while already inactive"); + } + is_active_ = false; +} + +bool AssistedTeleopBehaviorTester::defaultAssistedTeleopTest( + const float lin_vel, + const float ang_vel) +{ + if (!is_active_) { + RCLCPP_ERROR(node_->get_logger(), "Not activated"); + return false; + } + + RCLCPP_INFO(node_->get_logger(), "Sending goal"); + + auto goal_handle_future = client_ptr_->async_send_goal(nav2_msgs::action::AssistedTeleop::Goal()); + + if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node_->get_logger(), "send goal call failed :("); + return false; + } + + rclcpp_action::ClientGoalHandle::SharedPtr goal_handle = goal_handle_future.get(); + if (!goal_handle) { + RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); + return false; + } + + // Wait for the server to be done with the goal + auto result_future = client_ptr_->async_get_result(goal_handle); + + rclcpp::Rate r(1); + + counter_ = 0; + auto start_time = std::chrono::system_clock::now(); + while (rclcpp::ok()) { + geometry_msgs::msg::Twist cmd_vel = geometry_msgs::msg::Twist(); + cmd_vel.linear.x = lin_vel; + cmd_vel.angular.z = ang_vel; + cmd_vel_pub_->publish(cmd_vel); + + if (counter_ > 1) { + break; + } + + auto current_time = std::chrono::system_clock::now(); + if (current_time - start_time > 25s) { + RCLCPP_ERROR(node_->get_logger(), "Exceeded Timeout"); + return false; + } + + rclcpp::spin_some(node_); + r.sleep(); + } + + auto preempt_msg = std_msgs::msg::Empty(); + preempt_pub_->publish(preempt_msg); + + RCLCPP_INFO(node_->get_logger(), "Waiting for result"); + if (rclcpp::spin_until_future_complete(node_, result_future) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node_->get_logger(), "get result call failed :("); + return false; + } + + rclcpp_action::ClientGoalHandle::WrappedResult + wrapped_result = result_future.get(); + + switch (wrapped_result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: break; + case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR( + node_->get_logger(), + "Goal was aborted"); + return false; + case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR( + node_->get_logger(), + "Goal was canceled"); + return false; + default: RCLCPP_ERROR(node_->get_logger(), "Unknown result code"); + return false; + } + + RCLCPP_INFO(node_->get_logger(), "result received"); + + geometry_msgs::msg::PoseStamped current_pose; + if (!nav2_util::getCurrentPose(current_pose, *tf_buffer_, "odom")) { + RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); + return false; + } + + geometry_msgs::msg::Pose2D pose_2d; + pose_2d.x = current_pose.pose.position.x; + pose_2d.y = current_pose.pose.position.y; + pose_2d.theta = tf2::getYaw(current_pose.pose.orientation); + + if (!collision_checker_->isCollisionFree(pose_2d)) { + RCLCPP_ERROR(node_->get_logger(), "Ended in collision"); + return false; + } + + return true; +} + +void AssistedTeleopBehaviorTester::sendInitialPose() +{ + geometry_msgs::msg::PoseWithCovarianceStamped pose; + pose.header.frame_id = "map"; + pose.header.stamp = stamp_; + pose.pose.pose.position.x = -2.0; + pose.pose.pose.position.y = -0.5; + pose.pose.pose.position.z = 0.0; + pose.pose.pose.orientation.x = 0.0; + pose.pose.pose.orientation.y = 0.0; + pose.pose.pose.orientation.z = 0.0; + pose.pose.pose.orientation.w = 1.0; + for (int i = 0; i < 35; i++) { + pose.pose.covariance[i] = 0.0; + } + pose.pose.covariance[0] = 0.08; + pose.pose.covariance[7] = 0.08; + pose.pose.covariance[35] = 0.05; + + initial_pose_pub_->publish(pose); + RCLCPP_INFO(node_->get_logger(), "Sent initial pose"); +} + +void AssistedTeleopBehaviorTester::amclPoseCallback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr) +{ + initial_pose_received_ = true; +} + +void AssistedTeleopBehaviorTester::filteredVelCallback( + geometry_msgs::msg::Twist::SharedPtr msg) +{ + if (msg->linear.x == 0.0f) { + counter_++; + } else { + counter_ = 0; + } +} + +} // namespace nav2_system_tests diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp new file mode 100644 index 0000000000..2cf46827e8 --- /dev/null +++ b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp @@ -0,0 +1,104 @@ +// Copyright (c) 2020 Samsung Research +// Copyright (c) 2018 Intel Corporation +// +// 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. Reserved. + +#ifndef BEHAVIORS__ASSISTED_TELEOP__ASSISTED_TELEOP_BEHAVIOR_TESTER_HPP_ +#define BEHAVIORS__ASSISTED_TELEOP__ASSISTED_TELEOP_BEHAVIOR_TESTER_HPP_ + +#include +#include +#include +#include +#include + +#include "angles/angles.h" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/pose2_d.hpp" +#include "nav2_costmap_2d/costmap_topic_collision_checker.hpp" +#include "nav2_msgs/action/assisted_teleop.hpp" +#include "nav2_util/node_thread.hpp" +#include "nav2_util/robot_utils.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/empty.hpp" + +#include "tf2/utils.h" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +namespace nav2_system_tests +{ + +class AssistedTeleopBehaviorTester +{ +public: + using AssistedTeleop = nav2_msgs::action::AssistedTeleop; + + AssistedTeleopBehaviorTester(); + ~AssistedTeleopBehaviorTester(); + + // Runs a single test with given target yaw + bool defaultAssistedTeleopTest( + const float lin_vel, + const float ang_vel); + + void activate(); + + void deactivate(); + + bool isActive() const + { + return is_active_; + } + +private: + void sendInitialPose(); + + void amclPoseCallback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr); + + void filteredVelCallback(geometry_msgs::msg::Twist::SharedPtr msg); + + unsigned int counter_; + bool is_active_; + bool initial_pose_received_; + rclcpp::Time stamp_; + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + rclcpp::Node::SharedPtr node_; + + // Publishers + rclcpp::Publisher::SharedPtr initial_pose_pub_; + rclcpp::Publisher::SharedPtr preempt_pub_; + rclcpp::Publisher::SharedPtr cmd_vel_pub_; + + // Subscribers + rclcpp::Subscription::SharedPtr subscription_; + rclcpp::Subscription::SharedPtr filtered_vel_sub_; + + // Action client to call AssistedTeleop action + rclcpp_action::Client::SharedPtr client_ptr_; + + // collision checking + std::shared_ptr costmap_sub_; + std::shared_ptr footprint_sub_; + std::unique_ptr collision_checker_; +}; + +} // namespace nav2_system_tests + +#endif // BEHAVIORS__ASSISTED_TELEOP__ASSISTED_TELEOP_BEHAVIOR_TESTER_HPP_ diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py b/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py new file mode 100755 index 0000000000..976eb02c2f --- /dev/null +++ b/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py @@ -0,0 +1,103 @@ +#! /usr/bin/env python3 +# Copyright (c) 2012 Samsung Research America +# +# 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. + +import os +import sys + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch import LaunchService +from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_testing.legacy import LaunchTestService + +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + map_yaml_file = os.getenv('TEST_MAP') + world = os.getenv('TEST_WORLD') + + bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML')) + + bringup_dir = get_package_share_directory('nav2_bringup') + params_file = os.path.join(bringup_dir, 'params/nav2_params.yaml') + + # Replace the `use_astar` setting on the params file + configured_params = RewrittenYaml( + source_file=params_file, + root_key='', + param_rewrites='', + convert_types=True) + + return LaunchDescription([ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + + # Launch gazebo server for simulation + ExecuteProcess( + cmd=['gzserver', '-s', 'libgazebo_ros_init.so', + '--minimal_comms', world], + output='screen'), + + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), + + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={ + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': configured_params, + 'bt_xml_file': bt_navigator_xml, + 'autostart': 'True'}.items()), + ]) + + +def main(argv=sys.argv[1:]): + ld = generate_launch_description() + + testExecutable = os.getenv('TEST_EXECUTABLE') + + test1_action = ExecuteProcess( + cmd=[testExecutable], + name='test_assisted_teleop_behavior_node', + output='screen') + + lts = LaunchTestService() + lts.add_test_action(ld, test1_action) + ls = LaunchService(argv=argv) + ls.include_launch_description(ld) + return lts.run(ls) + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_node.cpp b/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_node.cpp new file mode 100644 index 0000000000..eb847272eb --- /dev/null +++ b/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_node.cpp @@ -0,0 +1,114 @@ +// Copyright (c) 2020 Samsung Research +// Copyright (c) 2020 Sarthak Mittal +// Copyright (c) 2022 Joshua Wallace +// +// 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. Reserved. + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "assisted_teleop_behavior_tester.hpp" +#include "nav2_msgs/action/back_up.hpp" + +using namespace std::chrono_literals; + +using nav2_system_tests::AssistedTeleopBehaviorTester; + +struct TestParameters +{ + float lin_vel; + float ang_vel; +}; + + +std::string testNameGenerator(const testing::TestParamInfo &) +{ + static int test_index = 0; + std::string name = "AssistedTeleopTest" + std::to_string(test_index); + ++test_index; + return name; +} + +class AssistedTeleopBehaviorTestFixture + : public ::testing::TestWithParam +{ +public: + static void SetUpTestCase() + { + assisted_teleop_behavior_tester = new AssistedTeleopBehaviorTester(); + if (!assisted_teleop_behavior_tester->isActive()) { + assisted_teleop_behavior_tester->activate(); + } + } + + static void TearDownTestCase() + { + delete assisted_teleop_behavior_tester; + assisted_teleop_behavior_tester = nullptr; + } + +protected: + static AssistedTeleopBehaviorTester * assisted_teleop_behavior_tester; +}; + +AssistedTeleopBehaviorTester * +AssistedTeleopBehaviorTestFixture::assisted_teleop_behavior_tester = nullptr; + +TEST_P(AssistedTeleopBehaviorTestFixture, testAssistedTeleopBehavior) +{ + auto test_params = GetParam(); + + if (!assisted_teleop_behavior_tester->isActive()) { + assisted_teleop_behavior_tester->activate(); + } + + bool success = false; + success = assisted_teleop_behavior_tester->defaultAssistedTeleopTest( + test_params.lin_vel, + test_params.ang_vel); + + EXPECT_TRUE(success); +} + +std::vector test_params = {TestParameters{-0.1, 0.0}, + TestParameters{0.35, 0.05}}; + +INSTANTIATE_TEST_SUITE_P( + TestAssistedTeleopBehavior, + AssistedTeleopBehaviorTestFixture, + ::testing::Values( + test_params[0], + test_params[1]), + testNameGenerator +); + + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + bool all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml index d886ca3e4e..94cd7553e0 100644 --- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml +++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml @@ -54,6 +54,7 @@ bt_navigator: - nav2_follow_path_action_bt_node - nav2_spin_action_bt_node - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node - nav2_back_up_action_bt_node - nav2_drive_on_heading_bt_node - nav2_clear_costmap_service_bt_node @@ -86,6 +87,7 @@ bt_navigator: - nav2_wait_cancel_bt_node - nav2_spin_cancel_bt_node - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node - nav2_drive_on_heading_cancel_bt_node controller_server: diff --git a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml index ba5eede1ad..e9e0c4550b 100644 --- a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml @@ -54,6 +54,7 @@ bt_navigator: - nav2_follow_path_action_bt_node - nav2_spin_action_bt_node - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node - nav2_back_up_action_bt_node - nav2_drive_on_heading_bt_node - nav2_clear_costmap_service_bt_node @@ -87,6 +88,7 @@ bt_navigator: - nav2_wait_cancel_bt_node - nav2_spin_cancel_bt_node - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node - nav2_drive_on_heading_cancel_bt_node controller_server: diff --git a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml index fcf5352f9c..b2a8f82eb2 100644 --- a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml @@ -54,6 +54,7 @@ bt_navigator: - nav2_follow_path_action_bt_node - nav2_spin_action_bt_node - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node - nav2_back_up_action_bt_node - nav2_drive_on_heading_bt_node - nav2_clear_costmap_service_bt_node @@ -87,6 +88,7 @@ bt_navigator: - nav2_wait_cancel_bt_node - nav2_spin_cancel_bt_node - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node - nav2_drive_on_heading_cancel_bt_node controller_server: