Velodyne 多雷達的網絡設置和座標對齊

 

1.路由器查查看IP地址

 

2.找到對應IP後從路由器進入Velodyne界面

 

找到需要我們設置的地方

 

圖中的Host 對應開機時自動向該地址廣播的設備驅動地址，自動向192.168.1.155的局域網IP廣播數據，此處的Data Port記下，每個雷達的IP不同端口也最好不同

上圖對應雷達自身IP，設置完之後最好關閉DHCP

 

雷達端設置並不多，主要是記下幾個需要在ROS驅動launch和cfg文件設置的參數，並且把各個帶有IP地址的設備PING通

3.單個Velodyne 的ROS包設置

在官或者ros下載最新Velodyne驅動包，並且按照常規設置先驅動一個雷達，看之前的內容是否生效

 

- name: /multi_lidar_calibrator publish: [/points_calibrated] subscribe: [/lidar*/points_raw]

• 單VLP16的launch文件：

• <!-- -*- mode: XML -*- -->
  <!-- run velodyne_pointcloud/CloudNodelet in a nodelet manager for a VLP-16 -->
  
  <launch>
  
    <!-- declare arguments with default values -->
    <arg name="calibration" default="$(find velodyne_pointcloud)/params/VLP16db.yaml"/> 
    
    將剛纔對應的局域網IP地址填入
    <arg name="device_ip" default="192.168.1.201" />
    對應的frame
    <arg name="frame_id" default="base_laser" />
    <arg name="manager" default="$(arg frame_id)_nodelet_manager" />
    <arg name="max_range" default="130.0" />
    <arg name="min_range" default="0.4" />
    <arg name="pcap" default="" />
    <arg name="port" default="8369" />
    <arg name="read_fast" default="false" />
    <arg name="read_once" default="false" />
    <arg name="repeat_delay" default="0.0" />
    <arg name="rpm" default="600.0" />
    <arg name="gps_time" default="false" />
    <arg name="cut_angle" default="-0.01" />
    <arg name="timestamp_first_packet" default="false" />
    <arg name="laserscan_ring" default="-1" />
    <arg name="laserscan_resolution" default="0.007" />
    <arg name="organize_cloud" default="false" />
  
    <!-- start nodelet manager and driver nodelets -->
    <include file="$(find velodyne_driver)/launch/nodelet_manager.launch">
      <arg name="device_ip" value="$(arg device_ip)"/>
      <arg name="frame_id" value="base_laser"/>
      <arg name="manager" value="$(arg manager)" />
      <arg name="model" value="VLP16"/>
      <arg name="pcap" value="$(arg pcap)"/>
      <arg name="port" value="$(arg port)"/>
      <arg name="read_fast" value="$(arg read_fast)"/>
      <arg name="read_once" value="$(arg read_once)"/>
      <arg name="repeat_delay" value="$(arg repeat_delay)"/>
      <arg name="rpm" value="$(arg rpm)"/>
      <arg name="gps_time" value="$(arg gps_time)"/>
      <arg name="cut_angle" value="$(arg cut_angle)"/>
      <arg name="timestamp_first_packet" value="$(arg timestamp_first_packet)"/>
    </include>
  
    <!-- start cloud nodelet -->
    <include file="$(find velodyne_pointcloud)/launch/cloud_nodelet.launch">
      <arg name="calibration" value="$(arg calibration)"/>
      <arg name="manager" value="$(arg manager)" />
      <arg name="max_range" value="$(arg max_range)"/>
      <arg name="min_range" value="$(arg min_range)"/>
      <arg name="organize_cloud" value="$(arg organize_cloud)"/>
    </include>
  啓動laser_scan,發佈單線掃描結果
    <!-- start laserscan nodelet -->
    <include file="$(find velodyne_pointcloud)/launch/laserscan_nodelet.launch">
      <arg name="manager" value="$(arg manager)" />
      <arg name="ring" value="$(arg laserscan_ring)"/>
      <arg name="resolution" value="$(arg laserscan_resolution)"/>
    </include>
  
  </launch>

   

• 打開RVIZ 查看（白色VLP6，中間第九條爲提取後的/scan）

•  

   

   

  4.兩個多個Velodyne 的ROS包設置

  launch文件：

• <!-- -*- mode: XML -*- -->
  <!-- run velodyne_pointcloud/CloudNodelet in a nodelet manager for a VLP-16 -->
  <!-- Run two Velodyne VLP-16 LiDARs -->
  
  <launch>
     通過group ns 命名空間 區分開多個Lidar
  	<!-- Name lidar devices -->
  	<arg name="lidar1_name" default="lidar_parent" />
  	<arg name="lidar2_name" default="lidar_child" />
  
  
  <!-- FOR first VLP16 -->
   	<group ns="$(arg lidar1_name)">
    <!-- hardware parameters -->
      <arg name="calibration" default="$(find velodyne_pointcloud)/params/VLP16db.yaml"/>
      填入剛纔記下的IP和端口號，也可以自定義Frame
      <arg name="device_ip" default="192.168.1.202" />
      <arg name="port" default="8370" />
      <arg name="frame_id" default="velodyne" />
      <arg name="manager" default="$(arg frame_id)_nodelet_manager" />
      <arg name="max_range" default="130.0" />
      <arg name="min_range" default="0.4" />
      <arg name="pcap" default="" />
      <arg name="read_fast" default="false" />
      <arg name="read_once" default="false" />
      <arg name="repeat_delay" default="0.0" />
      <arg name="rpm" default="600.0" />
      <arg name="cut_angle" default="-0.01" />
      <arg name="laserscan_ring" default="-1" />
      <arg name="laserscan_resolution" default="0.007" />
  
    <!-- start nodelet manager and driver nodelets -->
      <include file="$(find velodyne_driver)/launch/nodelet_manager.launch">
        <arg name="device_ip" value="192.168.1.202"/>
        <arg name="frame_id" value="velodyne"/>
        <arg name="manager" value="$(arg frame_id)_nodelet_manager" />
        <arg name="model" value="VLP16"/>
        <arg name="pcap" value="$(arg pcap)"/>
        <arg name="port" value="$(arg port)"/>
        <arg name="read_fast" value="$(arg read_fast)"/>
        <arg name="read_once" value="$(arg read_once)"/>
        <arg name="repeat_delay" value="$(arg repeat_delay)"/>
        <arg name="rpm" value="$(arg rpm)"/>
        <arg name="cut_angle" value="$(arg cut_angle)"/>
      </include>
  
    <!-- start cloud nodelet -->
      <include file="$(find velodyne_pointcloud)/launch/cloud_nodelet.launch">
        <arg name="calibration" value="$(arg calibration)"/>
        <arg name="manager" value="$(arg frame_id)_nodelet_manager" />
        <arg name="max_range" value="$(arg max_range)"/>
        <arg name="min_range" value="$(arg min_range)"/>
      </include>
    </group>
  
   對之後的VLP也同樣的設置
  <!--For VELODYNE 02 -->
  
  	<group ns="$(arg lidar2_name)">
    <!-- declare arguments with default values -->
      <arg name="calibration" default="$(find velodyne_pointcloud)/params/VLP16db.yaml"/>
      <arg name="device_ip" default="192.168.1.201" />
      <arg name="port" default="8369" />
      <arg name="frame_id" default="velodyne" />
      <arg name="manager" default="$(arg frame_id)_nodelet_manager" />
      <arg name="max_range" default="130.0" />
      <arg name="min_range" default="0.4" />
      <arg name="pcap" default="" />
      <arg name="read_fast" default="false" />
      <arg name="read_once" default="false" />
      <arg name="repeat_delay" default="0.0" />
      <arg name="rpm" default="600.0" />
      <arg name="cut_angle" default="-0.01" />
      <arg name="laserscan_ring" default="-1" />
      <arg name="laserscan_resolution" default="0.007" />
  
    <!-- start nodelet manager and driver nodelets -->
      <include file="$(find velodyne_driver)/launch/nodelet_manager.launch">
        <arg name="device_ip" value="192.168.1.201"/>
        <arg name="frame_id" value="velodyne"/>
        <arg name="manager" value="$(arg frame_id)_nodelet_manager" />
        <arg name="model" value="VLP16"/>
        <arg name="pcap" value="$(arg pcap)"/>
        <arg name="port" value="$(arg port)"/>
        <arg name="read_fast" value="$(arg read_fast)"/>
        <arg name="read_once" value="$(arg read_once)"/>
        <arg name="repeat_delay" value="$(arg repeat_delay)"/>
        <arg name="rpm" value="$(arg rpm)"/>
        <arg name="cut_angle" value="$(arg cut_angle)"/>
      </include>
  
    <!-- start cloud nodelet -->
      <include file="$(find velodyne_pointcloud)/launch/cloud_nodelet.launch">
        <arg name="calibration" value="$(arg calibration)"/>
        <arg name="manager" value="$(arg frame_id)_nodelet_manager" />
        <arg name="max_range" value="$(arg max_range)"/>
        <arg name="min_range" value="$(arg min_range)"/>
      </include>
    </group>
  
  </launch>

   

• RVIZ

•  

   

• 兩顆雷達實際位姿：

•  

• 可以看到雷達之間還沒設置位姿變換，兩禎之間有距離間隔
• 參數設置在同一座標系下，接收兩個不同的自定義話題
• 節點接收來自多個話題的激光雷達數據，發佈一個外部校準的一個點雲話題

5.代碼

#include "multi_lidar_calibrator.h"


void ROSMultiLidarCalibratorApp::PublishCloud(const ros::Publisher& in_publisher, pcl::PointCloud<PointT>::ConstPtr in_cloud_to_publish_ptr)
{
	sensor_msgs::PointCloud2 cloud_msg;
	pcl::toROSMsg(*in_cloud_to_publish_ptr, cloud_msg);
	cloud_msg.header.frame_id = parent_frame_;
	in_publisher.publish(cloud_msg);
}

void ROSMultiLidarCalibratorApp::PointsCallback(const sensor_msgs::PointCloud2::ConstPtr &in_parent_cloud_msg,
                                                  const sensor_msgs::PointCloud2::ConstPtr &in_child_cloud_msg)
{
	pcl::PointCloud<PointT>::Ptr in_parent_cloud(new pcl::PointCloud<PointT>);
	pcl::PointCloud<PointT>::Ptr in_child_cloud(new pcl::PointCloud<PointT>);

	pcl::PointCloud<PointT>::Ptr child_filtered_cloud (new pcl::PointCloud<PointT>);

	pcl::fromROSMsg(*in_parent_cloud_msg, *in_parent_cloud);
	pcl::fromROSMsg(*in_child_cloud_msg, *in_child_cloud);

	parent_frame_ = in_parent_cloud_msg->header.frame_id;
	child_frame_ = in_child_cloud_msg->header.frame_id;

	DownsampleCloud(in_child_cloud, child_filtered_cloud, voxel_size_);

	// Initializing Normal Distributions Transform (NDT).
	pcl::NormalDistributionsTransform<PointT, PointT> ndt;

	ndt.setTransformationEpsilon(ndt_epsilon_);
	ndt.setStepSize(ndt_step_size_);
	ndt.setResolution(ndt_resolution_);

	ndt.setMaximumIterations(ndt_iterations_);

	ndt.setInputSource(child_filtered_cloud);
	ndt.setInputTarget(in_parent_cloud);

	pcl::PointCloud<PointT>::Ptr output_cloud(new pcl::PointCloud<PointT>);

	Eigen::Translation3f init_translation(initial_x_, initial_y_, initial_z_);
	Eigen::AngleAxisf init_rotation_x(initial_roll_, Eigen::Vector3f::UnitX());
	Eigen::AngleAxisf init_rotation_y(initial_pitch_, Eigen::Vector3f::UnitY());
	Eigen::AngleAxisf init_rotation_z(initial_yaw_, Eigen::Vector3f::UnitZ());

	Eigen::Matrix4f init_guess_ = (init_translation * init_rotation_z * init_rotation_y * init_rotation_x).matrix();

	if(current_guess_ == Eigen::Matrix4f::Identity())
	{
		current_guess_ = init_guess_;
	}

	ndt.align(*output_cloud, current_guess_);

	std::cout << "Normal Distributions Transform converged:" << ndt.hasConverged ()
	          << " score: " << ndt.getFitnessScore () << " prob:" << ndt.getTransformationProbability() << std::endl;

	std::cout << "transformation from " << child_frame_ << " to " << parent_frame_ << std::endl;

	// Transforming unfiltered, input cloud using found transform.
	pcl::transformPointCloud (*in_child_cloud, *output_cloud, ndt.getFinalTransformation());

	current_guess_ = ndt.getFinalTransformation();

	Eigen::Matrix3f rotation_matrix = current_guess_.block(0,0,3,3);
	Eigen::Vector3f translation_vector = current_guess_.block(0,3,3,1);
	std::cout << "This transformation can be replicated using:" << std::endl;
	std::cout << "rosrun tf static_transform_publisher " << translation_vector.transpose()
	          << " " << rotation_matrix.eulerAngles(2,1,0).transpose() << " /" << parent_frame_
	          << " /" << child_frame_ << " 10" << std::endl;

	std::cout << "Corresponding transformation matrix:" << std::endl
	          << std::endl << current_guess_ << std::endl << std::endl;

	PublishCloud(calibrated_cloud_publisher_, output_cloud);
	// timer end
	//auto end = std::chrono::system_clock::now();
	//auto usec = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();
	//std::cout << "time: " << usec / 1000.0 << " [msec]" << std::endl;

}

/*void ROSMultiLidarCalibratorApp::InitialPoseCallback(geometry_msgs::PoseWithCovarianceStamped::ConstPtr in_initialpose)
{
	ROS_INFO("[%s] Initial Pose received.", __APP_NAME__);
	tf::Quaternion pose_quaternion(in_initialpose->pose.pose.orientation.x,
	                 in_initialpose->pose.pose.orientation.y,
	                 in_initialpose->pose.pose.orientation.z,
	                 in_initialpose->pose.pose.orientation.w);

	//rotation
	initialpose_quaternion_ = pose_quaternion;

	//translation
	initialpose_position_.setX(in_initialpose->pose.pose.position.x);
	initialpose_position_.setY(in_initialpose->pose.pose.position.y);
	initialpose_position_.setZ(in_initialpose->pose.pose.position.z);


}*/

void ROSMultiLidarCalibratorApp::DownsampleCloud(pcl::PointCloud<PointT>::ConstPtr in_cloud_ptr,
                                                 pcl::PointCloud<PointT>::Ptr out_cloud_ptr,
                                                 double in_leaf_size)
{
	pcl::VoxelGrid<PointT> voxelized;
	voxelized.setInputCloud(in_cloud_ptr);
	voxelized.setLeafSize((float)in_leaf_size, (float)in_leaf_size, (float)in_leaf_size);
	voxelized.filter(*out_cloud_ptr);
}

void ROSMultiLidarCalibratorApp::InitializeROSIo(ros::NodeHandle &in_private_handle)
{
	//get params
	std::string points_parent_topic_str, points_child_topic_str;
	std::string initial_pose_topic_str = "/initialpose";
	std::string calibrated_points_topic_str = "/points_calibrated";

	in_private_handle.param<std::string>("points_parent_src", points_parent_topic_str, "points_raw");
	ROS_INFO("[%s] points_parent_src: %s",__APP_NAME__, points_parent_topic_str.c_str());

	in_private_handle.param<std::string>("points_child_src", points_child_topic_str, "points_raw");
	ROS_INFO("[%s] points_child_src: %s",__APP_NAME__, points_child_topic_str.c_str());

	in_private_handle.param<double>("voxel_size", voxel_size_, 0.1);
	ROS_INFO("[%s] ndt_epsilon: %.2f",__APP_NAME__, voxel_size_);

	in_private_handle.param<double>("ndt_epsilon", ndt_epsilon_, 0.01);
	ROS_INFO("[%s] voxel_size: %.2f",__APP_NAME__, ndt_epsilon_);

	in_private_handle.param<double>("ndt_step_size", ndt_step_size_, 0.1);
	ROS_INFO("[%s] ndt_step_size: %.2f",__APP_NAME__, ndt_step_size_);

	in_private_handle.param<double>("ndt_resolution", ndt_resolution_, 1.0);
	ROS_INFO("[%s] ndt_resolution: %.2f",__APP_NAME__, ndt_resolution_);

	in_private_handle.param<int>("ndt_iterations", ndt_iterations_, 400);
	ROS_INFO("[%s] ndt_iterations: %d",__APP_NAME__, ndt_iterations_);

	in_private_handle.param<double>("x", initial_x_, 0.0);
	in_private_handle.param<double>("y", initial_y_, 0.0);
	in_private_handle.param<double>("z", initial_z_, 0.0);
	in_private_handle.param<double>("roll", initial_roll_, 0.0);
	in_private_handle.param<double>("pitch", initial_pitch_, 0.0);
	in_private_handle.param<double>("yaw", initial_yaw_, 0.0);

	ROS_INFO("[%s] Initialization Transform x: %.2f y: %.2f z: %.2f roll: %.2f pitch: %.2f yaw: %.2f", __APP_NAME__,
	         initial_x_, initial_y_, initial_z_,
	         initial_roll_, initial_pitch_, initial_yaw_);

	//generate subscribers and synchronizer
	cloud_parent_subscriber_ = new message_filters::Subscriber<sensor_msgs::PointCloud2>(node_handle_,
	                                                                                     points_parent_topic_str, 10);
	ROS_INFO("[%s] Subscribing to... %s",__APP_NAME__, points_parent_topic_str.c_str());

	cloud_child_subscriber_ = new message_filters::Subscriber<sensor_msgs::PointCloud2>(node_handle_,
	                                                                                        points_child_topic_str, 10);
	ROS_INFO("[%s] Subscribing to... %s",__APP_NAME__, points_child_topic_str.c_str());

	/*initialpose_subscriber_ = node_handle_.subscribe(initial_pose_topic_str, 10,
	                                                          &ROSMultiLidarCalibratorApp::InitialPoseCallback, this);
	ROS_INFO("[%s] Subscribing to... %s",__APP_NAME__, initial_pose_topic_str.c_str());*/

	calibrated_cloud_publisher_ = node_handle_.advertise<sensor_msgs::PointCloud2>(calibrated_points_topic_str, 1);
	ROS_INFO("[%s] Publishing PointCloud to... %s",__APP_NAME__, calibrated_points_topic_str.c_str());

	cloud_synchronizer_ =
			new message_filters::Synchronizer<SyncPolicyT>(SyncPolicyT(100),
			                                               *cloud_parent_subscriber_,
			                                               *cloud_child_subscriber_);
	cloud_synchronizer_->registerCallback(boost::bind(&ROSMultiLidarCalibratorApp::PointsCallback, this, _1, _2));

}


void ROSMultiLidarCalibratorApp::Run()
{
	ros::NodeHandle private_node_handle("~");

	InitializeROSIo(private_node_handle);

	ROS_INFO("[%s] Ready. Waiting for data...",__APP_NAME__);

	ros::spin();

	ROS_INFO("[%s] END",__APP_NAME__);
}

ROSMultiLidarCalibratorApp::ROSMultiLidarCalibratorApp()
{
	//initialpose_quaternion_ = tf::Quaternion::getIdentity();
	current_guess_ = Eigen::Matrix4f::Identity();
}

說明：

Multi LiDAR Calibrator

This package allows to obtain the extrinsic calibration between two PointClouds with the help of the NDT algorithm.

The multi_lidar_calibrator node receives two PointCloud2 messages (parent and child), and an initialization pose.

If possible, the transformation required to transform the child to the parent point cloud is calculated, and output to the terminal.

How to launch

1. You'll need to provide an initial guess, otherwise the transformation won't converge.
2. In a sourced terminal:

Using rosrun

rosrun multi_lidar_calibrator multi_lidar_calibrator _points_child_src:=/lidar_child/points_raw _points_parent_src:=/lidar_parent/points_raw _x:=0.0 _y:=0.0 _z:=0.0 _roll:=0.0 _pitch:=0.0 _yaw:=0.0

Using roslaunch

roslaunch multi_lidar_calibrator multi_lidar_calibrator points_child_src:=/lidar_child/points_raw points_parent_src:=/lidar_parent/points_raw x:=0.0 y:=0.0 z:=0.0 roll:=0.0 pitch:=0.0 yaw:=0.0

1. Play a rosbag with both lidar data /lidar_child/points_raw and /lidar_parent/points_raw
2. The resulting transformation will be shown in the terminal as shown in the Output section.
3. Open RViz and set the fixed frame to the Parent
4. Add both point cloud /lidar_parent/points_raw and /points_calibrated
5. If the algorithm converged, both PointClouds will be shown in rviz.

Input topics

Parameter	Type	Description
points_parent_src	String	PointCloud topic name to subscribe and synchronize with the child.
points_child_src	String	PointCloud topic name to subscribe and synchronize with the parent.
voxel_size	double	Size of the Voxel used to downsample the CHILD pointcloud. Default: 0.5
ndt_epsilon	double	The transformation epsilon in order for an optimization to be considered as having converged to the final solution. Default: 0.01
ndt_step_size	double	Set/change the newton line search maximum step length. Default: 0.1
ndt_resolution	double	Size of the Voxel used to downsample the PARENT pointcloud. Default: 1.0
ndt_iterations	double	The maximum number of iterations the internal optimization should run for. Default: 400
x	double	Initial Guess of the transformation x. Meters
y	double	Initial Guess of the transformation y. Meters
z	double	Initial Guess of the transformation z. Meters
roll	double	Initial Guess of the transformation roll. Radians
pitch	double	Initial Guess of the transformation pitch. Radians
yaw	double	Initial Guess of the transformation yaw. Radians

Output

1. Child Point cloud transformed to the Parent frame and published in /points_calibrated.
2. Output in the terminal showing the X,Y,Z,Yaw,Pitch,Roll transformation between child and parent. These values can be used later with the static_transform_publisher.

 

 

 

以上爲對齊後效果