diff --git a/nav2_behaviors/src/behavior_server.cpp b/nav2_behaviors/src/behavior_server.cpp index 1017bf595a3..a04aea40315 100644 --- a/nav2_behaviors/src/behavior_server.cpp +++ b/nav2_behaviors/src/behavior_server.cpp @@ -78,7 +78,7 @@ BehaviorServer::~BehaviorServer() } nav2_util::CallbackReturn -BehaviorServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +BehaviorServer::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring"); @@ -91,6 +91,7 @@ BehaviorServer::on_configure(const rclcpp_lifecycle::State & /*state*/) behavior_types_.resize(behavior_ids_.size()); if (!loadBehaviorPlugins()) { + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } setupResourcesForBehaviorPlugins(); diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 9ae64016743..c49fae0e2b9 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -56,7 +56,7 @@ BtNavigator::~BtNavigator() } nav2_util::CallbackReturn -BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) +BtNavigator::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring"); @@ -133,6 +133,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_FATAL( get_logger(), "Failed to create navigator id %s." " Exception: %s", navigator_ids[i].c_str(), ex.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } @@ -141,11 +142,12 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) } nav2_util::CallbackReturn -BtNavigator::on_activate(const rclcpp_lifecycle::State & /*state*/) +BtNavigator::on_activate(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Activating"); for (size_t i = 0; i != navigators_.size(); i++) { if (!navigators_[i]->on_activate()) { + on_deactivate(state); return nav2_util::CallbackReturn::FAILURE; } } diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index d8e03e19ed4..7f87bc1bb92 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -40,7 +40,7 @@ CollisionDetector::~CollisionDetector() } nav2_util::CallbackReturn -CollisionDetector::on_configure(const rclcpp_lifecycle::State & /*state*/) +CollisionDetector::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring"); @@ -60,6 +60,7 @@ CollisionDetector::on_configure(const rclcpp_lifecycle::State & /*state*/) // Obtaining ROS parameters if (!getParameters()) { + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index c0bd15cb63a..2b0991c9941 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -42,7 +42,7 @@ CollisionMonitor::~CollisionMonitor() } nav2_util::CallbackReturn -CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) +CollisionMonitor::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring"); @@ -60,6 +60,7 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) // Obtaining ROS parameters if (!getParameters(cmd_vel_in_topic, cmd_vel_out_topic, state_topic)) { + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } @@ -90,6 +91,7 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) nav2_util::setSoftRealTimePriority(); } catch (const std::runtime_error & e) { RCLCPP_ERROR(get_logger(), "%s", e.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index ec9dc3d81b8..c221c2db17b 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -82,7 +82,7 @@ ControllerServer::~ControllerServer() } nav2_util::CallbackReturn -ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +ControllerServer::on_configure(const rclcpp_lifecycle::State & state) { auto node = shared_from_this(); @@ -152,6 +152,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_FATAL( get_logger(), "Failed to create progress_checker. Exception: %s", ex.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } @@ -178,6 +179,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_FATAL( get_logger(), "Failed to create goal checker. Exception: %s", ex.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } @@ -206,6 +208,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_FATAL( get_logger(), "Failed to create controller. Exception: %s", ex.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } @@ -242,6 +245,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) true /*spin thread*/, server_options, use_realtime_priority_ /*soft realtime*/); } catch (const std::runtime_error & e) { RCLCPP_ERROR(get_logger(), "Error creating action server! %s", e.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index 0d5034bd60c..b350cb40262 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -43,7 +43,7 @@ DockingServer::DockingServer(const rclcpp::NodeOptions & options) } nav2_util::CallbackReturn -DockingServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +DockingServer::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring %s", get_name()); auto node = shared_from_this(); @@ -90,6 +90,7 @@ DockingServer::on_configure(const rclcpp_lifecycle::State & /*state*/) navigator_ = std::make_unique(node); dock_db_ = std::make_unique(mutex_); if (!dock_db_->initialize(node, tf2_buffer_)) { + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } diff --git a/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp b/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp index 144a612f3bd..f12741774c8 100644 --- a/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp +++ b/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp @@ -175,22 +175,65 @@ TEST(DockingServerTests, testErrorExceptions) EXPECT_TRUE(false); } } + node->on_deactivate(rclcpp_lifecycle::State()); + node->on_cleanup(rclcpp_lifecycle::State()); + node->on_shutdown(rclcpp_lifecycle::State()); + node.reset(); } TEST(DockingServerTests, getateGoalDock) { auto node = std::make_shared(); + + // Setup 1 instance of the test failure dock & its plugin instance + node->declare_parameter( + "docks", + rclcpp::ParameterValue(std::vector{"test_dock"})); + node->declare_parameter( + "test_dock.type", + rclcpp::ParameterValue(std::string{"dock_plugin"})); + node->declare_parameter( + "test_dock.pose", + rclcpp::ParameterValue(std::vector{0.0, 0.0, 0.0})); + node->declare_parameter( + "dock_plugins", + rclcpp::ParameterValue(std::vector{"dock_plugin"})); + node->declare_parameter( + "dock_plugin.plugin", + rclcpp::ParameterValue(std::string{"opennav_docking::TestFailureDock"})); + node->on_configure(rclcpp_lifecycle::State()); std::shared_ptr goal = std::make_shared(); auto dock = node->generateGoalDock(goal); - EXPECT_EQ(dock->plugin, nullptr); + EXPECT_NE(dock->plugin, nullptr); EXPECT_EQ(dock->frame, std::string()); node->stashDockData(false, dock, true); + node->on_cleanup(rclcpp_lifecycle::State()); + node->on_shutdown(rclcpp_lifecycle::State()); + node.reset(); } TEST(DockingServerTests, testDynamicParams) { auto node = std::make_shared(); + + // Setup 1 instance of the test failure dock & its plugin instance + node->declare_parameter( + "docks", + rclcpp::ParameterValue(std::vector{"test_dock"})); + node->declare_parameter( + "test_dock.type", + rclcpp::ParameterValue(std::string{"dock_plugin"})); + node->declare_parameter( + "test_dock.pose", + rclcpp::ParameterValue(std::vector{0.0, 0.0, 0.0})); + node->declare_parameter( + "dock_plugins", + rclcpp::ParameterValue(std::vector{"dock_plugin"})); + node->declare_parameter( + "dock_plugin.plugin", + rclcpp::ParameterValue(std::string{"opennav_docking::TestFailureDock"})); + node->on_configure(rclcpp_lifecycle::State()); node->on_activate(rclcpp_lifecycle::State()); @@ -221,6 +264,11 @@ TEST(DockingServerTests, testDynamicParams) EXPECT_EQ(node->get_parameter("base_frame").as_string(), std::string("hi")); EXPECT_EQ(node->get_parameter("fixed_frame").as_string(), std::string("hi")); EXPECT_EQ(node->get_parameter("max_retries").as_int(), 7); + + node->on_deactivate(rclcpp_lifecycle::State()); + node->on_cleanup(rclcpp_lifecycle::State()); + node->on_shutdown(rclcpp_lifecycle::State()); + node.reset(); } } // namespace opennav_docking diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index bb59b0b0235..7233a7ba53d 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -79,7 +79,7 @@ PlannerServer::~PlannerServer() } nav2_util::CallbackReturn -PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +PlannerServer::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring"); @@ -120,6 +120,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_FATAL( get_logger(), "Failed to create global planner. Exception: %s", ex.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index 5947f5e36e4..5b2bf306df0 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -253,7 +253,10 @@ geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathPt() } } - return current_path_.poses.back(); + auto goal = current_path_.poses.back(); + goal.header.frame_id = current_path_.header.frame_id; + goal.header.stamp = clock_->now(); + return goal; } geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathGoal() diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp index 868c644e4b1..f73f21952be 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp @@ -23,6 +23,7 @@ #include // ROS +#include "nav2_lifecycle_manager/lifecycle_manager_client.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "rviz_common/panel.hpp" @@ -37,6 +38,9 @@ class QPushButton; namespace nav2_rviz_plugins { +class InitialDockThread; + +/// Panel to interface to the docking server class DockingPanel : public rviz_common::Panel { Q_OBJECT @@ -44,11 +48,20 @@ class DockingPanel : public rviz_common::Panel public: explicit DockingPanel(QWidget * parent = 0); virtual ~DockingPanel(); + void onInitialize() override; + /// Load and save configuration data + void load(const rviz_common::Config & config) override; + void save(rviz_common::Config config) const override; + private Q_SLOTS: + void startThread(); + void onStartup(); void onDockingButtonPressed(); void onUndockingButtonPressed(); + void onCancelDocking(); + void onCancelUndocking(); void dockIdCheckbox(); private: @@ -57,14 +70,6 @@ private Q_SLOTS: using DockGoalHandle = rclcpp_action::ClientGoalHandle; using UndockGoalHandle = rclcpp_action::ClientGoalHandle; - // Start the actions - void startDocking(); - void startUndocking(); - - // Cancel the actions - void cancelDocking(); - void cancelUndocking(); - // The (non-spinning) client node used to invoke the action client void timerEvent(QTimerEvent * event) override; @@ -84,14 +89,33 @@ private Q_SLOTS: // The (non-spinning) client node used to invoke the action client rclcpp::Node::SharedPtr client_node_; + + // The Node pointer that we need to keep alive for the duration of this plugin. + std::shared_ptr node_ptr_; + // Timeout value when waiting for action servers to respond std::chrono::milliseconds server_timeout_; + // A timer used to check on the completion status of the action + QBasicTimer action_timer_; + + // The Dock and Undock action client + rclcpp_action::Client::SharedPtr dock_client_; + rclcpp_action::Client::SharedPtr undock_client_; + + // Docking / Undocking action feedback subscribers + rclcpp::Subscription::SharedPtr docking_feedback_sub_; + rclcpp::Subscription::SharedPtr undocking_feedback_sub_; + rclcpp::Subscription::SharedPtr docking_goal_status_sub_; + rclcpp::Subscription::SharedPtr undocking_goal_status_sub_; + + // Goal related state + DockGoalHandle::SharedPtr dock_goal_handle_; + UndockGoalHandle::SharedPtr undock_goal_handle_; + // Flags to indicate if the plugins have been loaded bool plugins_loaded_ = false; bool server_failed_ = false; - bool tried_once_ = false; - QBasicTimer timer_; QVBoxLayout * main_layout_{nullptr}; QHBoxLayout * info_layout_{nullptr}; @@ -121,20 +145,62 @@ private Q_SLOTS: bool undocking_in_progress_ = false; bool use_dock_id_ = false; - // The Dock and Undock action client - rclcpp_action::Client::SharedPtr dock_client_; - rclcpp_action::Client::SharedPtr undock_client_; - DockGoalHandle::SharedPtr dock_goal_handle_; - UndockGoalHandle::SharedPtr undock_goal_handle_; + QStateMachine state_machine_; + InitialDockThread * initial_thread_; - // The Node pointer that we need to keep alive for the duration of this plugin. - std::shared_ptr node_ptr_; + QState * pre_initial_{nullptr}; + QState * idle_{nullptr}; + QState * docking_{nullptr}; + QState * undocking_{nullptr}; + QState * canceled_docking_{nullptr}; + QState * canceled_undocking_{nullptr}; +}; - // Docking / Undocking action feedback subscribers - rclcpp::Subscription::SharedPtr docking_feedback_sub_; - rclcpp::Subscription::SharedPtr undocking_feedback_sub_; - rclcpp::Subscription::SharedPtr docking_goal_status_sub_; - rclcpp::Subscription::SharedPtr undocking_goal_status_sub_; +class InitialDockThread : public QThread +{ + Q_OBJECT + +public: + explicit InitialDockThread( + rclcpp_action::Client::SharedPtr & dock_client, + rclcpp_action::Client::SharedPtr & undock_client) + : dock_client_(dock_client), undock_client_(undock_client) + {} + + void run() override + { + while (!dock_active_) { + dock_active_ = dock_client_->wait_for_action_server(std::chrono::seconds(1)); + } + + while (!undock_active_) { + undock_active_ = undock_client_->wait_for_action_server(std::chrono::seconds(1)); + } + + if (dock_active_) { + emit dockingActive(); + } else { + emit dockingInactive(); + } + + if (undock_active_) { + emit undockingActive(); + } else { + emit undockingInactive(); + } + } + +signals: + void dockingActive(); + void dockingInactive(); + void undockingActive(); + void undockingInactive(); + +private: + rclcpp_action::Client::SharedPtr dock_client_; + rclcpp_action::Client::SharedPtr undock_client_; + bool dock_active_ = false; + bool undock_active_ = false; }; } // namespace nav2_rviz_plugins diff --git a/nav2_rviz_plugins/src/docking_panel.cpp b/nav2_rviz_plugins/src/docking_panel.cpp index 7daa327079c..e43f718ebee 100644 --- a/nav2_rviz_plugins/src/docking_panel.cpp +++ b/nav2_rviz_plugins/src/docking_panel.cpp @@ -29,6 +29,7 @@ #include "nav2_util/geometry_utils.hpp" #include "nav2_rviz_plugins/docking_panel.hpp" +#include "nav2_rviz_plugins/ros_action_qevent.hpp" #include "nav2_rviz_plugins/utils.hpp" using namespace std::chrono_literals; @@ -40,8 +41,7 @@ DockingPanel::DockingPanel(QWidget * parent) : Panel(parent), server_timeout_(100) { - client_node_ = std::make_shared("nav2_rviz_docking_panel_node"); - + // Create the control buttons and its tooltip main_layout_ = new QVBoxLayout; info_layout_ = new QHBoxLayout; feedback_layout_ = new QVBoxLayout; @@ -50,8 +50,8 @@ DockingPanel::DockingPanel(QWidget * parent) dock_pose_layout_ = new QHBoxLayout; nav_stage_layout_ = new QHBoxLayout; dock_type_ = new QComboBox; - docking_button_ = new QPushButton("Dock robot"); - undocking_button_ = new QPushButton("Undock robot"); + docking_button_ = new QPushButton; + undocking_button_ = new QPushButton; docking_goal_status_indicator_ = new QLabel; docking_feedback_indicator_ = new QLabel; docking_result_indicator_ = new QLabel; @@ -62,27 +62,147 @@ DockingPanel::DockingPanel(QWidget * parent) dock_pose_y_ = new QLineEdit; dock_pose_yaw_ = new QLineEdit; - docking_button_->setEnabled(false); - undocking_button_->setEnabled(false); - use_dock_id_checkbox_->setEnabled(false); - nav_to_staging_checkbox_->setEnabled(false); - dock_id_->setEnabled(false); - dock_pose_x_->setEnabled(false); - dock_pose_y_->setEnabled(false); - dock_pose_yaw_->setEnabled(false); + // Create the state machine used to present the proper control button states in the UI + const char * nav_to_stage_msg = "Navigate to the staging pose before docking"; + const char * use_dock_id_msg = "Use the dock id or the dock pose to dock the robot"; + const char * dock_msg = "Dock the robot at the specified docking station"; + const char * undock_msg = "Undock the robot from the docking station"; + const char * cancel_dock_msg = "Cancel the current docking action"; + const char * cancel_undock_msg = "Cancel the current undocking action"; + docking_goal_status_indicator_->setText(nav2_rviz_plugins::getGoalStatusLabel()); docking_feedback_indicator_->setText(getDockFeedbackLabel()); docking_goal_status_indicator_->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); docking_feedback_indicator_->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); - nav_to_staging_checkbox_->setFixedWidth(150); + pre_initial_ = new QState(); + pre_initial_->setObjectName("pre_initial"); + pre_initial_->assignProperty(docking_button_, "text", "Dock robot"); + pre_initial_->assignProperty(docking_button_, "enabled", false); + + pre_initial_->assignProperty(undocking_button_, "text", "Undock robot"); + pre_initial_->assignProperty(undocking_button_, "enabled", false); + + pre_initial_->assignProperty(nav_to_staging_checkbox_, "enabled", false); + pre_initial_->assignProperty(nav_to_staging_checkbox_, "checked", true); + pre_initial_->assignProperty(use_dock_id_checkbox_, "enabled", false); + pre_initial_->assignProperty(use_dock_id_checkbox_, "checked", true); + pre_initial_->assignProperty(dock_id_, "enabled", false); + pre_initial_->assignProperty(dock_type_, "enabled", false); + pre_initial_->assignProperty(dock_pose_x_, "enabled", false); + pre_initial_->assignProperty(dock_pose_y_, "enabled", false); + pre_initial_->assignProperty(dock_pose_yaw_, "enabled", false); + + // State entered when the docking / undocking action is not active + idle_ = new QState(); + idle_->setObjectName("idle"); + idle_->assignProperty(docking_button_, "text", "Dock robot"); + idle_->assignProperty(docking_button_, "toolTip", dock_msg); + idle_->assignProperty(docking_button_, "enabled", true); + + idle_->assignProperty(undocking_button_, "text", "Undock robot"); + idle_->assignProperty(undocking_button_, "toolTip", undock_msg); + idle_->assignProperty(undocking_button_, "enabled", true); + + idle_->assignProperty(nav_to_staging_checkbox_, "enabled", true); + idle_->assignProperty(nav_to_staging_checkbox_, "toolTip", nav_to_stage_msg); + idle_->assignProperty(use_dock_id_checkbox_, "enabled", true); + idle_->assignProperty(use_dock_id_checkbox_, "toolTip", use_dock_id_msg); + idle_->assignProperty(dock_id_, "enabled", true); + idle_->assignProperty(dock_type_, "enabled", true); + + // State entered to cancel the docking action + canceled_docking_ = new QState(); + canceled_docking_->setObjectName("canceled_docking"); + + // State entered to cancel the undocking action + canceled_undocking_ = new QState(); + canceled_undocking_->setObjectName("canceled_undocking"); + + // State entered while the docking action is active + docking_ = new QState(); + docking_->setObjectName("docking"); + docking_->assignProperty(docking_button_, "text", "Cancel docking"); + docking_->assignProperty(docking_button_, "toolTip", cancel_dock_msg); + + docking_->assignProperty(undocking_button_, "enabled", false); + + // State entered while the undocking action is active + undocking_ = new QState(); + undocking_->setObjectName("undocking"); + undocking_->assignProperty(docking_button_, "enabled", false); + + undocking_->assignProperty(undocking_button_, "text", "Cancel undocking"); + undocking_->assignProperty(undocking_button_, "toolTip", cancel_undock_msg); + + QObject::connect(docking_, SIGNAL(entered()), this, SLOT(onDockingButtonPressed())); + QObject::connect(undocking_, SIGNAL(entered()), this, SLOT(onUndockingButtonPressed())); + QObject::connect(canceled_docking_, SIGNAL(exited()), this, SLOT(onCancelDocking())); + QObject::connect(canceled_undocking_, SIGNAL(exited()), this, SLOT(onCancelUndocking())); + + // Start/Cancel button click transitions + idle_->addTransition(docking_button_, SIGNAL(clicked()), docking_); + idle_->addTransition(undocking_button_, SIGNAL(clicked()), undocking_); + docking_->addTransition(docking_button_, SIGNAL(clicked()), canceled_docking_); + undocking_->addTransition(undocking_button_, SIGNAL(clicked()), canceled_undocking_); + + // Internal state transitions + canceled_docking_->addTransition(canceled_docking_, SIGNAL(entered()), idle_); + canceled_undocking_->addTransition(canceled_undocking_, SIGNAL(entered()), idle_); + + // ROSAction Transitions: So when actions are updated remotely (failing, succeeding, etc) + // the state of the application will also update. This means that if in the processing + // states and then goes inactive, move back to the idle state. Vise versa as well. + ROSActionQTransition * idleDockTransition = new ROSActionQTransition(QActionState::INACTIVE); + idleDockTransition->setTargetState(docking_); + idle_->addTransition(idleDockTransition); + + ROSActionQTransition * idleUndockTransition = new ROSActionQTransition(QActionState::INACTIVE); + idleUndockTransition->setTargetState(undocking_); + idle_->addTransition(idleUndockTransition); + + ROSActionQTransition * dockingTransition = new ROSActionQTransition(QActionState::ACTIVE); + dockingTransition->setTargetState(idle_); + docking_->addTransition(dockingTransition); + + ROSActionQTransition * undockingTransition = new ROSActionQTransition(QActionState::ACTIVE); + undockingTransition->setTargetState(idle_); + undocking_->addTransition(undockingTransition); + + client_node_ = std::make_shared("nav2_rviz_docking_panel_node"); + + state_machine_.addState(pre_initial_); + state_machine_.addState(idle_); + state_machine_.addState(docking_); + state_machine_.addState(undocking_); + state_machine_.addState(canceled_docking_); + state_machine_.addState(canceled_undocking_); + + state_machine_.setInitialState(pre_initial_); + + // Delay starting initial thread until state machine has started or a race occurs + QObject::connect(&state_machine_, SIGNAL(started()), this, SLOT(startThread())); + state_machine_.start(); + + // Create the layout for the panel info_layout_->addWidget(docking_goal_status_indicator_); info_layout_->addWidget(docking_result_indicator_); feedback_layout_->addWidget(docking_feedback_indicator_); - dock_id_layout_->addWidget(new QLabel("Dock id")); + + QLabel * nav_stage_label = new QLabel("Nav. to staging pose"); + QLabel * dock_id_label = new QLabel("Dock id"); + QLabel * dock_type_label = new QLabel("Dock type"); + + nav_stage_label->setFixedWidth(150); + dock_id_label->setFixedWidth(150); + dock_type_label->setFixedWidth(170); + + nav_stage_layout_->addWidget(nav_stage_label); + nav_stage_layout_->addWidget(nav_to_staging_checkbox_); + dock_id_layout_->addWidget(dock_id_label); dock_id_layout_->addWidget(use_dock_id_checkbox_); dock_id_layout_->addWidget(dock_id_); - dock_type_layout_->addWidget(new QLabel("Dock type")); + dock_type_layout_->addWidget(dock_type_label); dock_type_layout_->addWidget(dock_type_); dock_pose_layout_->addWidget(new QLabel("Dock pose {X")); dock_pose_layout_->addWidget(dock_pose_x_); @@ -91,28 +211,53 @@ DockingPanel::DockingPanel(QWidget * parent) dock_pose_layout_->addWidget(new QLabel("θ")); dock_pose_layout_->addWidget(dock_pose_yaw_); dock_pose_layout_->addWidget(new QLabel("}")); - nav_stage_layout_->addWidget(nav_to_staging_checkbox_); - nav_stage_layout_->addWidget(new QLabel("Navigate to staging pose")); + + QGroupBox * group_box = new QGroupBox(); + QVBoxLayout * group_box_layout = new QVBoxLayout; + group_box_layout->addLayout(nav_stage_layout_); + group_box_layout->addLayout(dock_id_layout_); + group_box_layout->addLayout(dock_type_layout_); + group_box_layout->addLayout(dock_pose_layout_); + group_box->setLayout(group_box_layout); main_layout_->setContentsMargins(10, 10, 10, 10); main_layout_->addLayout(info_layout_); main_layout_->addLayout(feedback_layout_); - main_layout_->addLayout(nav_stage_layout_); - main_layout_->addLayout(dock_id_layout_); - main_layout_->addLayout(dock_type_layout_); - main_layout_->addLayout(dock_pose_layout_); + main_layout_->addWidget(group_box); main_layout_->addWidget(docking_button_); main_layout_->addWidget(undocking_button_); setLayout(main_layout_); - timer_.start(200, this); + action_timer_.start(200, this); dock_client_ = rclcpp_action::create_client(client_node_, "dock_robot"); undock_client_ = rclcpp_action::create_client(client_node_, "undock_robot"); + initial_thread_ = new InitialDockThread(dock_client_, undock_client_); + connect(initial_thread_, &InitialDockThread::finished, initial_thread_, &QObject::deleteLater); + + QSignalTransition * activeDockSignal = new QSignalTransition( + initial_thread_, &InitialDockThread::dockingActive); + activeDockSignal->setTargetState(idle_); + pre_initial_->addTransition(activeDockSignal); + + QSignalTransition * activeUndockSignal = new QSignalTransition( + initial_thread_, &InitialDockThread::undockingActive); + activeUndockSignal->setTargetState(idle_); + pre_initial_->addTransition(activeUndockSignal); + + QObject::connect( + initial_thread_, &InitialDockThread::dockingActive, + [this] { + // Load the plugins if not already loaded + if (!plugins_loaded_) { + RCLCPP_INFO(client_node_->get_logger(), "Loading dock plugins"); + nav2_rviz_plugins::pluginLoader( + client_node_, server_failed_, "docking_server", "dock_plugins", dock_type_); + plugins_loaded_ = true; + } + }); // Conect buttons with functions - QObject::connect(docking_button_, SIGNAL(clicked()), this, SLOT(onDockingButtonPressed())); - QObject::connect(undocking_button_, SIGNAL(clicked()), this, SLOT(onUndockingButtonPressed())); QObject::connect( use_dock_id_checkbox_, &QCheckBox::stateChanged, this, &DockingPanel::dockIdCheckbox); } @@ -135,18 +280,6 @@ void DockingPanel::onInitialize() rclcpp::SystemDefaultsQoS(), [this](const Dock::Impl::FeedbackMessage::SharedPtr msg) { docking_feedback_indicator_->setText(getDockFeedbackLabel(msg->feedback)); - docking_button_->setText("Cancel docking"); - undocking_button_->setEnabled(false); - docking_in_progress_ = true; - }); - - undocking_feedback_sub_ = node->create_subscription( - "undock_robot/_action/feedback", - rclcpp::SystemDefaultsQoS(), - [this](const Undock::Impl::FeedbackMessage::SharedPtr /*msg*/) { - docking_button_->setEnabled(false); - undocking_button_->setText("Cancel undocking"); - undocking_in_progress_ = true; }); // Create action goal status subscribers @@ -156,11 +289,6 @@ void DockingPanel::onInitialize() [this](const action_msgs::msg::GoalStatusArray::SharedPtr msg) { docking_goal_status_indicator_->setText( nav2_rviz_plugins::getGoalStatusLabel("Feedback", msg->status_list.back().status)); - if (msg->status_list.back().status != action_msgs::msg::GoalStatus::STATUS_EXECUTING) { - docking_button_->setText("Dock robot"); - undocking_button_->setEnabled(true); - docking_in_progress_ = false; - } // Reset values when action is completed if (msg->status_list.back().status == action_msgs::msg::GoalStatus::STATUS_SUCCEEDED) { docking_feedback_indicator_->setText(getDockFeedbackLabel()); @@ -173,37 +301,30 @@ void DockingPanel::onInitialize() [this](const action_msgs::msg::GoalStatusArray::SharedPtr msg) { docking_goal_status_indicator_->setText( nav2_rviz_plugins::getGoalStatusLabel("Feedback", msg->status_list.back().status)); - if (msg->status_list.back().status != action_msgs::msg::GoalStatus::STATUS_EXECUTING) { - docking_button_->setEnabled(true); - undocking_button_->setText("Undock robot"); - undocking_in_progress_ = false; - } }); } +void DockingPanel::startThread() +{ + // start initial thread now that state machine is started + initial_thread_->start(); +} + DockingPanel::~DockingPanel() { } -void DockingPanel::onDockingButtonPressed() +void DockingPanel::load(const rviz_common::Config & config) { - if (!docking_in_progress_) { - startDocking(); - } else { - cancelDocking(); - } + Panel::load(config); } -void DockingPanel::onUndockingButtonPressed() +void DockingPanel::save(rviz_common::Config config) const { - if (!undocking_in_progress_) { - startUndocking(); - } else { - cancelUndocking(); - } + Panel::save(config); } -void DockingPanel::startDocking() +void DockingPanel::onDockingButtonPressed() { auto is_action_server_ready = dock_client_->wait_for_action_server(std::chrono::seconds(5)); @@ -286,10 +407,10 @@ void DockingPanel::startDocking() return; } - timer_.start(200, this); + action_timer_.start(200, this); } -void DockingPanel::startUndocking() +void DockingPanel::onUndockingButtonPressed() { auto is_action_server_ready = undock_client_->wait_for_action_server(std::chrono::seconds(5)); @@ -344,7 +465,7 @@ void DockingPanel::startUndocking() return; } - timer_.start(200, this); + action_timer_.start(200, this); } void DockingPanel::dockIdCheckbox() @@ -364,7 +485,7 @@ void DockingPanel::dockIdCheckbox() } } -void DockingPanel::cancelDocking() +void DockingPanel::onCancelDocking() { if (dock_goal_handle_) { auto future_cancel = dock_client_->async_cancel_goal(dock_goal_handle_); @@ -377,9 +498,11 @@ void DockingPanel::cancelDocking() dock_goal_handle_.reset(); } } + + action_timer_.stop(); } -void DockingPanel::cancelUndocking() +void DockingPanel::onCancelUndocking() { if (undock_goal_handle_) { auto future_cancel = undock_client_->async_cancel_goal(undock_goal_handle_); @@ -392,35 +515,53 @@ void DockingPanel::cancelUndocking() undock_goal_handle_.reset(); } } + + action_timer_.stop(); } void DockingPanel::timerEvent(QTimerEvent * event) { - if (event->timerId() == timer_.timerId()) { - if (!plugins_loaded_) { - nav2_rviz_plugins::pluginLoader( - client_node_, server_failed_, "docking_server", "dock_plugins", dock_type_); - plugins_loaded_ = true; - docking_button_->setEnabled(true); - undocking_button_->setEnabled(true); - use_dock_id_checkbox_->setEnabled(true); - use_dock_id_checkbox_->setChecked(true); - nav_to_staging_checkbox_->setEnabled(true); - nav_to_staging_checkbox_->setChecked(true); - dock_id_->setEnabled(true); - } + if (event->timerId() == action_timer_.timerId()) { + // Check the status of the action servers + if (state_machine_.configuration().contains(docking_)) { + if (!dock_goal_handle_) { + RCLCPP_DEBUG(client_node_->get_logger(), "Waiting for Goal"); + state_machine_.postEvent(new ROSActionQEvent(QActionState::INACTIVE)); + return; + } - // Restart the timer if the one of the server fails - if (server_failed_ && !tried_once_) { - RCLCPP_INFO(client_node_->get_logger(), "Retrying to connect to the failed server."); - server_failed_ = false; - plugins_loaded_ = false; - tried_once_ = true; - timer_.start(200, this); - return; - } + rclcpp::spin_some(client_node_); + auto status = dock_goal_handle_->get_status(); - timer_.stop(); + // Check if the goal is still executing + if (status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED || + status == action_msgs::msg::GoalStatus::STATUS_EXECUTING) + { + state_machine_.postEvent(new ROSActionQEvent(QActionState::ACTIVE)); + } else { + state_machine_.postEvent(new ROSActionQEvent(QActionState::INACTIVE)); + action_timer_.stop(); + } + } else if (state_machine_.configuration().contains(undocking_)) { + if (!undock_goal_handle_) { + RCLCPP_DEBUG(client_node_->get_logger(), "Waiting for Goal"); + state_machine_.postEvent(new ROSActionQEvent(QActionState::INACTIVE)); + return; + } + + rclcpp::spin_some(client_node_); + auto status = undock_goal_handle_->get_status(); + + // Check if the goal is still executing + if (status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED || + status == action_msgs::msg::GoalStatus::STATUS_EXECUTING) + { + state_machine_.postEvent(new ROSActionQEvent(QActionState::ACTIVE)); + } else { + state_machine_.postEvent(new ROSActionQEvent(QActionState::INACTIVE)); + action_timer_.stop(); + } + } } } diff --git a/nav2_rviz_plugins/src/utils.cpp b/nav2_rviz_plugins/src/utils.cpp index 6c7fcbbad03..96cf7784bf1 100644 --- a/nav2_rviz_plugins/src/utils.cpp +++ b/nav2_rviz_plugins/src/utils.cpp @@ -38,9 +38,7 @@ void pluginLoader( RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for the service. Exiting."); rclcpp::shutdown(); } - RCLCPP_INFO( - node->get_logger(), - "%s service not available", server_name.c_str()); + RCLCPP_INFO(node->get_logger(), "%s service not available", server_name.c_str()); server_unavailable = true; server_failed = true; break; diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index e7baa5e4839..fe462cb1c6b 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -63,7 +63,7 @@ SmootherServer::~SmootherServer() } nav2_util::CallbackReturn -SmootherServer::on_configure(const rclcpp_lifecycle::State &) +SmootherServer::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring smoother server"); @@ -100,6 +100,7 @@ SmootherServer::on_configure(const rclcpp_lifecycle::State &) *costmap_sub_, *footprint_sub_, this->get_name()); if (!loadSmootherPlugins()) { + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } @@ -162,7 +163,7 @@ bool SmootherServer::loadSmootherPlugins() } nav2_util::CallbackReturn -SmootherServer::on_activate(const rclcpp_lifecycle::State &) +SmootherServer::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index 535b6b08f21..9519ab94b61 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -44,7 +44,7 @@ VelocitySmoother::~VelocitySmoother() } nav2_util::CallbackReturn -VelocitySmoother::on_configure(const rclcpp_lifecycle::State &) +VelocitySmoother::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring velocity smoother"); auto node = shared_from_this(); @@ -76,24 +76,35 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &) for (unsigned int i = 0; i != 3; i++) { if (max_decels_[i] > 0.0) { - throw std::runtime_error( - "Positive values set of deceleration! These should be negative to slow down!"); + RCLCPP_ERROR( + get_logger(), + "Positive values set of deceleration! These should be negative to slow down!"); + on_cleanup(state); + return nav2_util::CallbackReturn::FAILURE; } if (max_accels_[i] < 0.0) { - throw std::runtime_error( - "Negative values set of acceleration! These should be positive to speed up!"); + RCLCPP_ERROR( + get_logger(), + "Negative values set of acceleration! These should be positive to speed up!"); + on_cleanup(state); + return nav2_util::CallbackReturn::FAILURE; } if (min_velocities_[i] > 0.0) { - throw std::runtime_error( - "Positive values set of min_velocities! These should be negative!"); + RCLCPP_ERROR( + get_logger(), "Positive values set of min_velocities! These should be negative!"); + on_cleanup(state); + return nav2_util::CallbackReturn::FAILURE; } if (max_velocities_[i] < 0.0) { - throw std::runtime_error( - "Negative values set of max_velocities! These should be positive!"); + RCLCPP_ERROR( + get_logger(), "Negative values set of max_velocities! These should be positive!"); + on_cleanup(state); + return nav2_util::CallbackReturn::FAILURE; } if (min_velocities_[i] > max_velocities_[i]) { - throw std::runtime_error( - "Min velocities are higher than max velocities!"); + RCLCPP_ERROR(get_logger(), "Min velocities are higher than max velocities!"); + on_cleanup(state); + return nav2_util::CallbackReturn::FAILURE; } } @@ -112,9 +123,12 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &) if (max_velocities_.size() != 3 || min_velocities_.size() != 3 || max_accels_.size() != 3 || max_decels_.size() != 3 || deadband_velocities_.size() != 3) { - throw std::runtime_error( - "Invalid setting of kinematic and/or deadband limits!" - " All limits must be size of 3 representing (x, y, theta)."); + RCLCPP_ERROR( + get_logger(), + "Invalid setting of kinematic and/or deadband limits!" + " All limits must be size of 3 representing (x, y, theta)."); + on_cleanup(state); + return nav2_util::CallbackReturn::FAILURE; } // Get control type @@ -124,7 +138,11 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &) open_loop_ = false; odom_smoother_ = std::make_unique(node, odom_duration_, odom_topic_); } else { - throw std::runtime_error("Invalid feedback_type, options are OPEN_LOOP and CLOSED_LOOP."); + RCLCPP_ERROR( + get_logger(), + "Invalid feedback_type, options are OPEN_LOOP and CLOSED_LOOP."); + on_cleanup(state); + return nav2_util::CallbackReturn::FAILURE; } // Setup inputs / outputs @@ -144,6 +162,7 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &) nav2_util::setSoftRealTimePriority(); } catch (const std::runtime_error & e) { RCLCPP_ERROR(get_logger(), "%s", e.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } diff --git a/nav2_velocity_smoother/test/test_velocity_smoother.cpp b/nav2_velocity_smoother/test/test_velocity_smoother.cpp index 25754d0f355..76c5e0498b6 100644 --- a/nav2_velocity_smoother/test/test_velocity_smoother.cpp +++ b/nav2_velocity_smoother/test/test_velocity_smoother.cpp @@ -40,7 +40,10 @@ class VelSmootherShim : public nav2_velocity_smoother::VelocitySmoother public: VelSmootherShim() : VelocitySmoother() {} - void configure(const rclcpp_lifecycle::State & state) {this->on_configure(state);} + nav2_util::CallbackReturn configure(const rclcpp_lifecycle::State & state) + { + return this->on_configure(state); + } void activate(const rclcpp_lifecycle::State & state) {this->on_activate(state);} void deactivate(const rclcpp_lifecycle::State & state) {this->on_deactivate(state);} void cleanup(const rclcpp_lifecycle::State & state) {this->on_cleanup(state);} @@ -594,10 +597,10 @@ TEST(VelocitySmootherTest, testInvalidParams) std::vector max_vels{0.0, 0.0}; // invalid size smoother->declare_parameter("max_velocity", rclcpp::ParameterValue(max_vels)); rclcpp_lifecycle::State state; - EXPECT_THROW(smoother->configure(state), std::runtime_error); + EXPECT_EQ(smoother->configure(state), nav2_util::CallbackReturn::FAILURE); smoother->set_parameter(rclcpp::Parameter("feedback", std::string("LAWLS"))); - EXPECT_THROW(smoother->configure(state), std::runtime_error); + EXPECT_EQ(smoother->configure(state), nav2_util::CallbackReturn::FAILURE); } TEST(VelocitySmootherTest, testInvalidParamsAccelDecel) @@ -613,13 +616,13 @@ TEST(VelocitySmootherTest, testInvalidParamsAccelDecel) smoother->declare_parameter("max_velocity", rclcpp::ParameterValue(bad_test_max_vel)); smoother->declare_parameter("min_velocity", rclcpp::ParameterValue(bad_test_min_vel)); rclcpp_lifecycle::State state; - EXPECT_THROW(smoother->configure(state), std::runtime_error); + EXPECT_EQ(smoother->configure(state), nav2_util::CallbackReturn::FAILURE); smoother->set_parameter(rclcpp::Parameter("max_accel", rclcpp::ParameterValue(bad_test_accel))); - EXPECT_THROW(smoother->configure(state), std::runtime_error); + EXPECT_EQ(smoother->configure(state), nav2_util::CallbackReturn::FAILURE); smoother->set_parameter(rclcpp::Parameter("max_decel", rclcpp::ParameterValue(bad_test_decel))); - EXPECT_THROW(smoother->configure(state), std::runtime_error); + EXPECT_EQ(smoother->configure(state), nav2_util::CallbackReturn::FAILURE); } TEST(VelocitySmootherTest, testDynamicParameter) diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 02a4158d280..0014e79a15d 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -54,7 +54,7 @@ WaypointFollower::~WaypointFollower() } nav2_util::CallbackReturn -WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) +WaypointFollower::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring"); @@ -123,6 +123,7 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_FATAL( get_logger(), "Failed to create waypoint_task_executor. Exception: %s", e.what()); + on_cleanup(state); } return nav2_util::CallbackReturn::SUCCESS;