Velodyne Grabber 例子

慧拓

说明

链接激光雷达；新建一个catkin工程，cpp，cmake和package文件都在这个文档里。

catkin_make
source devel/setup.sh
rosrun grabber_test test_node

main.cpp

#include <iostream>
#include <sstream>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/hdl_grabber.h>
#include <pcl/conversions.h>
#include <pcl_conversions/pcl_conversions.h>
typedef pcl::PointXYZI PointType;
int main(int argc, char *argv[])
{
    ros::init(argc, argv, "memc_cloud");
    ros::Time::init();
    std::string ipaddress("192.168.1.201");
    std::string port("2368");
    pcl::PointCloud<PointType>::ConstPtr cloud_;
    boost::mutex mutex;
    boost::function<void(const pcl::PointCloud<PointType>::ConstPtr &)> function =
        [&cloud_, &mutex](const pcl::PointCloud<PointType>::ConstPtr &ptr) {
            boost::mutex::scoped_lock lock(mutex);
            // 这里实际得到了一帧数据
            cloud_ = ptr;
            ROS_INFO("<pcl::PointCloud> Point Count: %ld", cloud_->points.size());
            // 转换成 pcl::PointCloud2
            double start_time = ros::Time::now().toSec();
            pcl::PCLPointCloud2 pcl_pc2;
            toPCLPointCloud2(*cloud_, pcl_pc2);
            ROS_INFO("<pcl::PointCloud2> Data in Bytes: %ld", pcl_pc2.data.size());
            // 转换成 ROS Message
            //sensor_msgs::PointCloud2 msg;
            //pcl_conversions::fromPCL(pcl_pc2, msg);
            //ROS_INFO("<sensor_msgs::PointCloud2> Data in Bytes: %ld", msg.data.size());
        };
    // HDL Grabber
    boost::shared_ptr<pcl::HDLGrabber> grabber;
    std::cout << "Capture from Sensor..." << std::endl;
    grabber = boost::shared_ptr<pcl::HDLGrabber>(
        new pcl::HDLGrabber(
            boost::asio::ip::address::from_string(ipaddress),
            boost::lexical_cast<unsigned short>(port)));
    // Register Callback Function
    boost::signals2::connection connection = grabber->registerCallback(function);
    // Start Grabber
    grabber->start();
    // Just loop...
    while (ros::ok())
    {
    }
    // Stop Grabber
    grabber->stop();
    // Disconnect Callback Function
    if (connection.connected())
    {
        connection.disconnect();
    }
    return 0;
}

CMake

cmake_minimum_required(VERSION 2.8.3)
project(grabber_test)
add_compile_options(-std=c++11)
find_package(catkin REQUIRED COMPONENTS
  roscpp 
  pcl_ros
)
catkin_package(
)
###########
## Build ##
###########
include_directories(
  include
  ${catkin_INCLUDE_DIRS}
)
find_package(PCL 1.7 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
message(STATUS "PCL_INCLUDE_DIRS:" ${PCL_INCLUDE_DIRS})
message(STATUS "PCL_LIBRARY_DIRS:" ${PCL_LIBRARY_DIRS})
message(STATUS "PCL_DEFINITIONS:" ${PCL_DEFINITIONS})
find_package(OpenMP)
if (OPENMP_FOUND)
    set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
    set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
endif()
#############
## Install ##
#############
#############
## Testing ##
#############
add_executable(test_node src/main.cpp)
target_link_libraries(test_node ${catkin_LIBRARIES})

Package.xml

<?xml version="1.0"?>
<package format="2">
  <name>grabber_test</name>
  <version>0.0.0</version>
  <description>Fetch Point Cloud and Cache them into Memcached</description>
  <maintainer email="pigmon@todo.todo">pigmon</maintainer>
  <license>TODO</license>
  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>pcl_ros</build_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>pcl_ros</build_export_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>pcl_ros</exec_depend>
  <export>
  </export>
</package>