vendredi 31 août 2018

Using ICP in ROS Kinetic

I have written a program using class in ROS to perform ICP using a stored PCD file and the data being streamed from the LiDAR sensor.

I am getting the following error:

/usr/include/boost/smart_ptr/shared_ptr.hpp:641: typename boost::detail::sp_dereference::type boost::shared_ptr::operator*() const [with T = pcl::PointCloud; typename boost::detail::sp_dereference::type = pcl::PointCloud&]: Assertion `px != 0' failed. Aborted (core dumped)

Here's my code below

#include "ros/ros.h"
// PCL specific includes
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <icp.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>

using namespace ros;
using namespace std;
using namespace pcl;

class icp
{
private:
    NodeHandle n;
    Subscriber sub;
    Publisher pub = n.advertise<sensor_msgs::PointCloud2>("icp_topic",1);
    PointCloud<PointXYZ> stream_cloud;
    PointCloud<PointXYZ> pcd_cloud;
    PointCloud<PointXYZ> :: Ptr stream_cloudPTR;
    PointCloud<PointXYZ> :: Ptr pcd_cloudPTR;

public:

    icp()
    {

        PointCloud<PointXYZ> :: Ptr pcd_cloudPTR (new PointCloud<PointXYZ>);
        PointCloud<PointXYZ> :: Ptr stream_cloudPTR (new PointCloud<PointXYZ>);
        if(io::loadPCDFile<PointXYZ> ("~/PCD/2.pcd", *pcd_cloudPTR) == -1)
        {
            PCL_ERROR ("Couldn't read file\n");
            ROS_ERROR("Couldn't read file\n");
            return;
        }

        sub = n.subscribe("/apollo/sensor/velodyne64/compensator/PointCloud2", 1, &icp::read_stream, this);
    }
    void read_stream(const sensor_msgs::PointCloud2ConstPtr& msg)
    {
        //conversion from pcl_ros to pcl
        fromROSMsg(*msg, *stream_cloudPTR);

        // Perform ICP
        IterativeClosestPoint<PointXYZ, PointXYZ> icp;
        icp.setInputSource(stream_cloudPTR);
        icp.setInputTarget(pcd_cloudPTR);

        PointCloud<PointXYZ> Final;
        icp.align(Final);

        sensor_msgs::PointCloud2 output;
        toROSMsg( *stream_cloudPTR, output );

        pub.publish(output);
    }

};
int main(int argc, char **argv)
{
    init(argc, argv, "icp_module");
    icp icp_object;
    //multithreaded spinner-use 10 threads
    MultiThreadedSpinner spinner(10);
    spinner.spin();
    return 0;
}

Kindly help me in this regard and oblige. Thanks in advance.

Aucun commentaire:

Enregistrer un commentaire