[feature] vlc32c driver, velodyne drivers updated (autowarefoundation…

…#1166) * Squashed 'ros/src/sensing/drivers/lidar/packages/velodyne/' changes from 776a358..1a70413 1a70413 Merge branch 'master' into Autoware 7976d12 support vlp32c now 273520e Added hdl32c, fixed naming e21b522 Merge pull request autowarefoundation#146 from stsundermann/patch-2 0e5a200 Merge pull request autowarefoundation#150 from ros-drivers/mikaelarguedas-patch-1 db6b5ee update to use non deprecated pluginlib macro 560fe12 Use std::abs instead of fabsf git-subtree-dir: ros/src/sensing/drivers/lidar/packages/velodyne git-subtree-split: 1a704135c529c5d2995cd2c1972ca4f59d5ae1ad * Squashed 'ros/src/sensing/drivers/lidar/packages/velodyne/' changes from 1a70413..52c0a0d 52c0a0d README format git-subtree-dir: ros/src/sensing/drivers/lidar/packages/velodyne git-subtree-split: 52c0a0d63594ee71a156755954d240d24966829e * Squashed 'ros/src/sensing/drivers/lidar/packages/velodyne/' changes from 52c0a0d..a1d6f18 a1d6f18 Update and rename README.rst to README.md git-subtree-dir: ros/src/sensing/drivers/lidar/packages/velodyne git-subtree-split: a1d6f186d3340f3ce5059e234ed7e3dcb828d09d
NuronLabs · Mar 19, 2018 · 3a8d8d3 · 3a8d8d3
1 parent 1e977aa
commit 3a8d8d3
Show file tree

Hide file tree

Showing 11 changed files with 199 additions and 10 deletions.
diff --git a/...rivers/lidar/packages/velodyne/README.rst → ...drivers/lidar/packages/velodyne/README.md b/...rivers/lidar/packages/velodyne/README.rst → ...drivers/lidar/packages/velodyne/README.md
@@ -7,12 +7,16 @@ This repo adds support to HDL-64 S3 and creates the launch files used by Autowar
 If you need to modify **any** file inside this folder structure, please use the following commands to either push or fetch changes from the subtree.
 All the commands written here will suppose you're in the root of Autoware path.
 
-#### Pulling in commits from the repository subtree 
+## Pulling in commits from the repository subtree 
+
 Bring latest commits from https://github.com/CPFL/velodyne
+
 `git subtree pull --prefix ros/src/sensing/drivers/lidar/packages/velodyne https://github.com/CPFL/velodyne Autoware --squash`
 
-#### Pushing changes to the repository subtree 
+## Pushing changes to the repository subtree 
+
 If you made any modification to the subtree you are encouraged to commit and publish your changes to the fork. You can do with the following command.
+
 `git subtree push --prefix ros/src/sensing/drivers/lidar/packages/velodyne https://github.com/CPFL/velodyne Autoware`
 
 **End of Section**

diff --git a/ros/src/sensing/drivers/lidar/packages/velodyne/velodyne_driver/src/driver/driver.cc b/ros/src/sensing/drivers/lidar/packages/velodyne/velodyne_driver/src/driver/driver.cc
@@ -60,7 +60,7 @@ VelodyneDriver::VelodyneDriver(ros::NodeHandle node,
     }
     else if (config_.model == "32C")
     {
-      packet_rate = 1808.0;
+      packet_rate = 1507.0;
       model_full_name = std::string("VLP-") + config_.model;
     }
   else if (config_.model == "VLP16")

diff --git a/ros/src/sensing/drivers/lidar/packages/velodyne/velodyne_laserscan/src/VelodyneLaserScan.cpp b/ros/src/sensing/drivers/lidar/packages/velodyne/velodyne_laserscan/src/VelodyneLaserScan.cpp
@@ -96,7 +96,7 @@ void VelodyneLaserScan::recvCallback(const sensor_msgs::PointCloud2ConstPtr& msg
 
   // Construct LaserScan message
   if ((offset_x >= 0) && (offset_y >= 0) && (offset_r >= 0)) {
-    const float RESOLUTION = fabsf(cfg_.resolution);
+    const float RESOLUTION = std::abs(cfg_.resolution);
     const size_t SIZE = 2.0 * M_PI / RESOLUTION;
     sensor_msgs::LaserScanPtr scan(new sensor_msgs::LaserScan());
     scan->header = msg->header;

diff --git a/ros/src/sensing/drivers/lidar/packages/velodyne/velodyne_laserscan/src/nodelet.cpp b/ros/src/sensing/drivers/lidar/packages/velodyne/velodyne_laserscan/src/nodelet.cpp
@@ -21,4 +21,4 @@ class LaserScanNodelet: public nodelet::Nodelet
 
 }
 
-PLUGINLIB_DECLARE_CLASS(velodyne_laserscan, LaserScanNodelet, velodyne_laserscan::LaserScanNodelet, nodelet::Nodelet);
+PLUGINLIB_EXPORT_CLASS(velodyne_laserscan::LaserScanNodelet, nodelet::Nodelet);
diff --git a/...drivers/lidar/packages/velodyne/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h b/...drivers/lidar/packages/velodyne/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h
@@ -146,7 +146,7 @@ namespace velodyne_rawdata
      */
     int setupOffline(std::string calibration_file, double max_range_, double min_range_);
 
-    void unpack(const velodyne_msgs::VelodynePacket &pkt, VPointCloud &pc);
+    void unpack(const velodyne_msgs::VelodynePacket &pkt, VPointCloud &pc, int packets_num);
 
     void setParameters(double min_range, double max_range, double view_direction,
                        double view_width);

diff --git a/ros/src/sensing/drivers/lidar/packages/velodyne/velodyne_pointcloud/launch/32c_points.launch b/ros/src/sensing/drivers/lidar/packages/velodyne/velodyne_pointcloud/launch/32c_points.launch
@@ -0,0 +1,28 @@
+<!-- -*- mode: XML -*- -->
+<!-- run velodyne_pointcloud/CloudNodelet in a nodelet manager for an HDL-32E
+
+     arg: calibration = path to calibration file (default: standard 32db.yaml)
+          pcap = path to packet capture file (default: use real device)
+
+     $Id$
+  -->
+
+<launch>
+<remap from="velodyne_points" to="points_raw"/>
+
+  <!-- declare arguments with default values -->
+  <arg name="pcap" default="" />
+  <arg name="calibration" default="$(find velodyne_pointcloud)/params/32c-db.yaml"/>
+
+  <!-- start nodelet manager and driver nodelets -->
+  <include file="$(find velodyne_driver)/launch/nodelet_manager.launch">
+    <arg name="model" value="32C"/>
+    <arg name="pcap" value="$(arg pcap)"/>
+  </include>
+
+  <!-- start cloud nodelet -->
+  <include file="$(find velodyne_pointcloud)/launch/cloud_nodelet.launch">
+    <arg name="calibration" value="$(arg calibration)"/>
+  </include>
+
+</launch>
diff --git a/...sensing/drivers/lidar/packages/velodyne/velodyne_pointcloud/launch/velodyne_hdl32c.launch b/...sensing/drivers/lidar/packages/velodyne/velodyne_pointcloud/launch/velodyne_hdl32c.launch
@@ -0,0 +1,53 @@
+<!-- -*- mode: XML -*- -->
+<!-- run velodyne_pointcloud/CloudNodelet in a nodelet manager for an VLP-32C -->
+
+<launch>
+
+  <!-- declare arguments with default values -->
+  <arg name="calibration" default="$(find velodyne_pointcloud)/params/VeloView-VLP-32C.yaml"/>
+  <arg name="device_ip" default="" />
+  <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="port" default="2368" />
+  <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="laserscan_ring" default="-1" />
+  <arg name="laserscan_resolution" default="0.007" />
+  <arg name="topic_name" default="points_raw"/>
+
+  <!-- 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="$(arg frame_id)"/>
+    <arg name="manager" value="$(arg manager)" />
+    <arg name="model" value="32C"/>
+    <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)"/>
+  </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)"/>
+    <remap from="velodyne_points" to="$(arg topic_name)"/>
+  </include>
+
+  <!-- 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>
diff --git a/ros/src/sensing/drivers/lidar/packages/velodyne/velodyne_pointcloud/params/32c-db.yaml b/ros/src/sensing/drivers/lidar/packages/velodyne/velodyne_pointcloud/params/32c-db.yaml
@@ -0,0 +1,98 @@
+lasers:
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 0, rot_correction: -0.024434609527920613,
+  vert_correction: -0.4363323129985824, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 1, rot_correction: 0.07330382858376185,
+  vert_correction: -0.017453292519943295, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 2, rot_correction: -0.024434609527920613,
+  vert_correction: -0.029094638630745476, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 3, rot_correction: 0.024434609527920613,
+  vert_correction: -0.2729520417193932, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 4, rot_correction: -0.024434609527920613,
+  vert_correction: -0.19739673840055869, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 5, rot_correction: 0.024434609527920613,
+  vert_correction: 0.0, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 6, rot_correction: -0.07330382858376185,
+  vert_correction: -0.011641346110802179, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 7, rot_correction: 0.024434609527920613,
+  vert_correction: -0.15433946575385857, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 8, rot_correction: -0.024434609527920613,
+  vert_correction: -0.12660618393966866, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 9, rot_correction: 0.07330382858376185,
+  vert_correction: 0.005811946409141118, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 10, rot_correction: -0.024434609527920613,
+  vert_correction: -0.005811946409141118, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 11, rot_correction: 0.024434609527920613,
+  vert_correction: -0.10730284241261137, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 12, rot_correction: -0.07330382858376185,
+  vert_correction: -0.0930784090088576, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 13, rot_correction: 0.024434609527920613,
+  vert_correction: 0.02326523892908441, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 14, rot_correction: -0.07330382858376185,
+  vert_correction: 0.011641346110802179, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 15, rot_correction: 0.024434609527920613,
+  vert_correction: -0.06981317007977318, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 16, rot_correction: -0.024434609527920613,
+  vert_correction: -0.08145451619057535, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 17, rot_correction: 0.07330382858376185,
+  vert_correction: 0.029094638630745476, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 18, rot_correction: -0.024434609527920613,
+  vert_correction: 0.017453292519943295, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 19, rot_correction: 0.07330382858376185,
+  vert_correction: -0.06400122367063206, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 20, rot_correction: -0.07330382858376185,
+  vert_correction: -0.058171823968971005, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 21, rot_correction: 0.024434609527920613,
+  vert_correction: 0.058171823968971005, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 22, rot_correction: -0.024434609527920613,
+  vert_correction: 0.04071853144902771, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 23, rot_correction: 0.024434609527920613,
+  vert_correction: -0.04654793115068877, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 24, rot_correction: -0.024434609527920613,
+  vert_correction: -0.05235987755982989, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 25, rot_correction: 0.024434609527920613,
+  vert_correction: 0.12217304763960307, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 26, rot_correction: -0.024434609527920613,
+  vert_correction: 0.08145451619057535, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 27, rot_correction: 0.07330382858376185,
+  vert_correction: -0.04071853144902771, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 28, rot_correction: -0.07330382858376185,
+  vert_correction: -0.03490658503988659, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 29, rot_correction: 0.024434609527920613,
+  vert_correction: 0.2617993877991494, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 30, rot_correction: -0.024434609527920613,
+  vert_correction: 0.18034487160857407, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 31, rot_correction: 0.024434609527920613,
+  vert_correction: -0.02326523892908441, vert_offset_correction: 0.0}
+num_lasers: 32
diff --git a/...rc/sensing/drivers/lidar/packages/velodyne/velodyne_pointcloud/src/conversions/convert.cc b/...rc/sensing/drivers/lidar/packages/velodyne/velodyne_pointcloud/src/conversions/convert.cc
@@ -69,7 +69,7 @@ namespace velodyne_pointcloud
     // process each packet provided by the driver
     for (size_t i = 0; i < scanMsg->packets.size(); ++i)
       {
-        data_->unpack(scanMsg->packets[i], *outMsg);
+        data_->unpack(scanMsg->packets[i], *outMsg, scanMsg->packets.size());
       }
 
     // publish the accumulated cloud message

diff --git a/.../sensing/drivers/lidar/packages/velodyne/velodyne_pointcloud/src/conversions/transform.cc b/.../sensing/drivers/lidar/packages/velodyne/velodyne_pointcloud/src/conversions/transform.cc
@@ -91,7 +91,7 @@ namespace velodyne_pointcloud
         pcl_conversions::toPCL(header, inPc_.header);
 
         // unpack the raw data
-        data_->unpack(scanMsg->packets[next], inPc_);
+        data_->unpack(scanMsg->packets[next], inPc_, scanMsg->packets.size());
 
         // clear transform point cloud for this packet
         tfPc_.points.clear();           // is this needed?

diff --git a/ros/src/sensing/drivers/lidar/packages/velodyne/velodyne_pointcloud/src/lib/rawdata.cc b/ros/src/sensing/drivers/lidar/packages/velodyne/velodyne_pointcloud/src/lib/rawdata.cc
@@ -144,7 +144,7 @@ namespace velodyne_rawdata
    *  @param pc shared pointer to point cloud (points are appended)
    */
   void RawData::unpack(const velodyne_msgs::VelodynePacket &pkt,
-                       VPointCloud &pc)
+                       VPointCloud &pc, int packets_num)
   {
     ROS_DEBUG_STREAM("Received packet, time: " << pkt.stamp);
 
@@ -190,7 +190,13 @@ namespace velodyne_rawdata
              ||(config_.min_angle > config_.max_angle 
              && (raw->blocks[i].rotation <= config_.max_angle 
              || raw->blocks[i].rotation >= config_.min_angle))){
-          float distance = tmp.uint * DISTANCE_RESOLUTION;
+
+          float distance;
+          if (packets_num==(int) ceil(1507.0 / 10))
+            distance = tmp.uint * DISTANCE_RESOLUTION * 2;
+          else
+            distance = tmp.uint * DISTANCE_RESOLUTION;
+
           distance += corrections.dist_correction;
 
           float cos_vert_angle = corrections.cos_vert_correction;
Original file line number	Diff line number	Diff line change
Expand Up		@@ -21,4 +21,4 @@ class LaserScanNodelet: public nodelet::Nodelet

		}

		PLUGINLIB_DECLARE_CLASS(velodyne_laserscan, LaserScanNodelet, velodyne_laserscan::LaserScanNodelet, nodelet::Nodelet);
		PLUGINLIB_EXPORT_CLASS(velodyne_laserscan::LaserScanNodelet, nodelet::Nodelet);