Merge pull request #381 from naturerobots/feature/rviz-set-planner-controller

RViz MBF Plugin Update

Author: amock

rviz_mbf_plugins/include/rviz_mbf_plugins/mbf_goal_actions_panel.hpp

@@ -54,10 +54,13 @@
 #include <QLabel>
 #include <QGroupBox>
 #include <QVBoxLayout>
+#include <QPushButton>
 
 #include <memory>
 #include <optional>
 #include <future>
+#include <thread>
+#include <atomic>
 
 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<geometry_msgs::msg::PoseStamped>::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<rclcpp::AsyncParametersClient>  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<mbf_msgs::action::GetPath::Goal> 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<rclcpp::AsyncParametersClient>  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<mbf_msgs::action::ExePath::Goal> 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<rclcpp::AsyncParametersClient>  planner_parameter_client_;
-  std::shared_ptr<rclcpp::AsyncParametersClient>  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