ROS PCL visualizer fails on Turtlebot
Hello,
I am trying to run PCL visualizer on the turtlebot's computer such that it will subscribe to the Kinect and display a point cloud, and request three floor points from the mouse. It will use this to create and visualize a point cloud that detects people. This is from a PCL tutorial. The code I have used to run this is below. When I run the code on a separate computer that is directly connected to a Kinect, the visualizer runs fine. However, when I run it on the turtlebot, the initial point cloud is displayed, but after I input the three points and run the second point cloud, it exits and displays the following error:
Illegal instruction (core dumped)
Any thoughts? Here is the code.
Thank you!
#include <iostream>
#include <cstdio>
#include <cstdlib>
#include <ros/ros.h>
#include <pcl/console/parse.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/people/ground_based_people_detection_app.h>
#include <pcl/common/time.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloudT;
// PCL viewer //
pcl::visualization::PCLVisualizer viewer("PCL Viewer");
// Mutex: //
boost::mutex cloud_mutex;
bool new_cloud_available_flag;
enum { COLS = 640, ROWS = 480 };
int print_help()
{
cout << "*******************************************************" << std::endl;
cout << "Ground based people detection app options:" << std::endl;
cout << " --help <show_this_help>" << std::endl;
cout << " --svm <path_to_svm_file>" << std::endl;
cout << " --conf <minimum_HOG_confidence (default = -1.5)>" << std::endl;
cout << " --min_h <minimum_person_height (default = 1.3)>" << std::endl;
cout << " --max_h <maximum_person_height (default = 2.3)>" << std::endl;
cout << " --sample <sampling_factor (default = 1)>" << std::endl;
cout << "*******************************************************" <<std::endl;
}
PointCloudT::Ptr cloud(new PointCloudT);
void callback(const sensor_msgs::PointCloud2ConstPtr& msg){
cloud_mutex.lock();
sensor_msgs::PointCloud2 msg0 = *msg;
PointCloudT cloud0;
pcl::fromROSMsg(msg0, cloud0);
*cloud = cloud0;
new_cloud_available_flag = true;
cloud_mutex.unlock();
}
struct callback_args{
// structure used to pass arguments to the callback function
PointCloudT::Ptr clicked_points_3d;
pcl::visualization::PCLVisualizer::Ptr viewerPtr;
};
void
pp_callback (const pcl::visualization::PointPickingEvent& event, void* args)
{
struct callback_args* data = (struct callback_args *)args;
if (event.getPointIndex () == -1)
return;
PointT current_point;
event.getPoint(current_point.x, current_point.y, current_point.z);
data->clicked_points_3d->points.push_back(current_point);
// Draw clicked points in red:
pcl::visualization::PointCloudColorHandlerCustom<PointT> red (data->clicked_points_3d, 255, 0, 0);
data->viewerPtr->removePointCloud("clicked_points");
data->viewerPtr->addPointCloud(data->clicked_points_3d, red, "clicked_points");
data->viewerPtr->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10, "clicked_points");
std::cout << current_point.x << " " << current_point.y << " " << current_point.z << std::endl;
}
int main (int argc, char** argv)
{
ros::init(argc, argv, "viewer_node");
ros::NodeHandle n;
new_cloud_available_flag = false;
ros::Subscriber sub = n.subscribe<sensor_msgs::PointCloud2>("/camera/depth_registered/points", 1000, callback);
if(pcl::console::find_switch (argc, argv, "--help") || pcl::console::find_switch (argc, argv, "-h"))
return print_help();
// Algorithm parameters:
std::string svm_filename = "/usr/share/doc/libpcl-1.7-doc/doc/pcl-1.7/tutorials/sources/ground_based_rgbd_people_detection/data/trainedLinearSVMForPeopleDetectionWithHOG.yaml";
float min_confidence = -1.5;
float min_height = 1.3;
float max_height = 2.3;
float voxel_size = 0.06;
float sampling_factor = 1;
Eigen::Matrix3f rgb_intrinsics_matrix;
rgb_intrinsics_matrix << 525, 0.0, 319.5, 0.0, 525, 239.5, 0.0, 0.0, 1.0; // Kinect RGB camera intrinsics ...
Hello, I tried to test this code in my computer and I got the following error:
free(): invalid pointer: 0x0000000000742ba0 *
Did you have any problem similiar to this?
My computer supports SSE2. Could this be a problem?