Linux 下轻量级显示程序imgui+ros

传感器过多的情况下，ros命令检测比较麻烦，直接运行显示界面，可直观查看传感器状态。

 依赖：ros+imgui 下载

下载：GitHub - ocornut/imgui at v1.81

 整体程序框架

程序源码

#include "imgui.h"
#include "imgui_impl_glfw.h"
#include "imgui_impl_opengl3.h"
#include <stdio.h>
#include <GL/gl3w.h>            // Initialize with gl3wInit()
// Include glfw3.h after our OpenGL definitions
#include <GLFW/glfw3.h>
#include <fstream>
#include "ros/ros.h"    //包含了使用ROS节点的必要文件
#include "std_msgs/String.h"    //包含了使用的数据类型
#include "std_msgs/UInt8MultiArray.h"
#include "std_msgs/MultiArrayDimension.h"
#include "std_msgs/Int8.h" //包含了使用的数据类型
#include "std_msgs/Int16MultiArray.h"
#include "std_msgs/Bool.h"
#include <sstream>
#include <geometry_msgs/TwistStamped.h>
#include <mutex> //定义了C++11标准中的一些互斥访问的类与方法
#include <image_transport/image_transport.h>
#include <sensor_msgs/PointCloud2.h>
#include "sensor_msgs/NavSatFix.h"
#include <cv_bridge/cv_bridge.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgcodecs.hpp>
#include <pcl_ros/transforms.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <sensor_msgs/Imu.h>
std::mutex loc_mutex;
typedef union{
    struct Pst{
            float rtk_x;
            float rtk_y;
            float rtk_z;
            float x;
            float y;
            float z;
            float roll;
            float pitch;
            float yaw;
    }Pst_data;
    uint8_t detail[36];
}Pst_union;
Pst_union loc_pst={0};
bool      is_camera_connected    = false;
bool      is_lidar_connected     = false;
bool      is_realsense_connected = false;
bool      is_odometry_connected  = false;
ros::Time lidar_callback_time;
ros::Time camera_callback_time;
ros::Time realsense_callback_time;
ros::Time odometry_callback_time;
float     cutgo_linear_x = 0;
float     cutgo_imu_acc_z= 0;

double    odom_hz=0;
const int odom_fps_data_num=100;
double    odom_fps[odom_fps_data_num+1]={0};
int       odom_fps_num=0;

double    rgb_hz=0;
const int rgb_fps_data_num=30;
double    rgb_fps[odom_fps_data_num+1]={0};
int       rgb_fps_num=0;

double    lidar_hz=0;
const int lidar_fps_data_num=30;
double    lidar_fps[odom_fps_data_num+1]={0};
int       lidar_fps_num=0;

double    realsense_hz=0;
const int realsense_fps_data_num=30;
double    realsense_fps[odom_fps_data_num+1]={0};
int       realsense_fps_num=0;

float rtk_x=0.00,rtk_y=0.00,rtk_rcv_x=0.00,rtk_rcv_y=0.00;
float rtk_pos[300][9]={0};
int count=-1,before_count=0;
static int e = 0;
std::ofstream Out_dirStream;

static void glfw_error_callback(int error, const char* description)
{
    fprintf(stderr, "Glfw Error %d: %s\n", error, description);
}

void Loc_Callback(const std_msgs::UInt8MultiArray::ConstPtr &msg){
    if(msg->data.size()>0)
    {  
      loc_mutex.lock();
      for(int i = 0; i < msg->data.size();i++){  //vec_loc size is 12
        loc_pst.detail[i]=msg->data[i];
      }
      loc_mutex.unlock();
      //std::cout<<"rtk_x position"<<loc_pst.Pst_data.rtk_x<<std::endl;
    } 
}

void imageCallback(const sensor_msgs::ImageConstPtr &msg) {
    ros::Time time_now     = ros::Time::now();
    if(rgb_fps_num<rgb_fps_data_num)
    {      
       rgb_fps_num++;
    }
    else
    {
        rgb_fps_num=0;
    }
    rgb_fps[rgb_fps_num]=(time_now - camera_callback_time).toSec();
    double fps_average=0;
    for(int i=0;i<rgb_fps_data_num;i++)
    {
        fps_average=fps_average+rgb_fps[i]/rgb_fps_data_num;
    }
    rgb_hz = 1.0 / fps_average;
    camera_callback_time = time_now;
    try {
        cv::Mat img = cv_bridge::toCvShare(msg, "bgr8")->image;
        //*if img exist, then show it
        if (img.data) {
            is_camera_connected = true;
        } else {
            is_camera_connected = false;
        }
    } catch (cv_bridge::Exception &e) {
        ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
    }
}

void realsenseCallback(const sensor_msgs::ImageConstPtr &msg) {
    ros::Time time_now     = ros::Time::now();
    if(realsense_fps_num<realsense_fps_data_num)
    {      
       realsense_fps_num++;
    }
    else
    {
        realsense_fps_num=0;
    }
    realsense_fps[realsense_fps_num]=(time_now - realsense_callback_time).toSec();
    double fps_average=0;
    for(int i=0;i<realsense_fps_data_num;i++)
    {
        fps_average=fps_average+realsense_fps[i]/realsense_fps_data_num;
    }
    realsense_hz = 1.0 / fps_average;
    realsense_callback_time = time_now;
    try {
        cv::Mat img = cv_bridge::toCvShare(msg, "bgr8")->image;
        if (img.data) {
            is_realsense_connected = true;
        } else {
            is_realsense_connected = false;
        }
    } catch (cv_bridge::Exception &e) {
        ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
    }
}

void lidarCallback(const sensor_msgs::PointCloud2ConstPtr &msg) {
    ros::Time time_now     = ros::Time::now();
    if(lidar_fps_num<lidar_fps_data_num)
    {      
       lidar_fps_num++;
    }
    else
    {
        lidar_fps_num=0;
    }
    lidar_fps[lidar_fps_num]=(time_now - lidar_callback_time).toSec();
    double fps_average=0;
    for(int i=0;i<lidar_fps_data_num;i++)
    {
        fps_average=fps_average+lidar_fps[i]/lidar_fps_data_num;
    }
    lidar_hz = 1.0 / fps_average;
    lidar_callback_time = time_now;
    pcl::PointCloud<pcl::PointXYZ>::Ptr mSphereCloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(*msg, *mSphereCloud);
    if (mSphereCloud->points.size() > 0||lidar_hz>5) {
        is_lidar_connected = true;
    } else {
        is_lidar_connected = false;
    }
}

void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)
{
    cutgo_imu_acc_z = imu_msg->linear_acceleration.z;
}

void odometryCallback(const geometry_msgs::TwistStampedConstPtr &msg) {
    ros::Time time_now     = ros::Time::now();
    if(odom_fps_num<odom_fps_data_num)
    {      
       odom_fps_num++;
    }
    else
    {
        odom_fps_num=0;
    }
    odom_fps[odom_fps_num]=(time_now - odometry_callback_time).toSec();
    double fps_average=0;
    for(int i=0;i<odom_fps_data_num;i++)
    {
        fps_average=fps_average+odom_fps[i]/odom_fps_data_num;
    }
    odom_hz = 1.0 / fps_average;
    odometry_callback_time = time_now;
    //printf("fps=%lf\n",odom_hz);
    if (odom_hz > 90) {
        is_odometry_connected = true;
    } else {
        is_odometry_connected = false;
    }
    cutgo_linear_x = msg->twist.linear.x;
}
void show_rtk_window()
{
    if(ImGui::Button("Add Point"))
        {
            count++;
            loc_mutex.lock();
            rtk_pos[count][0]=loc_pst.Pst_data.rtk_x;
            rtk_pos[count][1]=loc_pst.Pst_data.rtk_y;
            rtk_pos[count][2]=loc_pst.Pst_data.rtk_z;
            rtk_pos[count][3]=loc_pst.Pst_data.x;
            rtk_pos[count][4]=loc_pst.Pst_data.y;
            rtk_pos[count][5]=loc_pst.Pst_data.z;
            rtk_pos[count][6]=loc_pst.Pst_data.roll;
            rtk_pos[count][7]=loc_pst.Pst_data.pitch;
            rtk_pos[count][8]=loc_pst.Pst_data.yaw;
            loc_mutex.unlock();
        }
        ImGui::SameLine();
        if(ImGui::Button("Delete Point"))
        {
            count--;
        }
        ImGui::SameLine();
        if(ImGui::Button("Clear Points"))
        {
            count=0;
        }
        static char str0[128] = "/home/nvidia/positec/robot_logs/SLAM/shadow.txt";
        ImGui::SetNextItemWidth(500);
        ImGui::InputText("Input save path", str0, IM_ARRAYSIZE(str0));
        ImGui::PushItemWidth(500);
        std::string mSavePath;
        mSavePath = str0;
        // add a button; if click, save this test name
        if (ImGui::Button("save")) 
        {
            Out_dirStream.open(mSavePath, std::ios::out);
            Out_dirStream.close();
            Out_dirStream.open(mSavePath, std::ios::app);
            Out_dirStream.setf(std::ios::fixed,std::ios::floatfield);
            Out_dirStream.precision(9);
            for(int j=0;j<count+1;j++)
            {			
                Out_dirStream   << rtk_pos[j][0] << " " 
                                << rtk_pos[j][1] << " "
                                << rtk_pos[j][2] << " " 
                                << rtk_pos[j][3] << " "
                                << rtk_pos[j][4] << " " 
                                << rtk_pos[j][5] << " "
                                << rtk_pos[j][6] << " " 
                                << rtk_pos[j][7] << " "
                                << rtk_pos[j][8] << " "
                                << std::endl;		
            }
            Out_dirStream.close();
            std::cout << "mSavePath: " << mSavePath << std::endl;
        }
        loc_mutex.lock();
        ImGui::Text("Cur car pst %f %f %f %f %f %f %f %f %f",loc_pst.Pst_data.rtk_x,loc_pst.Pst_data.rtk_y,loc_pst.Pst_data.rtk_z,loc_pst.Pst_data.x,loc_pst.Pst_data.y,loc_pst.Pst_data.z,loc_pst.Pst_data.roll,loc_pst.Pst_data.pitch,loc_pst.Pst_data.yaw);
        ImGui::Text("-----------------------------------------------------------------------");
        loc_mutex.unlock();
        for(int i=0;i<count+1;i++)
        {
            ImGui::Text("points_num=%d,position %f %f %f %f %f %f %f %f %f",i, rtk_pos[i][0],rtk_pos[i][1],rtk_pos[i][2],rtk_pos[i][3],rtk_pos[i][4],rtk_pos[i][5],rtk_pos[i][6],rtk_pos[i][7],rtk_pos[i][8]);
        }
}
int main(int argc, char **argv)
{
    // Setup window
    glfwSetErrorCallback(glfw_error_callback);
    if (!glfwInit())
        return 1;
    // GL 3.0 + GLSL 130
    const char* glsl_version = "#version 130";
    glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
    glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 0);
    // Create window with graphics context
    //glfwWindowHint(GLFW_VISIBLE, GLFW_FALSE);
    GLFWwindow* window = glfwCreateWindow(1280, 720, "cut and go", NULL, NULL);
    if (window == NULL)
        return 1;
    glfwMakeContextCurrent(window);
    glfwSwapInterval(1); // Enable vsync

    // Initialize OpenGL loader
    bool err = gl3wInit() != 0;
    if (err)
    {
        fprintf(stderr, "Failed to initialize OpenGL loader!\n");
        return 1;
    }

    // Setup Dear ImGui context
    IMGUI_CHECKVERSION();
    ImGui::CreateContext();
    ImGuiIO& io = ImGui::GetIO(); (void)io;
    ImGui::StyleColorsDark();
    ImGui_ImplGlfw_InitForOpenGL(window, true);
    ImGui_ImplOpenGL3_Init(glsl_version);
    //ImVec4 clear_color = ImVec4(0.45f, 0.55f, 0.60f, 1.00f);
    ros::init(argc, argv, "node_gui");    //初始化ROS，节点名命名为node_a，节点名必须保持唯一
    ros::NodeHandle n;    //实例化节点, 节点进程句柄
    ros::Subscriber sub = n.subscribe("Fusion_pose_50hz", 1000, Loc_Callback);
    ros::Subscriber sub_camera = n.subscribe("/camera/rgb/image_raw", 1, imageCallback);
    ros::Subscriber sub_realsense = n.subscribe("/camera/color/image_raw", 1, realsenseCallback);
    ros::Subscriber sub_lidar = n.subscribe("/zvision_lidar_points", 1, lidarCallback);
    ros::Subscriber sub_odometry  = n.subscribe("/Ser_odom", 1000, odometryCallback);
    ros::Subscriber sub_imu = n.subscribe("/Ser_imu", 100,imu_callback);
    ros::Rate loop_rate(50);    //设置发送数据的频率为50Hz
    ImVec4 clear_color = ImVec4(0.45f, 0.55f, 0.60f, 1.00f);
    // Main loop
    while (!glfwWindowShouldClose(window)&&ros::ok())
    {
        glfwPollEvents();
        ImGui_ImplOpenGL3_NewFrame();
        ImGui_ImplGlfw_NewFrame();
        ImGui::NewFrame(); 
        ros::spinOnce();    //不是必须，若程序中订阅话题则必须，否则回掉函数不起作用。
        loop_rate.sleep();    //按前面设置的10Hz频率将程序挂起

        static float f = 0.0f;
        ImGui::Begin("RTK TEST");                          // Create a window called "Hello, world!" and append into it.
        if (abs(lidar_callback_time.toSec() - ros::Time::now().toSec()) < 1) 
        {
            if (!is_lidar_connected) 
            {
                ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(255, 0, 0, 255));
                ImGui::Text("Lidar connect fail");
                ImGui::PopStyleColor();

            } 
            else 
            {
                ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(0, 255, 0, 255));
                ImGui::Text("Lidar connect success");
                ImGui::PopStyleColor();
            }
        } 
        else 
        {
            ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(255, 0, 0, 255));
            ImGui::Text("Lidar connect fail");
            ImGui::PopStyleColor();
        }
        ImGui::SameLine();
        if (abs(camera_callback_time.toSec() - ros::Time::now().toSec()) < 1) {
            // is_camera_connected
            if (!is_camera_connected) {
                ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(255, 0, 0, 255));
                ImGui::Text(" ----- Camera connect fail");
                ImGui::PopStyleColor();
            } else {
                ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(0, 255, 0, 255));
                ImGui::Text(" ----- Camera connect success");
                ImGui::PopStyleColor();
            }
        } else {
            ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(255, 0, 0, 255));
            ImGui::Text(" ----- Camera connect fail");
            ImGui::PopStyleColor();
        }

        //* check back realsense is connected or not, two topics: /camera/color/image_raw /camera/depth/image_rect_raw
        ImGui::SameLine();
        if (abs(realsense_callback_time.toSec() - ros::Time::now().toSec()) < 1) {
            //* if is_realsense_connected false, show the text: realsense detection Failed
            if (!is_realsense_connected) {
                ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(255, 0, 0, 255));
                ImGui::Text(" ----- realsense connect fail");
                ImGui::PopStyleColor();
            } else {
                ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(0, 255, 0, 255));
                ImGui::Text(" ----- realsense connect success");
                ImGui::PopStyleColor();
            }
        } else {
            ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(255, 0, 0, 255));
            ImGui::Text(" ----- realsense connect fail");
            ImGui::PopStyleColor();
        }
        //* check odometry is connected or not, two topics: /Ser_odom
        ImGui::SameLine();
        if (abs(odometry_callback_time.toSec() - ros::Time::now().toSec()) < 1) {
            //* if is_odometry_connected false, show the text: odometry detection Failed
            if (!is_odometry_connected) {
                ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(255, 0, 0, 255));
                ImGui::Text(" ----- odometry connect fail");
                ImGui::PopStyleColor();
            } else {
                ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(0, 255, 0, 255));
                ImGui::Text(" ----- odometry connnect success");
                ImGui::PopStyleColor();
            }
        } else {
            ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(255, 0, 0, 255));
            ImGui::Text("   -----   odometry connect fail");
            ImGui::PopStyleColor();
        }
        ImGui::SameLine();
	    if (ImGui::Button("--Exit System--"))
	    {
	    	ros::shutdown();
	    }
        ImGui::Text(" ----- car    imu    fps %.3f", odom_hz);
        ImGui::Text(" ----- car   camera  fps %.3f", rgb_hz);
        ImGui::Text(" ----- car realsense fps %.3f", realsense_hz);
        ImGui::Text(" ----- car    lidar  fps %.3f", lidar_hz);
        ImGui::Text(" ----- car move  speed %.3f m/s", cutgo_linear_x);
        ImGui::Text(" ----- car imu  acc  z %.3f m2/s", cutgo_imu_acc_z);
        ImGui::Text("-------------------------------------------------------------------------------------------------------------");
        // bool ImGui::RadioButton(const char *label, int *v, int v_button);
        // IMGUI_DEMO_MARKER("Widgets/Basic/RadioButton");
        static int e = 0;
        ImGui::RadioButton("RTK_Shadow", &e, 0);
        ImGui::SameLine();
        ImGui::RadioButton("No Function", &e, 1);
        ImGui::Text("-------------------------------------------------------------------------------------------------------------");
        if (e == 0) {
            show_rtk_window();
        } else if (e == 1) {
            //show_dynamic_window();
        } 
        ImGui::End();
        // Rendering
        ImGui::Render();
        int display_w, display_h;
        glfwGetFramebufferSize(window, &display_w, &display_h);
        glViewport(0, 0, display_w, display_h);
        //glClearColor(clear_color.x * clear_color.w, clear_color.y * clear_color.w, clear_color.z * clear_color.w, clear_color.w);
        glClear(GL_COLOR_BUFFER_BIT);
        ImGui_ImplOpenGL3_RenderDrawData(ImGui::GetDrawData());
        glfwSwapBuffers(window);
    }
    // Cleanup
    ImGui_ImplOpenGL3_Shutdown();
    ImGui_ImplGlfw_Shutdown();
    ImGui::DestroyContext();
    glfwDestroyWindow(window);
    glfwTerminate();
    return 0;
}

 CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(gui_rtk)
find_package(catkin REQUIRED COMPONENTS roscpp std_msgs cv_bridge   sensor_msgs
  image_transport pcl_ros)
catkin_package()
find_package(glfw3 REQUIRED)
find_package(OpenGL REQUIRED)
find_package(OpenCV REQUIRED)

include_directories(${catkin_INCLUDE_DIRS})
include_directories(${OPENGL_INCLUDE_DIR})
include_directories(${OpenCV_INCLUDE_DIRS})
include_directories(${PROJECT_SOURCE_DIR}/imgui-1.83
	${PROJECT_SOURCE_DIR}/imgui-1.83/backends
)
include_directories(${CMAKE_CURRENT_SOURCE_DIR}/imgui-1.83/examples/libs/gl3w)
add_definitions(-DIMGUI_IMPL_OPENGL_LOADER_GL3W)
file (GLOB PLATFORM_SRC
        ./imgui-1.83/examples/libs/gl3w/GL/gl3w.*
        ./imgui-1.83/backends/imgui_impl_glfw.*
        ./imgui-1.83/backends/imgui_impl_opengl3.*
)
file(GLOB SRC
    ./imgui-1.83/*.h
    ./imgui-1.83/*.cpp
)

add_executable(gui_rtk
    src/gui_rtk.cpp
    ${SRC} ${PLATFORM_SRC}
)
target_link_libraries(gui_rtk ${catkin_LIBRARIES}
    glfw
    ${OPENGL_gl_LIBRARY}
    GL
    dl
    ${OpenCV_LIBS}
    ${PCL_LIBRARIES}
)