sweet ai rviz panel

CMakeLists.txt

@@ -0,0 +1,74 @@
+cmake_minimum_required(VERSION 3.5)
+project(sweet_rviz_panel)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rviz_common REQUIRED)
+find_package(rviz_rendering REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(Qt5 REQUIRED COMPONENTS Widgets)
+
+set(CMAKE_AUTOMOC ON)
+set(CMAKE_AUTOUIC ON)
+set(CMAKE_AUTORCC ON)
+
+set(HEADERS
+  include/sweet_rviz_panel/sweet_rviz_panel.hpp
+)
+
+set(SOURCES
+  src/sweet_rviz_panel.cpp
+)
+
+add_library(${PROJECT_NAME} SHARED
+  ${SOURCES}
+  ${HEADERS}
+)
+
+target_include_directories(${PROJECT_NAME}
+  PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+ament_target_dependencies(${PROJECT_NAME}
+  rclcpp
+  rviz_common
+  rviz_rendering
+  pluginlib
+  geometry_msgs
+  tf2
+)
+
+target_link_libraries(${PROJECT_NAME}
+  Qt5::Widgets
+)
+
+pluginlib_export_plugin_description_file(
+  rviz_common
+  plugin_description.xml
+)
+
+install(
+  TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(
+  FILES plugin_description.xml
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()

include/sweet_rviz_panel/sweet_rviz_panel.hpp

@@ -0,0 +1,63 @@
+#ifndef SWEET_RVIZ_PANEL_HPP
+#define SWEET_RVIZ_PANEL_HPP
+
+#include <memory>
+
+#include <QLabel>
+#include <QPushButton>
+#include <QVBoxLayout>
+#include <QHBoxLayout>
+
+#include <rviz_common/panel.hpp>
+
+#include <rclcpp/rclcpp.hpp>
+#include "geometry_msgs/msg/twist.hpp"
+#include "geometry_msgs/msg/pose.hpp"
+#include <rviz_common/display_context.hpp>
+#include <rviz_common/ros_integration/ros_node_abstraction_iface.hpp>
+
+namespace sweet_rviz_panel
+{
+
+class SweetRvizPanel : public rviz_common::Panel
+{
+  Q_OBJECT
+
+public:
+  explicit SweetRvizPanel(QWidget *parent = nullptr);
+
+protected:
+  void onInitialize() override;
+
+private Q_SLOTS:
+  void onStart();
+  void onStop();
+
+private:
+  void poseCallback(
+    const geometry_msgs::msg::Pose::SharedPtr msg);
+  void velCmdCallback(
+    const geometry_msgs::msg::Twist::SharedPtr msg);
+  QLabel *x_label_;
+  QLabel *y_label_;
+  QLabel *yaw_label_;
+  QLabel *status_label_;
+
+  QLabel *vel_cmd_label_;
+  QLabel *yaw_cmd_label_;
+
+  QPushButton *start_button_;
+  QPushButton *stop_button_;
+
+  rclcpp::Node::SharedPtr node_;
+  std::shared_ptr<
+  rviz_common::ros_integration::RosNodeAbstractionIface> node_ptr_;
+  rclcpp::Subscription<geometry_msgs::msg::Pose>::SharedPtr pose_sub_;
+  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr twist_cmd_sub_;
+
+  bool running_;
+};
+
+}  // namespace sweet_rviz_panel
+
+#endif

package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<package format="3">
+
+  <name>sweet_rviz_panel</name>
+  <version>0.0.1</version>
+  <description>Sweet RViz2 vehicle debug panel</description>
+
+  <maintainer email="sweetai@example.com">sweetai</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rviz_common</depend>
+  <depend>rviz_rendering</depend>
+  <depend>pluginlib</depend>
+  <depend>geometry_msgs</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+    <rviz_common plugin="${prefix}/plugin_description.xml"/>
+  </export>
+
+</package>

plugin_description.xml

@@ -0,0 +1,10 @@
+<library path="sweet_rviz_panel">
+  <class
+    name="sweet_rviz_panel/SweetRvizPanel"
+    type="sweet_rviz_panel::SweetRvizPanel"
+    base_class_type="rviz_common::Panel">
+    <description>
+      Sweet Robot RViz2 debug panel.
+    </description>
+  </class>
+</library>

src/sweet_rviz_panel.cpp

@@ -0,0 +1,176 @@
+#include "sweet_rviz_panel/sweet_rviz_panel.hpp"
+
+#include <cmath>
+#include <QGroupBox>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Matrix3x3.h>
+
+#include <pluginlib/class_list_macros.hpp>
+
+namespace sweet_rviz_panel
+{
+
+SweetRvizPanel::SweetRvizPanel(QWidget *parent)
+: rviz_common::Panel(parent),
+  x_label_(nullptr),
+  y_label_(nullptr),
+  yaw_label_(nullptr),
+  status_label_(nullptr),
+  start_button_(nullptr),
+  stop_button_(nullptr),
+  vel_cmd_label_(nullptr),
+  yaw_cmd_label_(nullptr),
+  running_(false)
+{
+  auto *main_layout = new QVBoxLayout;
+  // vehicle pose
+  auto *pose_box = new QGroupBox("Vehicle Pose");
+  auto *pose_layout = new QVBoxLayout;
+
+  x_label_ = new QLabel("X: 0.000 m");
+  y_label_ = new QLabel("Y: 0.000 m");
+  yaw_label_ = new QLabel("Yaw: 0.000 rad");
+
+  pose_layout->addWidget(x_label_);
+  pose_layout->addWidget(y_label_);
+  pose_layout->addWidget(yaw_label_);
+
+  pose_box->setLayout(pose_layout);
+  // command 
+  auto *vel_cmd_box = new QGroupBox("Command Info");
+  auto *vel_cmd_layout = new QVBoxLayout;
+  vel_cmd_label_ = new QLabel("Velocity Command: 0.00 m/s");
+  yaw_cmd_label_ = new QLabel("Yaw Command: 0.00 rad/s");
+
+  vel_cmd_layout->addWidget(vel_cmd_label_);
+  vel_cmd_layout->addWidget(yaw_cmd_label_);
+  vel_cmd_box->setLayout(vel_cmd_layout);
+  // other buttons
+  auto *button_layout = new QHBoxLayout;
+
+  start_button_ = new QPushButton("Start");
+  stop_button_ = new QPushButton("Stop");
+
+  button_layout->addWidget(start_button_);
+  button_layout->addWidget(stop_button_);
+
+  status_label_ = new QLabel("Status: STOPPED");
+
+  main_layout->addWidget(pose_box);
+  main_layout->addWidget(vel_cmd_box);
+  main_layout->addLayout(button_layout);
+  main_layout->addWidget(status_label_);
+  main_layout->addStretch();
+
+  setLayout(main_layout);
+
+  connect(
+    start_button_,
+    SIGNAL(clicked()),
+    this,
+    SLOT(onStart()));
+
+  connect(
+    stop_button_,
+    SIGNAL(clicked()),
+    this,
+    SLOT(onStop()));
+}
+
+void SweetRvizPanel::onInitialize()
+{
+  node_ = std::make_shared<rclcpp::Node>("sweet_rviz_panel");
+  node_ptr_= getDisplayContext()->getRosNodeAbstraction().lock();
+  if (!node_ptr_) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("SweetRvizPanel"),
+      "Failed to get RViz ROS node");
+
+    return;
+  }
+  auto node = node_ptr_->get_raw_node();
+
+  pose_sub_ = node->create_subscription<
+    geometry_msgs::msg::Pose>(
+    "/rtk_pose",
+    10,
+    std::bind(&SweetRvizPanel::poseCallback,this,std::placeholders::_1));
+  
+    twist_cmd_sub_ = node->create_subscription<
+    geometry_msgs::msg::Twist>(
+    "/twist_cmd_vel",
+    10,
+    std::bind(&SweetRvizPanel::velCmdCallback,this,std::placeholders::_1));
+  
+  running_ = true;
+  status_label_->setText("Status: RUNNING");
+}
+
+void SweetRvizPanel::poseCallback(
+  const geometry_msgs::msg::Pose::SharedPtr msg)
+{
+  if (!running_) {
+    return;
+  }
+
+  const double x = msg->position.x;
+  const double y = msg->position.y;
+
+  const auto &q = msg->orientation;
+
+  tf2::Quaternion quaternion(
+    q.x,
+    q.y,
+    q.z,
+    q.w);
+
+  double roll;
+  double pitch;
+  double yaw;
+
+  tf2::Matrix3x3(quaternion).getRPY(
+    roll,
+    pitch,
+    yaw);
+
+  x_label_->setText(
+    QString("X: %1 m").arg(x, 0, 'f', 3));
+
+  y_label_->setText(
+    QString("Y: %1 m").arg(y, 0, 'f', 3));
+
+  yaw_label_->setText(
+    QString("Yaw: %1 rad").arg(yaw, 0, 'f', 3));
+}
+
+void SweetRvizPanel::velCmdCallback(
+  const geometry_msgs::msg::Twist::SharedPtr msg)
+{
+  if (!running_) {
+    return;
+  }
+
+  vel_cmd_label_->setText(
+    QString("Velocity Command: %1 m/s").arg(msg->linear.x, 0, 'f', 3));
+
+  yaw_cmd_label_->setText(
+    QString("Yaw Command: %1 rad/s").arg(msg->angular.z, 0, 'f', 3));
+}
+
+void SweetRvizPanel::onStart()
+{
+  running_ = true;
+  status_label_->setText("Status: RUNNING");
+}
+
+void SweetRvizPanel::onStop()
+{
+  running_ = false;
+  status_label_->setText("Status: STOPPED");
+}
+
+}  // namespace sweet_rviz_panel
+
+PLUGINLIB_EXPORT_CLASS(
+  sweet_rviz_panel::SweetRvizPanel,
+  rviz_common::Panel)