add error figures

CMakeLists.txt

@@ -10,6 +10,7 @@ find_package(rviz_common REQUIRED)
 find_package(rviz_rendering REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(Qt5 REQUIRED COMPONENTS Widgets)
 
@@ -18,11 +19,13 @@ set(CMAKE_AUTOUIC ON)
 set(CMAKE_AUTORCC ON)
 
 set(HEADERS
+  include/sweet_rviz_panel/qcustomplot.h
   include/sweet_rviz_panel/sweet_rviz_panel.hpp
 )
 
 set(SOURCES
   src/sweet_rviz_panel.cpp
+  src/qcustomplot.cpp
 )
 
 add_library(${PROJECT_NAME} SHARED
@@ -42,6 +45,7 @@ ament_target_dependencies(${PROJECT_NAME}
   rviz_rendering
   pluginlib
   geometry_msgs
+  nav_msgs
   tf2
 )
 

README.md

@@ -5,8 +5,8 @@ This package aims to add another panel in rviz to make some essential parametres
 ![final result](./rviz2.png)
 
 # installation
- sudo apt install libcurl4-openssl-dev
+ 1. sudo apt install libcurl4-openssl-dev
- sudo apt install nlohmann-json3-dev
+ 2. sudo apt install nlohmann-json3-dev
 
 # build and run
 1. colcon build 

include/sweet_rviz_panel/sweet_rviz_panel.hpp

@@ -2,20 +2,26 @@
 #define SWEET_RVIZ_PANEL_HPP
 
 #include <memory>
-
+#include <QTimer>
 #include <QLabel>
 #include <QPushButton>
 #include <QVBoxLayout>
 #include <QHBoxLayout>
-
+#include <QWidget>
+#include <QVector>
+#include <QElapsedTimer>
 #include <rviz_common/panel.hpp>
-
+#include "sweet_rviz_panel/qcustomplot.h"
 #include <rclcpp/rclcpp.hpp>
 #include "geometry_msgs/msg/twist.hpp"
 #include "geometry_msgs/msg/pose.hpp"
+#include <nav_msgs/msg/path.hpp>
+#include <std_msgs/msg/float64.hpp>
 #include <rviz_common/display_context.hpp>
 #include <rviz_common/ros_integration/ros_node_abstraction_iface.hpp>
 
+class QCustomPlot;
+class QCPGraph;
 namespace sweet_rviz_panel
 {
 
@@ -34,10 +40,13 @@ private Q_SLOTS:
   void onStop();
 
 private:
-  void poseCallback(
+  void poseCallback(const geometry_msgs::msg::Pose::SharedPtr msg);
-    const geometry_msgs::msg::Pose::SharedPtr msg);
+  void velCmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
-  void velCmdCallback(
+  void pathCallback(const nav_msgs::msg::Path::SharedPtr msg);
-    const geometry_msgs::msg::Twist::SharedPtr msg);
+  void calculateErrors();
+  void updateErrorPlots();
+  void initPlots();
+
   QLabel *x_label_;
   QLabel *y_label_;
   QLabel *yaw_label_;
@@ -45,15 +54,32 @@ private:
 
   QLabel *vel_cmd_label_;
   QLabel *yaw_cmd_label_;
-
   QPushButton *start_button_;
   QPushButton *stop_button_;
-
+  QTimer *timer_;
+  QCustomPlot *lateral_plot_;
+  QCustomPlot *heading_plot_;
   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_;
+  rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr odom_path_sub_;
+  double current_x_ = 0.0;
+  double current_y_ = 0.0;
+  double current_yaw_ = 0.0;
+  bool pose_received_ = false;
+  bool path_received_ = false;
+  nav_msgs::msg::Path current_path_;
+  QVector<double> error_time_;
+  QVector<double> lateral_errors_;
+  QVector<double> heading_errors_;
+  QElapsedTimer error_timer_;
+  static constexpr int MAX_ERROR_POINTS = 1000;
+
+  double search_distance_ = 10.0;
+  int start_index_=0;
+  int forward_index_ = 100;
 
   bool running_;
 };

package.xml

@@ -15,6 +15,7 @@
   <depend>rviz_rendering</depend>
   <depend>pluginlib</depend>
   <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
 
   <export>
     <build_type>ament_cmake</build_type>

src/sweet_rviz_panel.cpp

@@ -39,7 +39,7 @@ SweetRvizPanel::SweetRvizPanel(QWidget *parent)
   // 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");
+  vel_cmd_label_ = new QLabel("Vel Command: 0.00 m/s");
   yaw_cmd_label_ = new QLabel("Yaw Command: 0.00 rad/s");
 
   vel_cmd_layout->addWidget(vel_cmd_label_);
@@ -56,14 +56,19 @@ SweetRvizPanel::SweetRvizPanel(QWidget *parent)
 
   status_label_ = new QLabel("Status: STOPPED");
 
+  lateral_plot_ = new QCustomPlot(this);
+  heading_plot_ = new QCustomPlot(this);
+
   main_layout->addWidget(pose_box);
   main_layout->addWidget(vel_cmd_box);
-  main_layout->addLayout(button_layout);
+  main_layout->addWidget(lateral_plot_,1);
+  main_layout->addWidget(heading_plot_,1);
+  //main_layout->addLayout(button_layout);
   main_layout->addWidget(status_label_);
   main_layout->addStretch();
 
   setLayout(main_layout);
-
+  initPlots();
   connect(
     start_button_,
     SIGNAL(clicked()),
@@ -75,8 +80,66 @@ SweetRvizPanel::SweetRvizPanel(QWidget *parent)
     SIGNAL(clicked()),
     this,
     SLOT(onStop()));
-}
 
+  error_timer_.start();
+  timer_ = new QTimer(this);
+  connect(
+    timer_,
+    &QTimer::timeout,
+    this,
+    &SweetRvizPanel::calculateErrors);
+
+timer_->start(100);
+}
+void SweetRvizPanel::initPlots()
+{
+  lateral_plot_->addGraph();
+
+  lateral_plot_->graph(0)->setName(
+    "Lateral Error");
+
+  lateral_plot_->xAxis->setLabel(
+    "Time (s)");
+
+  lateral_plot_->yAxis->setLabel(
+    "Lateral Error (m)");
+
+  lateral_plot_->xAxis->setRange(
+    0,
+    30);
+
+  lateral_plot_->yAxis->setRange(
+    -1,
+    1);
+  QPen lateral_pen(Qt::blue);
+  lateral_pen.setWidth(3);
+  lateral_plot_->graph(0)->setPen(lateral_pen);
+  lateral_plot_->legend->setVisible(true);
+
+
+  heading_plot_->addGraph();
+
+  heading_plot_->graph(0)->setName(
+    "Heading Error");
+
+  heading_plot_->xAxis->setLabel(
+    "Time (s)");
+
+  heading_plot_->yAxis->setLabel(
+    "Heading Error (rad)");
+
+  heading_plot_->xAxis->setRange(
+    0,
+    30);
+
+  heading_plot_->yAxis->setRange(
+    -0.5,
+    0.5);
+  QPen heading_pen(Qt::red);
+  heading_pen.setWidth(3);
+  heading_plot_->graph(0)->setPen(heading_pen);
+  heading_plot_->legend->setVisible(true);
+}
 void SweetRvizPanel::onInitialize()
 {
   node_ = std::make_shared<rclcpp::Node>("sweet_rviz_panel");
@@ -90,18 +153,18 @@ void SweetRvizPanel::onInitialize()
   }
   auto node = node_ptr_->get_raw_node();
 
-  pose_sub_ = node->create_subscription<
+  pose_sub_ = node->create_subscription<geometry_msgs::msg::Pose>(
-    geometry_msgs::msg::Pose>(
+    "/local_pose",10,
-    "/rtk_pose",
-    10,
     std::bind(&SweetRvizPanel::poseCallback,this,std::placeholders::_1));
   
-    twist_cmd_sub_ = node->create_subscription<
+  twist_cmd_sub_ = node->create_subscription<geometry_msgs::msg::Twist>(
-    geometry_msgs::msg::Twist>(
+    "/twist_cmd_vel",10,
-    "/twist_cmd_vel",
-    10,
     std::bind(&SweetRvizPanel::velCmdCallback,this,std::placeholders::_1));
   
+  odom_path_sub_ = node->create_subscription<nav_msgs::msg::Path>(
+    "/odom_path",10,
+    std::bind(&SweetRvizPanel::pathCallback,this,std::placeholders::_1));
+
   running_ = true;
   status_label_->setText("Status: RUNNING");
 }
@@ -115,7 +178,6 @@ void SweetRvizPanel::poseCallback(
 
   const double x = msg->position.x;
   const double y = msg->position.y;
-
   const auto &q = msg->orientation;
 
   tf2::Quaternion quaternion(
@@ -132,15 +194,14 @@ void SweetRvizPanel::poseCallback(
     roll,
     pitch,
     yaw);
+  pose_received_ = true;
+  current_x_ = x;
+  current_y_ = y;
+  current_yaw_ = 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));
   
-  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(
@@ -156,6 +217,147 @@ void SweetRvizPanel::velCmdCallback(
   yaw_cmd_label_->setText(
     QString("Yaw Command: %1 rad/s").arg(msg->angular.z, 0, 'f', 3));
 }
+void SweetRvizPanel::pathCallback(
+  const nav_msgs::msg::Path::SharedPtr msg)
+{
+  if (msg->poses.empty())
+    return;
+  current_path_ = *msg;
+  path_received_ = true;
+
+  calculateErrors();
+}
+void SweetRvizPanel::calculateErrors()
+{
+  if (!pose_received_ || !path_received_)
+    return;
+
+  if (current_path_.poses.empty())
+    return;
+  double min_distance =std::numeric_limits<double>::max();
+
+  int nearest_index = -1;
+
+  for (size_t i = start_index_;i < current_path_.poses.size() && 
+                              i < start_index_ + forward_index_;++i)
+  {
+    const auto &pose =current_path_.poses[i].pose;
+    const double dx =pose.position.x - current_x_;
+    const double dy =pose.position.y - current_y_;
+    const double distance =std::sqrt(dx * dx + dy * dy);
+
+    if (distance > search_distance_)
+      continue;
+
+    if (distance < min_distance)
+    {
+      min_distance = distance;
+      nearest_index =
+        static_cast<int>(i);
+      start_index_ = nearest_index;
+    }
+  }
+
+  if (nearest_index < 0)
+    return;
+
+  const auto &target =current_path_.poses[nearest_index].pose;
+  const auto &q =target.orientation;
+  tf2::Quaternion target_q(
+    q.x,
+    q.y,
+    q.z,
+    q.w);
+
+  double roll;
+  double pitch;
+  double target_yaw;
+
+  tf2::Matrix3x3(target_q).getRPY(
+    roll,
+    pitch,
+    target_yaw);
+
+  const double dx =current_x_ - target.position.x;
+  const double dy =current_y_ - target.position.y;
+
+  const double lateral_error =
+    -std::sin(target_yaw) * dx
+    + std::cos(target_yaw) * dy;
+
+  double heading_error =
+    current_yaw_ - target_yaw;
+
+  while (heading_error > M_PI)
+    heading_error -= 2.0 * M_PI;
+
+  while (heading_error < -M_PI)
+    heading_error += 2.0 * M_PI;
+
+  const double t =
+    error_timer_.elapsed() / 1000.0;
+
+  error_time_.push_back(t);
+  lateral_errors_.push_back(lateral_error);
+  heading_errors_.push_back(heading_error);
+
+  if (error_time_.size() > MAX_ERROR_POINTS)
+  {
+    error_time_.remove(0);
+    lateral_errors_.remove(0);
+    heading_errors_.remove(0);
+  }
+
+  updateErrorPlots();
+}
+
+void SweetRvizPanel::updateErrorPlots()
+{
+
+  if (!pose_received_) {
+    RCLCPP_INFO_THROTTLE(
+      node_->get_logger(),
+      *node_->get_clock(),
+      1000,
+      "NO POSE");
+    return;
+  }
+  if (error_time_.isEmpty())
+    return;
+
+  lateral_plot_->graph(0)->setData(error_time_,lateral_errors_);
+  heading_plot_->graph(0)->setData(error_time_,heading_errors_);
+
+  const double t_end =error_time_.last();
+  const double t_start = std::max(0.0, t_end - 30.0);
+
+  lateral_plot_->xAxis->setRange(t_start,std::max(t_end, t_start + 1.0));
+  heading_plot_->xAxis->setRange(t_start,std::max(t_end, t_start + 1.0));
+
+  double max_lateral = 0.1;
+
+  for (double e : lateral_errors_)
+  {
+    if (std::abs(e) > max_lateral)
+      max_lateral = std::abs(e);
+  }
+
+  lateral_plot_->yAxis->setRange(-max_lateral * 1.2,max_lateral * 1.2);
+
+  double max_heading = 0.01;
+  for (double e : heading_errors_)
+  {
+    if (std::abs(e) > max_heading)
+      max_heading = std::abs(e);
+  }
+
+  heading_plot_->yAxis->setRange(
+    -max_heading * 1.2,
+    max_heading * 1.2);
+
+  lateral_plot_->replot(QCustomPlot::rpQueuedReplot);
+  heading_plot_->replot(QCustomPlot::rpQueuedReplot);
+}
 
 void SweetRvizPanel::onStart()
 {