liuhaoyan
/
robots_rviz_panel
1
0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

sweet ai rviz panel

This commit is contained in:
liuhaoyan 2026-08-24 09:58:08 +08:00
parent 9b80189710
commit dffea57921
5 changed files with 347 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

74
CMakeLists.txt Normal file
View File

@ -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()

63
include/sweet_rviz_panel/sweet_rviz_panel.hpp Normal file
View File

@ -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

24
package.xml Normal file
View File

@ -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>

10
plugin_description.xml Normal file
View File

@ -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>

176
src/sweet_rviz_panel.cpp Normal file
View File

@ -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)