diff --git a/rviz_mbf_plugins/include/rviz_mbf_plugins/mbf_goal_actions_panel.hpp b/rviz_mbf_plugins/include/rviz_mbf_plugins/mbf_goal_actions_panel.hpp index 8c925a93..24b3ce31 100644 --- a/rviz_mbf_plugins/include/rviz_mbf_plugins/mbf_goal_actions_panel.hpp +++ b/rviz_mbf_plugins/include/rviz_mbf_plugins/mbf_goal_actions_panel.hpp @@ -54,10 +54,13 @@ #include #include #include +#include #include #include #include +#include +#include namespace rviz_mbf_plugins { @@ -70,13 +73,13 @@ class MbfGoalActionsPanel : public rviz_common::Panel public: explicit MbfGoalActionsPanel(QWidget * parent = nullptr); - ~MbfGoalActionsPanel() noexcept override = default; + ~MbfGoalActionsPanel() noexcept override; void onInitialize() override; void save(rviz_common::Config config) const override; void load(const rviz_common::Config & config) override; - void newMeshGoalCallback(const geometry_msgs::msg::PoseStamped & msg); + void newGoalCallback(const geometry_msgs::msg::PoseStamped & msg); protected: //! Sets up the properties widget, which contains editable fields that configures the panel (e.g. which topic to subscribe to) @@ -98,23 +101,42 @@ private Q_SLOTS: void updateGoalInputSubscription(); void updateGetPathActionClient(); void updateExePathActionClient(); + void stopGetPathAction(); + void stopExePathAction(); protected: rclcpp::Subscription::SharedPtr goal_pose_subscription_; + rclcpp::Node::SharedPtr ros_node_; + //! Action client for getting a path + std::mutex get_path_action_client_mutex_; GetPathClient::SharedPtr action_client_get_path_; + std::shared_ptr planner_parameter_client_; + std::string get_path_node_name_; + std::string get_path_action_server_name_; + std::string get_path_planner_name_; + //! Goal handle of active get path action GetPathClient::GoalHandle::SharedPtr goal_handle_get_path_; - //! Potential next get path goal, used for quickly restarting the planner after cancelling the previous goal - std::optional next_get_path_goal_; //! Action client for traversing a path + std::mutex exe_path_action_client_mutex_; ExePathClient::SharedPtr action_client_exe_path_; + std::shared_ptr controller_parameter_client_; + std::string exe_path_node_name_; + std::string exe_path_action_server_name_; + std::string exe_path_controller_name_; + //! Goal handle of active exe path action ExePathClient::GoalHandle::SharedPtr goal_handle_exe_path_; - //! Potential next exe path goal, used for quickly restarting the path execution after cancelling the previous goal - std::optional next_exe_path_goal_; + + std::atomic_bool conn_check_thread_get_path_stop_; + std::thread conn_check_thread_get_path_; + std::atomic_bool conn_check_thread_exe_path_stop_; + std::thread conn_check_thread_exe_path_; + std::atomic_bool executor_thread_stop_; + std::thread executor_thread_; //! Retry counter for goal execution size_t goal_retry_cnt_; @@ -134,8 +156,6 @@ private Q_SLOTS: rviz_common::properties::RosActionProperty* exe_path_action_server_path_; rviz_common::properties::EditableEnumProperty* controller_name_property_; - std::shared_ptr planner_parameter_client_; - std::shared_ptr controller_parameter_client_; QGroupBox * goal_input_ui_box_; QVBoxLayout * goal_input_ui_layout_; @@ -149,6 +169,7 @@ private Q_SLOTS: QHBoxLayout * get_path_ui_layout_goal_status_; QLabel * get_path_action_goal_status_desc_; QLabel * get_path_action_goal_status_; + QPushButton * stop_get_path_button_; QGroupBox * exe_path_ui_box_; QVBoxLayout * exe_path_ui_layout_; @@ -158,6 +179,7 @@ private Q_SLOTS: QHBoxLayout * exe_path_ui_layout_goal_status_; QLabel * exe_path_action_goal_status_desc_; QLabel * exe_path_action_goal_status_; + QPushButton * stop_exe_path_button_; }; } // namespace rviz_mbf_plugins diff --git a/rviz_mbf_plugins/src/mbf_goal_actions_panel.cpp b/rviz_mbf_plugins/src/mbf_goal_actions_panel.cpp index eef61158..0931df79 100644 --- a/rviz_mbf_plugins/src/mbf_goal_actions_panel.cpp +++ b/rviz_mbf_plugins/src/mbf_goal_actions_panel.cpp @@ -46,10 +46,37 @@ #include +#include + namespace { -constexpr auto default_goal_input_topic = "pick topic"; -constexpr auto default_action_server_path = "input action server"; +constexpr auto notset_goal_input_topic = "pick topic"; +constexpr auto notset_action_server_path = "input action server"; + +// Color scheme for status messages +struct StatusStyle +{ + QString background; + QString text; +}; + +static const StatusStyle status_success = {"background-color: #90EE90;", "color: #1a1a1a;"}; // Light green, dark text +static const StatusStyle status_error = {"background-color: #FFB6C6;", "color: #ffffff;"}; // Light red, white text +static const StatusStyle status_warning = {"background-color: #FFE4B5;", "color: #1a1a1a;"}; // Light orange, dark text +static const StatusStyle status_info = {"background-color: #ADD8E6;", "color: #1a1a1a;"}; // Light blue, dark text +static const StatusStyle status_neutral = {"background-color: #D3D3D3;", "color: #1a1a1a;"}; // Light gray, dark text + +inline void setStatusLabel(QLabel* label, const QString& text, const StatusStyle& style) +{ + label->setText(text); + label->setStyleSheet(style.background + " " + style.text); +} + +inline void setStatusColor(QLabel* label, const StatusStyle& style) +{ + label->setStyleSheet(style.background + " " + style.text); +} + } // anonymous namespace namespace rviz_mbf_plugins @@ -73,13 +100,33 @@ MbfGoalActionsPanel::MbfGoalActionsPanel(QWidget * parent) setLayout(ui_layout_); } +MbfGoalActionsPanel::~MbfGoalActionsPanel() { + + // joining threads here + conn_check_thread_get_path_stop_ = true; + if(conn_check_thread_get_path_.joinable()) + { + conn_check_thread_get_path_.join(); + } + conn_check_thread_exe_path_stop_ = true; + if(conn_check_thread_exe_path_.joinable()) + { + conn_check_thread_exe_path_.join(); + } + executor_thread_stop_ = true; + if(executor_thread_.joinable()) + { + executor_thread_.join(); + } +} + void MbfGoalActionsPanel::constructPropertiesWidget() { properity_tree_model_ = new rviz_common::properties::PropertyTreeModel( new rviz_common::properties::Property()); goal_input_topic_ = new rviz_common::properties::RosTopicProperty( - "Goal Topic", default_goal_input_topic, + "Goal Topic", notset_goal_input_topic, QString::fromStdString(rosidl_generator_traits::name()), "Goal input topic to subscribe to", nullptr, @@ -87,7 +134,7 @@ void MbfGoalActionsPanel::constructPropertiesWidget() properity_tree_model_->getRoot()->addChild(goal_input_topic_); get_path_action_server_path_ = new rviz_common::properties::RosActionProperty( - "Planner Action", default_action_server_path, "mbf_msgs/action/GetPath", + "Planner Action", notset_action_server_path, "mbf_msgs/action/GetPath", "ROS path to move base flex get_path action server", nullptr, SLOT(updateGetPathActionClient()), this); @@ -98,7 +145,7 @@ void MbfGoalActionsPanel::constructPropertiesWidget() properity_tree_model_->getRoot()->addChild(planner_name_property_); exe_path_action_server_path_ = new rviz_common::properties::RosActionProperty( - "Controller Action", default_action_server_path, "mbf_msgs/action/ExePath", + "Controller Action", notset_action_server_path, "mbf_msgs/action/ExePath", "ROS path to move base flex exe_path action server", nullptr, SLOT(updateExePathActionClient()), this); @@ -136,9 +183,13 @@ void MbfGoalActionsPanel::constructGetPathWidget() get_path_ui_layout_goal_status_->addWidget(get_path_action_goal_status_desc_); get_path_ui_layout_goal_status_->addWidget(get_path_action_goal_status_); + stop_get_path_button_ = new QPushButton("Stop Planner Action"); + connect(stop_get_path_button_, &QPushButton::clicked, this, &MbfGoalActionsPanel::stopGetPathAction); + get_path_ui_layout_ = new QVBoxLayout(); get_path_ui_layout_->addLayout(get_path_ui_layout_server_status_); get_path_ui_layout_->addLayout(get_path_ui_layout_goal_status_); + get_path_ui_layout_->addWidget(stop_get_path_button_); get_path_ui_box_ = new QGroupBox("Get Path"); get_path_ui_box_->setLayout(get_path_ui_layout_); @@ -159,9 +210,13 @@ void MbfGoalActionsPanel::constructExePathWidget() exe_path_ui_layout_goal_status_->addWidget(exe_path_action_goal_status_desc_); exe_path_ui_layout_goal_status_->addWidget(exe_path_action_goal_status_); + stop_exe_path_button_ = new QPushButton("Stop Controller Action"); + connect(stop_exe_path_button_, &QPushButton::clicked, this, &MbfGoalActionsPanel::stopExePathAction); + exe_path_ui_layout_ = new QVBoxLayout(); exe_path_ui_layout_->addLayout(exe_path_ui_layout_server_status_); exe_path_ui_layout_->addLayout(exe_path_ui_layout_goal_status_); + exe_path_ui_layout_->addWidget(stop_exe_path_button_); exe_path_ui_box_ = new QGroupBox("Execute Path"); exe_path_ui_box_->setLayout(exe_path_ui_layout_); @@ -185,13 +240,11 @@ void MbfGoalActionsPanel::load(const rviz_common::Config & config) void MbfGoalActionsPanel::updateGoalInputSubscription() { - auto node = getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - const std::string selected_topic = goal_input_topic_->getTopic().toStdString(); - if (selected_topic != default_goal_input_topic) { - goal_pose_subscription_ = node->create_subscription( + if (selected_topic != notset_goal_input_topic) { + goal_pose_subscription_ = ros_node_->create_subscription( selected_topic, rclcpp::SystemDefaultsQoS(), - std::bind(&MbfGoalActionsPanel::newMeshGoalCallback, this, std::placeholders::_1)); + std::bind(&MbfGoalActionsPanel::newGoalCallback, this, std::placeholders::_1)); } } @@ -211,149 +264,597 @@ std::string node_name_guess(const std::string& topic) } } +template +bool sync_cancel_goal( + const typename ActionClientT::SharedPtr& action_client, + const typename ActionClientT::GoalHandle::SharedPtr& goal_handle, + std::chrono::duration timeout = std::chrono::seconds(5)) +{ + using CancelResponse = typename ActionClientT::CancelResponse; + std::atomic_bool cancel_process_finished = false; + std::atomic_bool cancel_successful = false; + action_client->async_cancel_goal(goal_handle, [&cancel_process_finished, &cancel_successful]( + const typename CancelResponse::SharedPtr & response) + { + cancel_successful = (response->return_code == CancelResponse::ERROR_NONE); + cancel_process_finished = true; + }); + // wait for cancel process to finish (with timeout) + auto start_time = std::chrono::steady_clock::now(); + while (!cancel_process_finished) { + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + if (std::chrono::steady_clock::now() - start_time > timeout) { + break; + } + } + + return cancel_successful; +} + void MbfGoalActionsPanel::updateGetPathActionClient() { - auto node = getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + // this function is changing the action client, so we lock the mutex for the whole function to avoid race conditions with the connection check thread + std::unique_lock lock(get_path_action_client_mutex_); const std::string selected_server = get_path_action_server_path_->getAction().toStdString(); - if (selected_server != default_action_server_path) { - action_client_get_path_ = rclcpp_action::create_client( - node, selected_server); + if(selected_server == notset_action_server_path) + { + // reset action client: first cancel active goal, then reset client + if (action_client_get_path_ && goal_handle_get_path_) { + + RCLCPP_INFO_STREAM(ros_node_->get_logger(), "Cancelling active get_path goal due to action server change..."); + setStatusLabel(get_path_action_goal_status_, "Cancelling active goal", status_warning); - if (action_client_get_path_->action_server_is_ready()) { - get_path_action_server_status_->setText("ready"); + action_client_get_path_->async_cancel_goal(goal_handle_get_path_, [this]( + const typename GetPathClient::CancelResponse::SharedPtr & response) + { + if(response->return_code == GetPathClient::CancelResponse::ERROR_NONE) + { + setStatusLabel(this->get_path_action_goal_status_, "Planner action stopped", status_success); + goal_handle_get_path_.reset(); + } else { + setStatusLabel(this->get_path_action_goal_status_, "Failed to stop planner action", status_error); + } + + // reset action client and planner parameter client + action_client_get_path_.reset(); + goal_handle_get_path_.reset(); + + planner_parameter_client_.reset(); + setStatusLabel(get_path_action_server_status_, "waiting for input", status_neutral); + setStatusLabel(get_path_action_goal_status_, "none sent yet", status_neutral); + }); + } else { - get_path_action_server_status_->setText("connecting"); + action_client_get_path_.reset(); + goal_handle_get_path_.reset(); + + planner_parameter_client_.reset(); + setStatusLabel(get_path_action_server_status_, "waiting for input", status_neutral); + setStatusLabel(get_path_action_goal_status_, "none sent yet", status_neutral); + } + + return; + } + + if(action_client_get_path_) + { + // was already initialized + if(selected_server == get_path_action_server_name_) + { + // same server than before: check if connection has been lost + if(!action_client_get_path_->action_server_is_ready()) + { + RCLCPP_WARN_STREAM(ros_node_->get_logger(), "Connection to get_path action server lost. Resetting action client..."); + setStatusLabel(get_path_action_server_status_, "connection lost", status_error); + action_client_get_path_.reset(); // reset so that reinitialization is triggered + goal_handle_get_path_.reset(); + planner_parameter_client_.reset(); + setStatusLabel(get_path_action_server_status_, "waiting for input", status_neutral); + setStatusLabel(get_path_action_goal_status_, "none sent yet", status_neutral); + } + } else { + // different server than before + + // reset action client: first cancel active goal, then reset client + if (goal_handle_get_path_) { + + RCLCPP_INFO_STREAM(ros_node_->get_logger(), "Cancelling active get_path goal due to action server change..."); + setStatusLabel(get_path_action_goal_status_, "Cancelling active goal", status_warning); + + action_client_get_path_->async_cancel_goal(goal_handle_get_path_, [this]( + const typename GetPathClient::CancelResponse::SharedPtr & response) + { + if(response->return_code == GetPathClient::CancelResponse::ERROR_NONE) + { + setStatusLabel(this->get_path_action_goal_status_, "Planner action stopped", status_success); + // next_get_path_goal_.reset(); + goal_handle_get_path_.reset(); + } else { + setStatusLabel(this->get_path_action_goal_status_, "Failed to stop planner action", status_error); + } + }); + } + + while(goal_handle_get_path_) + { + RCLCPP_INFO_STREAM(ros_node_->get_logger(), "Waiting for active get_path goal to be cancelled..."); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + + // reset action client and planner parameter client + action_client_get_path_.reset(); + goal_handle_get_path_.reset(); + + planner_parameter_client_.reset(); + setStatusLabel(get_path_action_server_status_, "waiting for input", status_neutral); + setStatusLabel(get_path_action_goal_status_, "none sent yet", status_neutral); + } + } + + // (re-)initialize action client if not initialized or resetted due to server change + if(!action_client_get_path_) + { + try { + action_client_get_path_ = rclcpp_action::create_client( + ros_node_, selected_server); + goal_handle_get_path_.reset(); + } catch (const std::exception& e) { + RCLCPP_ERROR_STREAM(ros_node_->get_logger(), "Failed to create action client for get_path action server at " << selected_server << ": " << e.what()); + setStatusLabel(get_path_action_server_status_, "connection failed!", status_error); + return; } + planner_parameter_client_.reset(); // force the following steps to reinitialize the parameter client } - // fetch planners via parameters: - // string list: [node_name_of_action_server].planners - planner_name_property_->clearOptions(); + // connecting + setStatusLabel(get_path_action_server_status_, "connecting...", status_info); + action_client_get_path_->wait_for_action_server( + std::chrono::seconds(1)); - // TODO(amock): this is a bit hacky. We extract the node name from the action server topic - // - but if the topic was remapped we get a problem. Currently I dont know an efficient (!) solution to that problem - std::string node_name = node_name_guess(selected_server); + if(!action_client_get_path_->action_server_is_ready()) + { + setStatusLabel(get_path_action_server_status_, "connection failed!", status_error); + RCLCPP_ERROR_STREAM(ros_node_->get_logger(), "Could not connect to get_path action server at " << selected_server); + return; + } + + get_path_action_server_name_ = selected_server; + RCLCPP_INFO_STREAM(ros_node_->get_logger(), "Connected to get_path action server at " << selected_server); + setStatusLabel(get_path_action_server_status_, "ready", status_success); - RCLCPP_INFO_STREAM(node->get_logger(), "Guessed node name for planner list: " << node_name); + // bool parameter_client_reinitialized = false; + if(!planner_parameter_client_) + { + // TODO(amock): this is a bit hacky. We extract the node name from the action server topic + // - but if the topic was remapped we get a problem. Currently I dont know an efficient (!) solution to that problem + get_path_node_name_ = node_name_guess(selected_server); - planner_parameter_client_ = std::make_shared(node, node_name); + RCLCPP_INFO_STREAM(ros_node_->get_logger(), "Guessed node name for planner list: " << get_path_node_name_); + setStatusLabel(get_path_action_server_status_, "connecting to parameters...", status_info); + planner_parameter_client_ = std::make_shared(ros_node_, get_path_node_name_); + } + planner_parameter_client_->wait_for_service(std::chrono::seconds(1)); + + if(!planner_parameter_client_->service_is_ready()) + { + setStatusLabel(get_path_action_server_status_, "Could not connect to parameters!", status_error); + RCLCPP_ERROR_STREAM(ros_node_->get_logger(), "Could not connect to parameter service of planner action server node " << get_path_node_name_); + return; + } + + // update list of planners + setStatusLabel(get_path_action_server_status_, "updating planner list...", status_info); planner_parameter_client_->get_parameters({"planners"}, - [this, node, selected_server](std::shared_future> future) { + [this, selected_server](std::shared_future> future) { + auto parameters = future.get(); if(!parameters.empty()) { + planner_name_property_->clearOptions(); const std::vector planners = parameters[0].as_string_array(); for(const std::string& planner : planners) { planner_name_property_->addOptionStd(planner); } - RCLCPP_INFO_STREAM(node->get_logger(), "Received planner list with " << planners.size() << " entries for planner action " << selected_server); + RCLCPP_INFO_STREAM(this->ros_node_->get_logger(), "Received planner list with " << planners.size() << " entries for planner action " << selected_server); + setStatusLabel(this->get_path_action_server_status_, "ready", status_success); } else { - this->get_path_action_goal_status_->setText(QString("No planner found!")); - RCLCPP_WARN_STREAM(node->get_logger(), "No planner found for planner action " << selected_server); + setStatusLabel(this->get_path_action_server_status_, "No planner found!", status_error); + RCLCPP_WARN_STREAM(this->ros_node_->get_logger(), "No planner found for planner action " << selected_server); } }); } void MbfGoalActionsPanel::updateExePathActionClient() { - auto node = getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - + std::unique_lock lock(exe_path_action_client_mutex_); + const std::string selected_server = exe_path_action_server_path_->getAction().toStdString(); - if (selected_server != default_action_server_path) { - action_client_exe_path_ = rclcpp_action::create_client( - node, selected_server); - if (action_client_exe_path_->action_server_is_ready()) { - exe_path_action_server_status_->setText("ready"); + + if(selected_server == notset_action_server_path) + { + // reset action client: first cancel active goal, then reset client + if (action_client_exe_path_ && goal_handle_exe_path_) { + + RCLCPP_INFO_STREAM(ros_node_->get_logger(), "Cancelling active exe_path goal due to action server change..."); + setStatusLabel(exe_path_action_goal_status_, "Cancelling active goal", status_warning); + + action_client_exe_path_->async_cancel_goal(goal_handle_exe_path_, [this]( + const typename ExePathClient::CancelResponse::SharedPtr & response) + { + if(response->return_code == ExePathClient::CancelResponse::ERROR_NONE) + { + setStatusLabel(this->exe_path_action_goal_status_, "Controller action stopped", status_success); + } else { + setStatusLabel(this->exe_path_action_goal_status_, "Failed to stop controller action", status_error); + } + + // reset action client and controller parameter client + action_client_exe_path_.reset(); + goal_handle_exe_path_.reset(); + + controller_parameter_client_.reset(); + setStatusLabel(exe_path_action_server_status_, "waiting for input", status_neutral); + setStatusLabel(exe_path_action_goal_status_, "none sent yet", status_neutral); + }); + } else { - exe_path_action_server_status_->setText("connecting"); + action_client_exe_path_.reset(); + goal_handle_exe_path_.reset(); + + controller_parameter_client_.reset(); + setStatusLabel(exe_path_action_server_status_, "waiting for input", status_neutral); + setStatusLabel(exe_path_action_goal_status_, "none sent yet", status_neutral); } + + return; } - // fetch controllers via parameters: - // string list: [node_name_of_action_server].controllers - controller_name_property_->clearOptions(); + if(action_client_exe_path_) + { + // was already initialized + if(selected_server == exe_path_action_server_name_) + { + // same server than before: check if connection has been lost + if(!action_client_exe_path_->action_server_is_ready()) + { + RCLCPP_WARN_STREAM(ros_node_->get_logger(), "Connection to exe_path action server lost. Resetting action client..."); + setStatusLabel(exe_path_action_server_status_, "connection lost", status_error); + action_client_exe_path_.reset(); // reset so that reinitialization is triggered + goal_handle_exe_path_.reset(); + controller_parameter_client_.reset(); + setStatusLabel(exe_path_action_server_status_, "waiting for input", status_neutral); + setStatusLabel(exe_path_action_goal_status_, "none sent yet", status_neutral); + } + } else { + // different server than before - // TODO(amock): this is a bit hacky. We extract the node name from the action server topic - // - but if the topic was remapped we get a problem. Currently I dont know an efficient (!) solution to that problem - std::string node_name = node_name_guess(selected_server); + // reset action client: first cancel active goal, then reset client + if (goal_handle_exe_path_) { + + RCLCPP_INFO_STREAM(ros_node_->get_logger(), "Cancelling active exe_path goal due to action server change..."); + setStatusLabel(exe_path_action_goal_status_, "Cancelling active goal", status_warning); + + action_client_exe_path_->async_cancel_goal(goal_handle_exe_path_, [this]( + const typename ExePathClient::CancelResponse::SharedPtr & response) + { + if(response->return_code == ExePathClient::CancelResponse::ERROR_NONE) + { + setStatusLabel(this->exe_path_action_goal_status_, "Controller action stopped", status_success); + } else { + setStatusLabel(this->exe_path_action_goal_status_, "Failed to stop controller action", status_error); + } + goal_handle_exe_path_.reset(); + }); + } + + while(goal_handle_exe_path_) + { + RCLCPP_INFO_STREAM(ros_node_->get_logger(), "Waiting for active exe_path goal to be cancelled..."); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } - RCLCPP_INFO_STREAM(node->get_logger(), "Guessed node name for controller list: " << node_name); + // reset action client and controller parameter client + action_client_exe_path_.reset(); - controller_parameter_client_ = std::make_shared(node, node_name); + controller_parameter_client_.reset(); + setStatusLabel(exe_path_action_server_status_, "waiting for input", status_neutral); + setStatusLabel(exe_path_action_goal_status_, "none sent yet", status_neutral); + } + } + + // (re-)initialize action client if not initialized or resetted due to server change + if(!action_client_exe_path_) + { + try { + action_client_exe_path_ = rclcpp_action::create_client( + ros_node_, selected_server); + goal_handle_exe_path_.reset(); + } catch (const std::exception& e) { + RCLCPP_ERROR_STREAM(ros_node_->get_logger(), "Failed to create action client for exe_path action server at " << selected_server << ": " << e.what()); + setStatusLabel(exe_path_action_server_status_, "connection failed!", status_error); + return; + } + controller_parameter_client_.reset(); // force the following steps to reinitialize the parameter client + } + + // connecting + setStatusLabel(exe_path_action_server_status_, "connecting...", status_info); + action_client_exe_path_->wait_for_action_server( + std::chrono::seconds(1)); + + if(!action_client_exe_path_->action_server_is_ready()) + { + setStatusLabel(exe_path_action_server_status_, "connection failed!", status_error); + RCLCPP_ERROR_STREAM(ros_node_->get_logger(), "Could not connect to exe_path action server at " << selected_server); + return; + } + + exe_path_action_server_name_ = selected_server; + RCLCPP_INFO_STREAM(ros_node_->get_logger(), "Connected to exe_path action server at " << selected_server); + setStatusLabel(exe_path_action_server_status_, "ready", status_success); + + // bool parameter_client_reinitialized = false; + if(!controller_parameter_client_) + { + // TODO(amock): this is a bit hacky. We extract the node name from the action server topic + // - but if the topic was remapped we get a problem. Currently I dont know an efficient (!) solution to that problem + exe_path_node_name_ = node_name_guess(selected_server); + + RCLCPP_INFO_STREAM(ros_node_->get_logger(), "Guessed node name for controller list: " << exe_path_node_name_); + + setStatusLabel(exe_path_action_server_status_, "connecting to parameters...", status_info); + controller_parameter_client_ = std::make_shared(ros_node_, exe_path_node_name_); + } + controller_parameter_client_->wait_for_service(std::chrono::seconds(1)); + + if(!controller_parameter_client_->service_is_ready()) + { + setStatusLabel(exe_path_action_server_status_, "Could not connect to parameters!", status_error); + RCLCPP_ERROR_STREAM(ros_node_->get_logger(), "Could not connect to parameter service of exe_path action server node " << exe_path_node_name_); + return; + } + // update list of controllers + setStatusLabel(exe_path_action_server_status_, "updating controller list...", status_info); controller_parameter_client_->get_parameters({"controllers"}, - [this, node, selected_server](std::shared_future> future) { + [this, selected_server](std::shared_future> future) { + auto parameters = future.get(); if(!parameters.empty()) { + controller_name_property_->clearOptions(); const std::vector controllers = parameters[0].as_string_array(); for(const std::string& controller : controllers) { controller_name_property_->addOptionStd(controller); } - RCLCPP_INFO_STREAM(node->get_logger(), "Received controller list with " << controllers.size() << " entries for controller action " << selected_server); + RCLCPP_INFO_STREAM(this->ros_node_->get_logger(), "Received controller list with " << controllers.size() << " entries for exe_path action " << selected_server); + setStatusLabel(this->exe_path_action_server_status_, "ready", status_success); } else { - this->exe_path_action_goal_status_->setText(QString("No controller found!")); - RCLCPP_WARN_STREAM(node->get_logger(), "No controller found for controller action " << selected_server); + setStatusLabel(this->exe_path_action_server_status_, "No controller found!", status_error); + RCLCPP_WARN_STREAM(this->ros_node_->get_logger(), "No controller found for exe_path action " << selected_server); } }); } -void MbfGoalActionsPanel::onInitialize() +void MbfGoalActionsPanel::stopGetPathAction() +{ + std::unique_lock lock(get_path_action_client_mutex_); + + if (!action_client_get_path_) + { + setStatusLabel(get_path_action_goal_status_, "No planner action to stop", status_neutral); + return; + } + + if(!goal_handle_get_path_) + { + setStatusLabel(get_path_action_goal_status_, "No planner goal to stop", status_neutral); + return; + } + + // start stopping planner action + setStatusLabel(get_path_action_goal_status_, "Stopping planner action...", status_warning); + action_client_get_path_->async_cancel_goal(goal_handle_get_path_, [this]( + const typename GetPathClient::CancelResponse::SharedPtr & response) + { + if(response->return_code == GetPathClient::CancelResponse::ERROR_NONE) + { + setStatusLabel(this->get_path_action_goal_status_, "Planner action stopped", status_success); + } else { + setStatusLabel(this->get_path_action_goal_status_, "Failed to stop planner action", status_error); + } + goal_handle_get_path_.reset(); + }); +} + +void MbfGoalActionsPanel::stopExePathAction() { - const auto ros_node_abstraction = getDisplayContext()->getRosNodeAbstraction(); - goal_input_topic_->initialize(ros_node_abstraction); - get_path_action_server_path_->initialize(ros_node_abstraction); - exe_path_action_server_path_->initialize(ros_node_abstraction); + std::unique_lock lock(exe_path_action_client_mutex_); + + if (!action_client_exe_path_) + { + setStatusLabel(exe_path_action_goal_status_, "No controller action to stop", status_neutral); + return; + } + + if(!goal_handle_exe_path_) + { + setStatusLabel(exe_path_action_goal_status_, "No controller goal to stop", status_neutral); + return; + } + + // start stopping controller action + setStatusLabel(exe_path_action_goal_status_, "Stopping controller action...", status_warning); + action_client_exe_path_->async_cancel_goal(goal_handle_exe_path_, [this]( + const typename ExePathClient::CancelResponse::SharedPtr & response) + { + if(response->return_code == ExePathClient::CancelResponse::ERROR_NONE) + { + setStatusLabel(this->exe_path_action_goal_status_, "Controller action stopped", status_success); + } else { + setStatusLabel(this->exe_path_action_goal_status_, "Failed to stop controller action", status_error); + } + goal_handle_exe_path_.reset(); + }); } -void MbfGoalActionsPanel::newMeshGoalCallback(const geometry_msgs::msg::PoseStamped & msg) +void MbfGoalActionsPanel::onInitialize() +{ + const auto rviz_ros_node_abstraction = getDisplayContext()->getRosNodeAbstraction(); + goal_input_topic_->initialize(rviz_ros_node_abstraction); + get_path_action_server_path_->initialize(rviz_ros_node_abstraction); + exe_path_action_server_path_->initialize(rviz_ros_node_abstraction); + + // rviz node is spinned externally. But we need full control when calling action clients + ros_node_ = rclcpp::Node::make_shared("mbf_goal_actions_panel"); + + conn_check_thread_get_path_stop_ = false; + conn_check_thread_get_path_ = std::thread([this]() -> void { + while (rclcpp::ok() && !conn_check_thread_get_path_stop_) { + + const bool server_set = (get_path_action_server_path_->getAction().toStdString() != notset_action_server_path); + + if(server_set) + { + if(!action_client_get_path_ || !action_client_get_path_->action_server_is_ready()) + { + RCLCPP_WARN_STREAM(this->ros_node_->get_logger(), "Get_path action server not ready. Attempting to reconnect..."); + setStatusLabel(get_path_action_server_status_, "not connected", status_error); + updateGetPathActionClient(); + } else { + std::unique_lock lock(get_path_action_client_mutex_); + setStatusLabel(get_path_action_server_status_, "ready", status_success); + } + } + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + } + }); + + conn_check_thread_exe_path_stop_ = false; + conn_check_thread_exe_path_ = std::thread([this]() -> void { + while (rclcpp::ok() && !conn_check_thread_exe_path_stop_) { + const bool server_set = (exe_path_action_server_path_->getAction().toStdString() != notset_action_server_path); + + if(server_set) + { + if(!action_client_exe_path_ || !action_client_exe_path_->action_server_is_ready()) + { + RCLCPP_WARN_STREAM(this->ros_node_->get_logger(), "Exe_path action server not ready. Attempting to reconnect..."); + setStatusLabel(exe_path_action_server_status_, "not connected", status_error); + updateExePathActionClient(); + } else { + std::unique_lock lock(exe_path_action_client_mutex_); + setStatusLabel(exe_path_action_server_status_, "ready", status_success); + } + } + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + } + }); + + // executor thread. needed so that actions work properly + executor_thread_stop_ = false; + executor_thread_ = std::thread([this]() -> void { + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(ros_node_); + while (rclcpp::ok() && !executor_thread_stop_) { + executor.spin_once(std::chrono::milliseconds(100)); + } + }); + +} + +void MbfGoalActionsPanel::newGoalCallback(const geometry_msgs::msg::PoseStamped & msg) { goal_input_status_->setText(QString("Goal received at t=%1").arg(msg.header.stamp.sec)); goal_retry_cnt_ = 0; current_goal_ = msg; - mbf_msgs::action::GetPath::Goal goal; - goal.target_pose = msg; - goal.planner = planner_name_property_->getStdString(); - goal.use_start_pose = false; // planner shall use the current robot pose + mbf_msgs::action::GetPath::Goal get_path_goal; + get_path_goal.target_pose = msg; + get_path_goal.planner = planner_name_property_->getStdString(); + get_path_goal.use_start_pose = false; // planner shall use the current robot pose + + { + std::unique_lock lock(get_path_action_client_mutex_); + if(!action_client_get_path_) + { + RCLCPP_ERROR_STREAM(ros_node_->get_logger(), "Get path action client not initialized"); + return; + } + if(!action_client_get_path_->action_server_is_ready()) + { + RCLCPP_ERROR_STREAM(ros_node_->get_logger(), "Get_path action server is still not ready after update. Cannot send goal."); + return; + } + } + if (goal_handle_get_path_) { // planner is currently active, cancel first // result callback will start new planner with next_get_path_goal_ as goal // UI currently cannot properly handle parallel actions - next_get_path_goal_ = goal; - get_path_action_goal_status_->setText("Cancelling..."); - action_client_get_path_->async_cancel_goal(goal_handle_get_path_); + // next_get_path_goal_ = get_path_goal; + + setStatusLabel(get_path_action_goal_status_, "Cancelling...", status_warning); + action_client_get_path_->async_cancel_goal(goal_handle_get_path_, [this, get_path_goal]( + const typename GetPathClient::CancelResponse::SharedPtr & response) + { + if(response->return_code == GetPathClient::CancelResponse::ERROR_NONE) + { + setStatusLabel(this->get_path_action_goal_status_, "Cancelled", status_success); + // next_get_path_goal_.reset(); + goal_handle_get_path_.reset(); + } else { + setStatusLabel(this->get_path_action_goal_status_, "Failed to cancel active goal", status_error); + } + + sendGetPathGoal(get_path_goal); + }); + } else { - sendGetPathGoal(goal); + sendGetPathGoal(get_path_goal); } } void MbfGoalActionsPanel::sendGetPathGoal(const mbf_msgs::action::GetPath::Goal & goal) { + { + std::unique_lock lock(get_path_action_client_mutex_); + if(!action_client_get_path_) + { + RCLCPP_ERROR_STREAM(ros_node_->get_logger(), "Get path action client not initialized when sending goal"); + setStatusLabel(get_path_action_goal_status_, "Goal sending failed: client not ready", status_error); + return; + } + } + GetPathClient::SendGoalOptions options; const auto goal_stamp = goal.target_pose.header.stamp; options.goal_response_callback = [goal_stamp, this](const GetPathClient::GoalHandle::SharedPtr & goal_handle) { this->goal_handle_get_path_ = goal_handle; if (goal_handle) { - this->get_path_action_goal_status_->setText(QString("(t=%1) accepted").arg(goal_stamp.sec)); + setStatusLabel(get_path_action_goal_status_, QString("(t=%1) accepted").arg(goal_stamp.sec), status_success); } else { - this->get_path_action_goal_status_->setText(QString("(t=%1) rejected").arg(goal_stamp.sec)); + setStatusLabel(get_path_action_goal_status_, QString("(t=%1) rejected").arg(goal_stamp.sec), status_error); } }; options.result_callback = std::bind( &MbfGoalActionsPanel::getPathResultCallback, this, std::placeholders::_1); - action_client_get_path_->async_send_goal(goal, options); - get_path_action_goal_status_->setText( - QString("(t=%1) sent, awaiting response").arg( - goal_stamp.sec)); + { + std::unique_lock lock(get_path_action_client_mutex_); + if(action_client_get_path_) + { + action_client_get_path_->async_send_goal(goal, options); + } + } + setStatusLabel(get_path_action_goal_status_, QString("(t=%1) sent, awaiting response").arg(goal_stamp.sec), status_info); } void MbfGoalActionsPanel::getPathResultCallback( @@ -362,44 +863,59 @@ void MbfGoalActionsPanel::getPathResultCallback( mbf_msgs::action::ExePath::Goal exe_path_goal; switch (wrapped_result.code) { case rclcpp_action::ResultCode::SUCCEEDED: - get_path_action_goal_status_->setText( - QString("Succeeded. Path cost %1").arg( - wrapped_result.result->cost)); + setStatusLabel(get_path_action_goal_status_, QString("Succeeded. Path cost %1").arg(wrapped_result.result->cost), status_success); exe_path_goal.path = wrapped_result.result->path; exe_path_goal.controller = controller_name_property_->getStdString(); + { + std::unique_lock lock(exe_path_action_client_mutex_); + if(!action_client_exe_path_) + { + RCLCPP_ERROR_STREAM(ros_node_->get_logger(), "Exe_path action client not initialized in result callback"); + return; + } + if(!action_client_exe_path_->action_server_is_ready()) + { + RCLCPP_ERROR_STREAM(ros_node_->get_logger(), "Exe_path action server is still not ready after update. Cannot send goal."); + return; + } + } + if (goal_handle_exe_path_) { + // path execution is currently active, cancel first // result callback will start new path execution with next_exe_path_goal_ as goal // UI currently cannot properly handle parallel actions - next_exe_path_goal_ = exe_path_goal; - exe_path_action_goal_status_->setText("Requested cancel"); - action_client_exe_path_->async_cancel_goal(goal_handle_exe_path_); + // next_exe_path_goal_ = exe_path_goal; + setStatusLabel(exe_path_action_goal_status_, "Cancel existing", status_warning); + + action_client_exe_path_->async_cancel_goal(goal_handle_exe_path_, [this, exe_path_goal]( + const typename ExePathClient::CancelResponse::SharedPtr & response) + { + if(response->return_code == ExePathClient::CancelResponse::ERROR_NONE) + { + setStatusLabel(this->exe_path_action_goal_status_, "Existing goal cancelled", status_success); + } else { + setStatusLabel(this->exe_path_action_goal_status_, "Failed to cancel existing goal", status_error); + } + + goal_handle_exe_path_.reset(); + sendExePathGoal(exe_path_goal); + }); + } else { sendExePathGoal(exe_path_goal); } break; case rclcpp_action::ResultCode::ABORTED: - get_path_action_goal_status_->setText( - QString("Aborted. %1").arg( - QString::fromStdString( - wrapped_result.result-> - message))); + setStatusLabel(get_path_action_goal_status_, QString("Aborted. %1").arg(QString::fromStdString(wrapped_result.result->message)), status_error); break; case rclcpp_action::ResultCode::CANCELED: - get_path_action_goal_status_->setText( - QString("Cancelled. %1").arg( - QString::fromStdString( - wrapped_result.result-> - message))); - if (next_get_path_goal_.has_value()) { - sendGetPathGoal(next_get_path_goal_.value()); - next_get_path_goal_.reset(); - } + setStatusLabel(get_path_action_goal_status_, QString("Cancelled. %1").arg(QString::fromStdString(wrapped_result.result->message)), status_warning); break; default: - get_path_action_goal_status_->setText("Error: Unknown action result code"); + setStatusLabel(get_path_action_goal_status_, "Error: Unknown action result code", status_error); break; } goal_handle_get_path_.reset(); @@ -407,39 +923,46 @@ void MbfGoalActionsPanel::getPathResultCallback( void MbfGoalActionsPanel::sendExePathGoal(const mbf_msgs::action::ExePath::Goal & goal) { + { + std::unique_lock lock(exe_path_action_client_mutex_); + if(!action_client_exe_path_) + { + RCLCPP_ERROR_STREAM(ros_node_->get_logger(), "Exe path action client not initialized when sending goal"); + setStatusLabel(exe_path_action_goal_status_, "Goal sending failed: client not ready", status_error); + return; + } + } + const auto goal_stamp = goal.path.header.stamp; ExePathClient::SendGoalOptions options; options.goal_response_callback = - [goal_stamp, this](const ExePathClient::GoalHandle::SharedPtr & goal_handle) { + [this, goal_stamp](const ExePathClient::GoalHandle::SharedPtr & goal_handle) { + RCLCPP_WARN(this->ros_node_->get_logger(), "SETTING GOAL HANDLE"); this->goal_handle_exe_path_ = goal_handle; if (goal_handle) { - this->exe_path_action_goal_status_->setText( - QString("Goal (t=%1) accepted").arg( - goal_stamp. - sec)); + setStatusLabel(this->exe_path_action_goal_status_, QString("Goal (t=%1) accepted").arg(goal_stamp.sec), status_success); } else { - this->exe_path_action_goal_status_->setText( - QString("Goal (t=%1) rejected").arg( - goal_stamp. - sec)); + setStatusLabel(this->exe_path_action_goal_status_, QString("Goal (t=%1) rejected").arg(goal_stamp.sec), status_error); } }; options.feedback_callback = [this]( ExePathClient::GoalHandle::SharedPtr, const mbf_msgs::action::ExePath::Feedback::ConstSharedPtr feedback) { - this->exe_path_action_goal_status_->setText( - QString("Executing. Remaining distance: %1m").arg( - feedback->dist_to_goal)); + setStatusLabel(this->exe_path_action_goal_status_, QString("Executing. Remaining distance: %1m").arg(feedback->dist_to_goal), status_info); }; options.result_callback = std::bind( &MbfGoalActionsPanel::exePathResultCallback, this, std::placeholders::_1); - action_client_exe_path_->async_send_goal(goal, options); - exe_path_action_goal_status_->setText( - QString("(t=%1) sent, awaiting response").arg( - goal_stamp.sec)); + { + std::unique_lock lock(exe_path_action_client_mutex_); + if(action_client_exe_path_) + { + action_client_exe_path_->async_send_goal(goal, options); + } + } + setStatusLabel(exe_path_action_goal_status_, QString("(t=%1) sent, awaiting response").arg(goal_stamp.sec), status_info); } void MbfGoalActionsPanel::exePathResultCallback( @@ -447,9 +970,7 @@ void MbfGoalActionsPanel::exePathResultCallback( { switch (wrapped_result.code) { case rclcpp_action::ResultCode::SUCCEEDED: - exe_path_action_goal_status_->setText( - QString("Succeeded (remaining distance: %1").arg( - wrapped_result.result->dist_to_goal)); + setStatusLabel(exe_path_action_goal_status_, QString("Succeeded (remaining distance: %1)").arg(wrapped_result.result->dist_to_goal), status_success); break; case rclcpp_action::ResultCode::ABORTED: if (wrapped_result.result->outcome == mbf_msgs::action::ExePath::Result::ROBOT_STUCK) { @@ -462,35 +983,19 @@ void MbfGoalActionsPanel::exePathResultCallback( goal.target_pose.header.set__stamp(getDisplayContext()->getClock()->now()); goal.use_start_pose = false; sendGetPathGoal(goal); - exe_path_action_goal_status_->setText( - QString("Robot stuck. Replanning ...") - ); + setStatusLabel(exe_path_action_goal_status_, QString("Robot stuck. Replanning ..."), status_info); } else { - exe_path_action_goal_status_->setText( - QString("Robot stuck. Maximum retries exceeded! Goal failed!") - ); + setStatusLabel(exe_path_action_goal_status_, QString("Robot stuck. Maximum retries exceeded! Goal failed!"), status_error); } break; } - exe_path_action_goal_status_->setText( - QString("Aborted. %1").arg( - QString::fromStdString( - wrapped_result.result-> - message))); + setStatusLabel(exe_path_action_goal_status_, QString("Aborted. %1").arg(QString::fromStdString(wrapped_result.result->message)), status_error); break; case rclcpp_action::ResultCode::CANCELED: - exe_path_action_goal_status_->setText( - QString("Cancelled. %1").arg( - QString::fromStdString( - wrapped_result.result-> - message))); - if (next_exe_path_goal_.has_value()) { - sendExePathGoal(next_exe_path_goal_.value()); - next_get_path_goal_.reset(); - } + setStatusLabel(exe_path_action_goal_status_, QString("Cancelled. %1").arg(QString::fromStdString(wrapped_result.result->message)), status_warning); break; default: - exe_path_action_goal_status_->setText("Error: Unknown action result code"); + setStatusLabel(exe_path_action_goal_status_, "Error: Unknown action result code", status_error); break; } goal_handle_exe_path_.reset();