Program Listing for File docking_panel.hpp

↰ Return to documentation for file (include/nav2_rviz_plugins/docking_panel.hpp)

// Copyright (c) 2024 Alberto J. Tudela Roldán
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef NAV2_RVIZ_PLUGINS__DOCKING_PANEL_HPP_
#define NAV2_RVIZ_PLUGINS__DOCKING_PANEL_HPP_

// QT
#include <QtWidgets>
#include <QBasicTimer>

#include <string>

// ROS
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rviz_common/panel.hpp"
#include "sensor_msgs/msg/battery_state.hpp"
#include "nav2_msgs/action/dock_robot.hpp"
#include "nav2_msgs/action/undock_robot.hpp"

class QLineEdit;
class QPushButton;

namespace nav2_rviz_plugins
{

class DockingPanel : public rviz_common::Panel
{
  Q_OBJECT

public:
  explicit DockingPanel(QWidget * parent = 0);
  virtual ~DockingPanel();
  void onInitialize() override;

private Q_SLOTS:
  void onDockingButtonPressed();
  void onUndockingButtonPressed();
  void dockIdCheckbox();

private:
  using Dock = nav2_msgs::action::DockRobot;
  using Undock = nav2_msgs::action::UndockRobot;
  using DockGoalHandle = rclcpp_action::ClientGoalHandle<Dock>;
  using UndockGoalHandle = rclcpp_action::ClientGoalHandle<Undock>;

  // 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;

  // Create label string from feedback msg
  static inline QString getDockFeedbackLabel(Dock::Feedback msg = Dock::Feedback());

  // Create label string from status msg
  template<typename T>
  static inline std::string toLabel(T & msg);

  // Round off double to the specified precision and convert to string
  static inline std::string toString(double val, int precision = 0);

  // Convert the dock state and error code to string
  static inline std::string dockStateToString(int16_t state);
  static inline std::string dockErrorToString(int16_t error_code);

  // The (non-spinning) client node used to invoke the action client
  rclcpp::Node::SharedPtr client_node_;
  // Timeout value when waiting for action servers to respond
  std::chrono::milliseconds server_timeout_;

  // 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};
  QVBoxLayout * feedback_layout_{nullptr};
  QHBoxLayout * dock_id_layout_{nullptr};
  QHBoxLayout * dock_type_layout_{nullptr};
  QHBoxLayout * dock_pose_layout_{nullptr};
  QHBoxLayout * nav_stage_layout_{nullptr};

  QComboBox * dock_type_{nullptr};
  QPushButton * docking_button_{nullptr};
  QPushButton * undocking_button_{nullptr};
  QCheckBox * use_dock_id_checkbox_{nullptr};
  QCheckBox * nav_to_staging_checkbox_{nullptr};

  QLabel * docking_goal_status_indicator_{nullptr};
  QLabel * docking_feedback_indicator_{nullptr};
  QLabel * docking_result_indicator_{nullptr};

  QLineEdit * dock_id_{nullptr};
  QLineEdit * dock_pose_x_{nullptr};
  QLineEdit * dock_pose_y_{nullptr};
  QLineEdit * dock_pose_yaw_{nullptr};

  // The current state of the docking and undocking actions
  bool docking_in_progress_ = false;
  bool undocking_in_progress_ = false;
  bool use_dock_id_ = false;

  // The Dock and Undock action client
  rclcpp_action::Client<Dock>::SharedPtr dock_client_;
  rclcpp_action::Client<Undock>::SharedPtr undock_client_;
  DockGoalHandle::SharedPtr dock_goal_handle_;
  UndockGoalHandle::SharedPtr undock_goal_handle_;

  // Docking / Undocking action feedback subscribers
  rclcpp::Subscription<Dock::Impl::FeedbackMessage>::SharedPtr docking_feedback_sub_;
  rclcpp::Subscription<Undock::Impl::FeedbackMessage>::SharedPtr undocking_feedback_sub_;
  rclcpp::Subscription<Dock::Impl::GoalStatusMessage>::SharedPtr docking_goal_status_sub_;
  rclcpp::Subscription<Undock::Impl::GoalStatusMessage>::SharedPtr undocking_goal_status_sub_;
};

}  // namespace nav2_rviz_plugins

#endif  // NAV2_RVIZ_PLUGINS__DOCKING_PANEL_HPP_