订阅和处理GNSS、IMU、LiDAR 和摄像头节点数据

1. GNSS接收机节点 gnss_receiver.cpp

#include "ros/ros.h"
#include "sensor_msgs/NavSatFix.h"

void gnssCallback(const sensor_msgs::NavSatFix::ConstPtr& msg) {
    ROS_INFO("GNSS Fix: Latitude: %f, Longitude: %f, Altitude: %f", msg->latitude, msg->longitude, msg->altitude);
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "gnss_receiver");
    ros::NodeHandle nh;

    ros::Subscriber sub = nh.subscribe("gnss_data", 10, gnssCallback);

    ros::spin();

    return 0;
}

2. IMU节点 imu_sensor.cpp

#include "ros/ros.h"
#include "sensor_msgs/Imu.h"

void imuCallback(const sensor_msgs::Imu::ConstPtr& msg) {
    ROS_INFO("IMU Data: Angular Velocity: [%f, %f, %f], Linear Acceleration: [%f, %f, %f]",
             msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z,
             msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z);
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "imu_sensor");
    ros::NodeHandle nh;

    ros::Subscriber sub = nh.subscribe("imu_data", 10, imuCallback);

    ros::spin();

    return 0;
}

3. LiDAR节点 lidar_driver.cpp

#include "ros/ros.h"
#include "sensor_msgs/PointCloud2.h"
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>

void lidarCallback(const sensor_msgs::PointCloud2::ConstPtr& msg) {
    pcl::PointCloud<pcl::PointXYZ> cloud;
    pcl::fromROSMsg(*msg, cloud);

    ROS_INFO("LiDAR Point Cloud: Number of points: %d", (int)cloud.points.size());
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "lidar_driver");
    ros::NodeHandle nh;

    ros::Subscriber sub = nh.subscribe("lidar_data", 10, lidarCallback);

    ros::spin();

    return 0;
}

4. 摄像头节点 camera_driver.cpp

#include "ros/ros.h"
#include "sensor_msgs/Image.h"
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>

void cameraCallback(const sensor_msgs::Image::ConstPtr& msg) {
    cv_bridge::CvImagePtr cv_ptr;
    try {
        cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    } catch (cv_bridge::Exception& e) {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    cv::imshow("Camera Image", cv_ptr->image);
    cv::waitKey(30);
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "camera_driver");
    ros::NodeHandle nh;

    cv::namedWindow("Camera Image");
    cv::startWindowThread();

    ros::Subscriber sub = nh.subscribe("camera_data", 10, cameraCallback);

    ros::spin();

    cv::destroyWindow("Camera Image");

    return 0;
}

编译和运行

1. 创建ROS包：
   如果还没有创建ROS包，可以使用以下命令创建：

   catkin_create_pkg my_sensors sensor_msgs cv_bridge pcl_ros
   

2. 添加源文件：
   将上述 .cpp 文件添加到 my_sensors/src/ 目录中。

3. 修改 CMakeLists.txt：
   在 CMakeLists.txt 文件中添加编译选项：

   add_executable(gnss_receiver src/gnss_receiver.cpp)
   target_link_libraries(gnss_receiver ${catkin_LIBRARIES})
   
   add_executable(imu_sensor src/imu_sensor.cpp)
   target_link_libraries(imu_sensor ${catkin_LIBRARIES})
   
   add_executable(lidar_driver src/lidar_driver.cpp)
   target_link_libraries(lidar_driver ${catkin_LIBRARIES})
   
   add_executable(camera_driver src/camera_driver.cpp)
   target_link_libraries(camera_driver ${catkin_LIBRARIES})
   

4. 编译：
   在 catkin_ws 目录下运行：

   catkin_make
   

5. 运行节点：
   按照之前的步骤，使用 rosrun 命令启动各个节点。