输出激光点云

我们在一开始就给模拟的Velodyne雷达添加了控制扫描速度的接口，并在rviz中看到了输出的激光扫描数据。 其实当时的demo还有一些问题的。首先我们的传感器的输出频率有点低，只有30Hz，意味着每秒中只有\(30 \times 32\)个点输出(我们的雷达是32线的)。这与官方声称的每秒695000个点相比差距太大， 所以我们在模型文件中将雷达输出频率提高到了100Hz。 其次如果每次接收到雷达的扫描数据就发布一条消息，将导致系统过于频繁的进行通信，极大的损害了系统的效率。

所以这里我们修改了回调函数OnLaserScanMsg的内容，让他累积一段时间的扫描数据后再以点云的形式输出。下图是rivz中可视化的激光点云。

下面是修改之后的回调函数OnLaserScanMsg，代码比较直白不再解释。

        private: void OnLaserScanMsg(ConstLaserScanStampedPtr & msg)
        {
            gazebo::msgs::Pose const & pose_msg = msg->scan().world_pose();
            ignition::math::Pose3d pose = gazebo::msgs::ConvertIgn(pose_msg);
            Eigen::Quaterniond qua(pose.Rot().W(), pose.Rot().X(), pose.Rot().Y(), pose.Rot().Z());

            int count = msg->scan().count();
            double angle_min = msg->scan().angle_min();
            double angle_step = msg->scan().angle_step();
            double range_min = msg->scan().range_min();
            double range_max = msg->scan().range_max();

            for (int i = 0; i < count; i++) {
                double range = msg->scan().ranges(i);
                if (std::isinf(range) || range > range_max || range < range_min)
                    continue;

                double angle = angle_min + angle_step * i;
                Eigen::Vector3d p(range * cos(angle), range * sin(angle), 0);
                p = qua.matrix() * p;
                pcl::PointXYZ point(p[0] + pose.Pos().X(), p[1] + pose.Pos().Y(), p[2] + pose.Pos().Z());
                mPointCloud.push_back(point);  
            }
            mPCCount++;

            if (mPCCount > 100) {
                sensor_msgs::PointCloud2 pc_msg;
                pcl::toROSMsg(mPointCloud, pc_msg);
                pc_msg.header.stamp = ros::Time(msg->time().sec(), msg->time().nsec());
                pc_msg.header.frame_id = "base";
                this->mRosPointCloudPub.publish(pc_msg);

                mPCCount = 0;
                mPointCloud.clear();
            }
        }