94 lines
2.4 KiB
C++
94 lines
2.4 KiB
C++
#ifndef SWEET_RVIZ_PANEL_HPP
|
|
#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
|
|
{
|
|
|
|
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);
|
|
void pathCallback(const nav_msgs::msg::Path::SharedPtr msg);
|
|
void calculateData();
|
|
void updateDataPlot();
|
|
void initPlots();
|
|
|
|
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_;
|
|
QTimer *timer_;
|
|
QCustomPlot *lateral_plot_;
|
|
QCustomPlot *heading_plot_;
|
|
QCustomPlot *yaw_cmd_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_;
|
|
QVector<double> yaw_cmd_;
|
|
QElapsedTimer error_timer_;
|
|
static constexpr int MAX_ERROR_POINTS = 110;
|
|
static constexpr int TIME_LIMIT = 10;
|
|
double search_distance_ = 10.0;
|
|
int start_index_=0;
|
|
int forward_index_ = 100;
|
|
|
|
double cur_vel_cmd_ = 0.0;
|
|
double cur_yaw_cmd_ = 0.0;
|
|
bool running_;
|
|
};
|
|
|
|
} // namespace sweet_rviz_panel
|
|
|
|
#endif
|