ROS激光PCA车道检测节点：无效Publisher调用错误求助

问题描述 #

基于PCA算法实现激光扫描数据车道检测的ROS节点，代码可通过catkin_make编译，但运行时出现Marker发布失败的致命错误，提示调用了无效的Publisher。

错误信息 #

[FATAL] [1682304485.986580762]: ASSERTION FAILED
    file = /opt/ros/noetic/include/ros/publisher.h
    line = 107
    cond = false
    message = 
[FATAL] [1682304485.989028993]: Call to publish() on an invalid Publisher
[FATAL] [1682304485.989061888]: 

节点代码 #

#include <iostream>
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <vector> 
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/PointCloud.h>
#include <laser_geometry/laser_geometry.h>
#include <tf/transform_listener.h>
#include <Eigen/Core>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include "ros/ros.h"
#include <pcl/common/pca.h>
#include <pcl/io/io.h>
#include <pcl/conversions.h> 
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <visualization_msgs/MarkerArray.h>
#include <pcl/io/pcd_io.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>

//using namespace std;
laser_geometry::LaserProjection projector_;
ros::Publisher pub_marker;

void getOrientation (const sensor_msgs::LaserScan::ConstPtr& scan)

{
    //convert laser scan into point clouds
    sensor_msgs::PointCloud2 cloud;
    projector_.projectLaser(*scan, cloud);

    //Construct a buffer used by the pca analysis
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud< pcl::PointXYZ>);   
    pcl::fromROSMsg (cloud, *cloud1);
    pcl::PCA<pcl::PointXYZ> pca;
    //pcl::PCA<PointT> pca;

    pca.setInputCloud(cloud1);
    Eigen::Vector3f eigen_values;
    eigen_values=pca.getEigenValues();
    Eigen::Matrix3f eigen_vector;
    eigen_vector=pca.getEigenVectors();

        /* 4. PCA Visualization */
    visualization_msgs::Marker points;
    points.header = cloud.header;
    points.header.frame_id = "/laser_frame"; // odom -> /base_link
    //points.header.frame_id = header.frame_id;
    points.header.stamp = cloud.header.stamp; // ros::Time::now() -> header.stamp
    
    points.ns = "pca"; // namespace + id
    points.id = 0; // pca/0
    points.action = visualization_msgs::Marker::ADD;
    points.type = visualization_msgs::Marker::ARROW;

    points.pose.orientation.w = 1.0;
    points.scale.x = 0.05;
    points.scale.y = 0.05;
    points.scale.z = 0.05;
    points.color.g = 1.0f;
    points.color.r = 0.0f;
    points.color.b = 0.0f;
    points.color.a = 1.0;

    geometry_msgs::Point p_0, p_1;
    p_0.x = 0; p_0.y = 0; p_0.z = 0; // get from tf
    p_1.x = eigen_vector(0,0);
    p_1.y = eigen_vector(0,1); // always negative
    std::cerr << "y = " << eigen_vector(0,1) << std::endl;
    //p_1.z = eigen_vector(0,2);
    points.points.push_back(p_0);
    points.points.push_back(p_1);
    pub_marker.publish(points);

}
 int main(int argc, char **argv)
 {
    
    // Initiate a new ROS node named "talker"
    ros::init(argc, argv, "lane_detection_pca_node");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("/scan", 100, getOrientation);
    ros::Publisher pub_marker = nh.advertise<visualization_msgs::Marker>("points", 100);
    ros::spin();
    return 0;
}

问题原因与解决方案 #

问题原因 #

代码中声明了全局的ros::Publisher pub_marker，但在main函数里又重新声明了一个同名的局部ros::Publisher pub_marker，局部变量覆盖了全局变量，导致全局的pub_marker从未被初始化（始终是无效状态），当回调函数getOrientation中调用pub_marker.publish(points)时就会触发报错。

解决方案 #

修改main函数中的Publisher初始化代码，去掉局部变量的声明，直接给全局pub_marker赋值：

修改后的main函数片段：

int main(int argc, char **argv)
{
    ros::init(argc, argv, "lane_detection_pca_node");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("/scan", 100, getOrientation);
    // 去掉局部变量声明，直接赋值给全局pub_marker
    pub_marker = nh.advertise<visualization_msgs::Marker>("points", 100);
    ros::spin();
    return 0;
}

这样全局的pub_marker会被正确初始化为有效的Publisher，回调函数中就能正常发布Marker消息了。

内容的提问来源于stack exchange，提问作者Bob9710