ubuntu下使用ROS驱动相机，halcon进行点云3D匹配

最近一个项目使用了Ensenso N35双目相机，由于需要使用ROS进行机械臂的路径规划，所以准备直接用ROS驱动相机，利用ROS-HALCON数据转换模块将点云处理成Halcon的HObjectModel3D对象，再进行3D物体的识别。

环境：Ubuntu16.04 ,ROS kinetic, halcon18, EnsensoSDK2.3

Ensenso相机的ROS驱动：http://wiki.ros.osuosl.org/ensenso_driver

ROS-halcon数据转换模块：https://github.com/asr-ros/asr_halcon_bridge

步骤一：驱动Ensenso相机，将点云发布在/point_cloud话题下

打开一个终端：$ roscore

另一个终端：打开相机

$ rosrun ensenso_camera ensenso_camera_node

另一个终端：请求数据

$ rosrun ensenso_camera request_data

步骤二：新建一个ros node，订阅/point_cloud话题，将得到的sensor_msgs::PointCloud2对象，转换成halcon的HObjectModel3D对象：

具体实现的部分代码：

#include <ros/ros.h>  
#include <geometry_msgs/Twist.h>
#include <std_msgs/String.h>
#include<ros/subscriber.h>

#include "HalconCpp.h"
#include "HObjectModel3D.h"
#include "HSurfaceModel.h"
#include "HSurfaceMatchingResult.h"
 
#include <ros/package.h>
#include <ros/console.h>

#include <halcon_pointcloud.h>
#ifndef __APPLE__
#  include "HalconCpp.h"
#  include "HDevThread.h"
#  if defined(__linux__) && (defined(__i386__) || defined(__x86_64__)) \
                         && !defined(NO_EXPORT_APP_MAIN)
#  endif
#else
#  ifndef HC_LARGE_IMAGES
#    include <HALCONCpp/HalconCpp.h>
#    include <HALCONCpp/HDevThread.h>
#  else
#    include <HALCONCppxl/HalconCpp.h>
#    include <HALCONCppxl/HDevThread.h>
#  endif
#  include <stdio.h>
#  include <HALCON/HpThread.h>
#  include <CoreFoundation/CFRunLoop.h>
#endif
s
using namespace std;
using namespace ros;
using namespace HalconCpp;

void estimated_pose_callback(const sensor_msgs::PointCloud2 &model)
{

  // Local iconic variables

  halcon_bridge::HalconPointcloudPtr Ptr;

  HObjectModel3D  hv_SceneObject;
  Ptr = halcon_bridge::toHalconCopy(model);

  hv_SceneObject =*Ptr->model;
  //hv_SceneObject就是我们得到的Halcon对象，接下来可以利用3D匹配算法进行识别
  //需要注意的一点是，halcon转为C++代码一般对象和函数参数为HTuple，而我们得到的是HObjectModel3D 
  //我们需要将部分函数替换成HObjectModel3D.h文件夹下的函数
}

int main(int argc,char **argv)
{
  ros::init(argc, argv, "ros_halcon");
  ros::NodeHandle nh;
  //subscribed_topic

  ros::Subscriber poseSub =nh.subscribe("/point_cloud", 1000, estimated_pose_callback);


  ROS_INFO ("Wating for data.\n");

  ros::spin();
 
  ROS_INFO ("All done!\n");
  return 0;
}