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