I want to visualization velodyne cloud point.
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();
}
Asked by victor_12 on 2019-10-28 04:00:39 UTC
Comments
Try increasing queue_size for both
velodyne_points
andaft_mapped_to_init
Asked by Choco93 on 2019-10-28 04:24:50 UTC
steal not working. I increased queue_size to 100.
Asked by victor_12 on 2019-10-28 04:40:44 UTC