I want to visualization velodyne cloud point.

asked 2019-10-28 04:00:39 -0600

victor_12 gravatar image

updated 2019-10-28 04:02:01 -0600

I want to visualization Velodyne cloud point. but my code doesn't work. waiting the first frame infinitely.please tell me why this happens.

this is my code

pcl::visualization::PCLVisualizer viewer("3D Viewer");

boost::mutex cloud_mutex;
enum{COLS=640,ROWS=480};
pcl::PointCloud<pcl::PointXYZ>::Ptr all_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr trans_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);

bool new_cloud_available_flag;
bool save_velo=false;
bool save_rt=false;

void keyboardCallback(const keyboard::KeyConstPtr& msg){
std::cout<<"recieve key_msg :"<<msg->code<<std::endl;
  if(msg->code==msg->KEY_SPACE){
    save_velo=true;
save_rt=true;
std::cout<<"capture key input"<<std::endl;
  }
}

void callback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg, const nav_msgs::OdometryConstPtr& odom_msg)
{
  cloud_mutex.lock();
  Eigen::Matrix3f R;
  Eigen::Quaternionf q;
  Eigen::Vector3f t;
  t.x()=float(odom_msg->pose.pose.position.x);
  t.y()=float(odom_msg->pose.pose.position.y);
  t.z()=float(odom_msg->pose.pose.position.z);
  q.x()=float(odom_msg->pose.pose.orientation.x);
  q.y()=float(odom_msg->pose.pose.orientation.y);
  q.z()=float(odom_msg->pose.pose.orientation.z);
  q.w()=float(odom_msg->pose.pose.orientation.w);

  sensor_msgs::PointCloud2 copy_cloud_msg=*cloud_msg;
  pcl::PointCloud<pcl::PointXYZ> cloud;
  pcl::fromROSMsg(copy_cloud_msg,cloud);
  *cloud_ptr=cloud;
  new_cloud_available_flag=true;
  pcl::transformPointCloud(*cloud_ptr,*trans_cloud_ptr,t,q);
  viewer.addPointCloud<pcl::PointXYZ>(cloud_ptr,"cloud");
  viewer.spinOnce();
  cloud_mutex.unlock();
}

int main(int argc, char **argv)
{

  viewer.setBackgroundColor(0,0,0);
  viewer.addCoordinateSystem(1.0);
  viewer.initCameraParameters();

  new_cloud_available_flag=false;
  save_velo=false;
  ros::init(argc, argv, "map_generater");
  ros::NodeHandle nh;


  message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_sub(nh,"/velodyne_points",1);
  message_filters::Subscriber<nav_msgs::Odometry> odom_sub(nh,"/aft_mapped_to_init",1);
  message_filters::TimeSynchronizer<sensor_msgs::PointCloud2, nav_msgs::Odometry> sync(cloud_sub, odom_sub, 10);
  sync.registerCallback(boost::bind(&callback, _1, _2));
  ros::Subscriber key_sub=nh.subscribe("/keyboard/keydown",10,keyboardCallback);


  while(!new_cloud_available_flag){
    std::cout<<"wating for first frame"<<std::endl;
    ros::Duration(0.1).sleep();
    ros::spinOnce();
  }

  ros::spin();
}
edit retag flag offensive close merge delete

Comments

Try increasing queue_size for both velodyne_points and aft_mapped_to_init

Choco93 gravatar imageChoco93 ( 2019-10-28 04:24:50 -0600 )edit

steal not working. I increased queue_size to 100.

victor_12 gravatar imagevictor_12 ( 2019-10-28 04:40:44 -0600 )edit