From 6281d2568b9bea9ccba193c557feb621bc36383f Mon Sep 17 00:00:00 2001 From: kennouni Date: Thu, 4 Jan 2018 08:30:31 -0500 Subject: [PATCH 1/3] initial commit --- velodyne_driver/src/driver/driver.cc | 5 + .../launch/VLP-32C_points.launch | 51 ++++++++++ .../params/VeloView-VLP-32C.yaml | 98 +++++++++++++++++++ 3 files changed, 154 insertions(+) create mode 100644 velodyne_pointcloud/launch/VLP-32C_points.launch create mode 100644 velodyne_pointcloud/params/VeloView-VLP-32C.yaml diff --git a/velodyne_driver/src/driver/driver.cc b/velodyne_driver/src/driver/driver.cc index fbb8678ad..03124dfcf 100644 --- a/velodyne_driver/src/driver/driver.cc +++ b/velodyne_driver/src/driver/driver.cc @@ -53,6 +53,11 @@ VelodyneDriver::VelodyneDriver(ros::NodeHandle node, packet_rate = 1808.0; model_full_name = std::string("HDL-") + config_.model; } + else if (config_.model == "32C") + { + packet_rate = 1808.0; + model_full_name = std::string("VLP-") + config_.model; + } else if (config_.model == "VLP16") { packet_rate = 754; // 754 Packets/Second for Last or Strongest mode 1508 for dual (VLP-16 User Manual) diff --git a/velodyne_pointcloud/launch/VLP-32C_points.launch b/velodyne_pointcloud/launch/VLP-32C_points.launch new file mode 100644 index 000000000..d745c269d --- /dev/null +++ b/velodyne_pointcloud/launch/VLP-32C_points.launch @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/velodyne_pointcloud/params/VeloView-VLP-32C.yaml b/velodyne_pointcloud/params/VeloView-VLP-32C.yaml new file mode 100644 index 000000000..fbd9b083a --- /dev/null +++ b/velodyne_pointcloud/params/VeloView-VLP-32C.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 From 29665186a17e329f3e6697d33715ff3e683e15a6 Mon Sep 17 00:00:00 2001 From: AutonomouStuff Developer Date: Thu, 4 Jan 2018 16:17:48 -0500 Subject: [PATCH 2/3] change readme --- README.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.rst b/README.rst index db6f1e185..a5470b1de 100644 --- a/README.rst +++ b/README.rst @@ -2,7 +2,7 @@ Overview ======== Velodyne_ is a collection of ROS_ packages supporting `Velodyne high -definition 3D LIDARs`_. +definition 3D LIDARs`_ . **Warning**:: From bf8f4d770cbd3937cff390ce673c50fc1f16d00d Mon Sep 17 00:00:00 2001 From: Autonomoustuff Developer Date: Thu, 4 Jan 2018 17:58:14 -0500 Subject: [PATCH 3/3] removed config file --- .../params/VeloView-VLP-32C.yaml | 98 ------------------- 1 file changed, 98 deletions(-) delete mode 100644 velodyne_pointcloud/params/VeloView-VLP-32C.yaml diff --git a/velodyne_pointcloud/params/VeloView-VLP-32C.yaml b/velodyne_pointcloud/params/VeloView-VLP-32C.yaml deleted file mode 100644 index fbd9b083a..000000000 --- a/velodyne_pointcloud/params/VeloView-VLP-32C.yaml +++ /dev/null @@ -1,98 +0,0 @@ -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