diff --git a/velodyne_lidar/.Rhistory b/velodyne_lidar/.Rhistory deleted file mode 100644 index e69de29b..00000000 diff --git a/velodyne_lidar/LICENSE b/velodyne_lidar/LICENSE index 2a34f4d1..9bd97f8a 100644 --- a/velodyne_lidar/LICENSE +++ b/velodyne_lidar/LICENSE @@ -1,6 +1,6 @@ Software License Agreement (BSD License) -Copyright (c) 2015-2017, Dataspeed Inc. +Copyright (c) 2015-2018, Dataspeed Inc. All rights reserved. Redistribution and use in source and binary forms, with or without modification, diff --git a/velodyne_lidar/README.md b/velodyne_lidar/README.md index ebe5f6f7..8771ba7a 100644 --- a/velodyne_lidar/README.md +++ b/velodyne_lidar/README.md @@ -5,12 +5,15 @@ URDF description and Gazebo plugins to simulate Velodyne laser scanners # Features * URDF with colored meshes -* Gazebo plugin based on [gazeboo_plugins/gazebo_ros_block_laser](https://github.com/ros-simulation/gazebo_ros_pkgs/blob/kinetic-devel/gazebo_plugins/src/gazebo_ros_block_laser.cpp) +* Gazebo plugin based on [gazebo_plugins/gazebo_ros_block_laser](https://github.com/ros-simulation/gazebo_ros_pkgs/blob/kinetic-devel/gazebo_plugins/src/gazebo_ros_block_laser.cpp) * Publishes PointCloud2 with same structure (x, y, z, intensity, ring) * Simulated Gaussian noise +* GPU acceleration ([with a modern Gazebo build](gazebo_upgrade.md)) * Supported models: * [VLP-16](velodyne_description/urdf/VLP-16.urdf.xacro) * [HDL-32E](velodyne_description/urdf/HDL-32E.urdf.xacro) + * Pull requests for other models are welcome +* Experimental support for clipping low-intensity returns # Parameters * ```*origin``` URDF transform from parent link. @@ -18,16 +21,20 @@ URDF description and Gazebo plugins to simulate Velodyne laser scanners * ```name``` URDF model name. Also used as tf frame_id for PointCloud2 output. Default ```velodyne``` * ```topic``` PointCloud2 output topic name. Default ```/velodyne_points``` * ```hz``` Update rate in hz. Default ```10``` -* ```lasers``` Number of vertical spinning lasers. Default ```VLP-16: 16, HDL-32E: 33``` +* ```lasers``` Number of vertical spinning lasers. Default ```VLP-16: 16, HDL-32E: 32``` * ```samples``` Nuber of horizontal rotating samples. Default ```VLP-16: 1875, HDL-32E: 2187``` * ```min_range``` Minimum range value in meters. Default ```0.9``` * ```max_range``` Maximum range value in meters. Default ```130.0``` * ```noise``` Gausian noise value in meters. Default ```0.008``` * ```min_angle``` Minimum horizontal angle in radians. Default ```-3.14``` * ```max_angle``` Maximum horizontal angle in radians. Default ```3.14``` +* ```gpu``` Use gpu_ray sensor instead of the standard ray sensor. Default ```false``` +* ```min_intensity``` The minimum intensity beneath which returns will be clipped. Can be used to remove low-intensity objects. # Known Issues * At full sample resolution, Gazebo can take up to 30 seconds to load the VLP-16 pluggin, 60 seconds for the HDL-32E +* With the default Gazebo version shipped with ROS, ranges are incorrect when accelerated with the GPU option ([issue](https://bitbucket.org/osrf/gazebo/issues/946/),[image](img/gpu.png)) + * Solution: Upgrade to a [modern Gazebo version](gazebo_upgrade.md) * Gazebo cannot maintain 10Hz with large pointclouds * Solution: User can reduce number of points (samples) or frequency (hz) in the urdf parameters, see [example.urdf.xacro](velodyne_description/urdf/example.urdf.xacro) * Gazebo crashes when updating HDL-32E sensors with default number of points. "Took over 1.0 seconds to update a sensor." @@ -37,3 +44,7 @@ URDF description and Gazebo plugins to simulate Velodyne laser scanners # Example Gazebo Robot ```roslaunch velodyne_description example.launch``` + +# Example Gazebo Robot (with GPU) +```roslaunch velodyne_description example.launch gpu:=true``` + diff --git a/velodyne_lidar/bitbucket-pipelines.yml b/velodyne_lidar/bitbucket-pipelines.yml index 214c58b4..bf4b0862 100644 --- a/velodyne_lidar/bitbucket-pipelines.yml +++ b/velodyne_lidar/bitbucket-pipelines.yml @@ -3,12 +3,50 @@ image: osrf/ros:kinetic-desktop-full pipelines: default: - - step: - script: - - mkdir -p /tmp/src/repo && mv `pwd`/* /tmp/src/repo && mv /tmp/src `pwd` # Move everything into the src directory - - source `find /opt/ros -name setup.bash | sort | head -1` && echo $ROS_DISTRO # Source ROS environment - - apt update && rosdep install --from-paths src --ignore-src -y # Install dependencies missing from the docker image - - catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release - - catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release --make-args tests - - catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release --make-args run_tests - - catkin_test_results build_isolated \ No newline at end of file + - parallel: + - step: + name: kinetic + image: osrf/ros:kinetic-desktop-full + script: + - mkdir -p /tmp/src/repo && mv `pwd`/* /tmp/src/repo && mv /tmp/src `pwd` # Move everything into the src directory + - source `find /opt/ros -name setup.bash | sort | head -1` && echo $ROS_DISTRO # Source ROS environment + - apt update && rosdep install --from-paths src --ignore-src -y # Install dependencies missing from the docker image + - catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release + - catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release --make-args tests + - catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release --make-args run_tests -j1 + - catkin_test_results build_isolated + - step: + name: lunar + image: osrf/ros:lunar-desktop-full + script: + - mkdir -p /tmp/src/repo && mv `pwd`/* /tmp/src/repo && mv /tmp/src `pwd` # Move everything into the src directory + - source `find /opt/ros -name setup.bash | sort | head -1` && echo $ROS_DISTRO # Source ROS environment + - apt update && rosdep install --from-paths src --ignore-src -y # Install dependencies missing from the docker image + - catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release + - catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release --make-args tests + - catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release --make-args run_tests -j1 + - catkin_test_results build_isolated + - step: + name: melodic + image: osrf/ros:melodic-desktop-full + script: + - mkdir -p /tmp/src/repo && mv `pwd`/* /tmp/src/repo && mv /tmp/src `pwd` # Move everything into the src directory + - source `find /opt/ros -name setup.bash | sort | head -1` && echo $ROS_DISTRO # Source ROS environment + - apt update && rosdep install --from-paths src --ignore-src -y # Install dependencies missing from the docker image + - catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release + - catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release --make-args tests + - catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release --make-args run_tests -j1 + - catkin_test_results build_isolated + branches: + gazebo2: + - step: + name: indigo + image: osrf/ros:indigo-desktop-full + script: + - mkdir -p /tmp/src/repo && mv `pwd`/* /tmp/src/repo && mv /tmp/src `pwd` # Move everything into the src directory + - source `find /opt/ros -name setup.bash | sort | head -1` && echo $ROS_DISTRO # Source ROS environment + - apt update && rosdep install --from-paths src --ignore-src -y # Install dependencies missing from the docker image + - catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release + - catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release --make-args tests + - catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release --make-args run_tests -j1 + - catkin_test_results build_isolated diff --git a/velodyne_lidar/gazebo_upgrade.md b/velodyne_lidar/gazebo_upgrade.md new file mode 100644 index 00000000..cf947b1e --- /dev/null +++ b/velodyne_lidar/gazebo_upgrade.md @@ -0,0 +1,14 @@ +# GPU issues +The GPU problems reported in this [issue](https://bitbucket.org/osrf/gazebo/issues/946/) have been solved with this [pull request](https://bitbucket.org/osrf/gazebo/pull-requests/2955/) for the ```gazebo7``` branch. The Gazebo versions from the ROS apt repository (7.0.0 for Kinetic, 9.0.0 for Melodic) do not have this fix. One solution is to pull up-to-date packages from the OSRF Gazebo apt repository. Another solution is to compile from source: http://gazebosim.org/tutorials?tut=install_from_source + +* The GPU fix was added to ```gazebo7``` in version 7.14.0 +* The GPU fix was added to ```gazebo9``` in version 9.4.0 + +# Use up-to-date packages from the OSRF Gazebo apt repository +``` +sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/gazebo-stable.list' +sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-keys D2486D2DD83DB69272AFE98867170598AF249743 +sudo apt update +sudo apt upgrade +``` + diff --git a/velodyne_lidar/img/gpu.png b/velodyne_lidar/img/gpu.png new file mode 100644 index 00000000..127e037a Binary files /dev/null and b/velodyne_lidar/img/gpu.png differ diff --git a/velodyne_lidar/velodyne_description/CHANGELOG.rst b/velodyne_lidar/velodyne_description/CHANGELOG.rst index 60ea9d19..fea9b0a1 100644 --- a/velodyne_lidar/velodyne_description/CHANGELOG.rst +++ b/velodyne_lidar/velodyne_description/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package velodyne_description ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.9 (2019-03-08) +------------------ + +1.0.8 (2018-09-08) +------------------ + +1.0.7 (2018-07-03) +------------------ +* Added GPU support +* Updated inertia tensors for VLP-16 and HDL-32E to realistic values +* Removed unnecessary file extraction code in cmake +* Contributors: Kevin Hallenbeck, Max Schwarz + 1.0.6 (2017-10-17) ------------------ * Use robotNamespace as prefix for PointCloud2 topic frame_id by default diff --git a/velodyne_lidar/velodyne_description/CMakeLists.txt b/velodyne_lidar/velodyne_description/CMakeLists.txt index 80f6a560..ef8c4bf3 100644 --- a/velodyne_lidar/velodyne_description/CMakeLists.txt +++ b/velodyne_lidar/velodyne_description/CMakeLists.txt @@ -5,16 +5,6 @@ find_package(catkin REQUIRED) catkin_package() -# Extract *.tar.gz files in the mesh folder -FILE(GLOB files RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}/meshes" "${CMAKE_CURRENT_SOURCE_DIR}/meshes/*.tar.gz") -FOREACH(filename ${files}) - MESSAGE(STATUS "Extracting file: ${filename}") - execute_process( - COMMAND ${CMAKE_COMMAND} -E tar xzf ${filename} - WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/meshes - ) -ENDFOREACH(filename) - install(DIRECTORY launch meshes rviz urdf world DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) diff --git a/velodyne_lidar/velodyne_description/launch/example.launch b/velodyne_lidar/velodyne_description/launch/example.launch index e2e855c6..89e680b6 100644 --- a/velodyne_lidar/velodyne_description/launch/example.launch +++ b/velodyne_lidar/velodyne_description/launch/example.launch @@ -20,7 +20,8 @@ - + + diff --git a/velodyne_lidar/velodyne_description/package.xml b/velodyne_lidar/velodyne_description/package.xml index e1bf745b..c7d47302 100644 --- a/velodyne_lidar/velodyne_description/package.xml +++ b/velodyne_lidar/velodyne_description/package.xml @@ -1,7 +1,7 @@ velodyne_description - 1.0.6 + 1.0.9 URDF and meshes describing Velodyne laser scanners. diff --git a/velodyne_lidar/velodyne_description/urdf/HDL-32E.urdf.xacro b/velodyne_lidar/velodyne_description/urdf/HDL-32E.urdf.xacro index 4a70d581..caa5e949 100644 --- a/velodyne_lidar/velodyne_description/urdf/HDL-32E.urdf.xacro +++ b/velodyne_lidar/velodyne_description/urdf/HDL-32E.urdf.xacro @@ -1,7 +1,7 @@ - + @@ -52,44 +52,86 @@ - - 0 0 0 0 0 0 - false - ${hz} - - - - ${samples} - 1 - ${min_angle} - ${max_angle} - - - ${lasers} - 1 - -${30.67*M_PI/180.0} - ${10.67*M_PI/180.0} - - - - ${collision_range} - ${max_range+1} - 0.001 - - - gaussian - 0.0 - 0.0 - - - - ${topic} - ${name} - ${min_range} - ${max_range} - ${noise} - - + + + 0 0 0 0 0 0 + false + ${hz} + + + + ${samples} + 1 + ${min_angle} + ${max_angle} + + + ${lasers} + 1 + -${30.67*M_PI/180.0} + ${10.67*M_PI/180.0} + + + + ${collision_range} + ${max_range+1} + 0.001 + + + gaussian + 0.0 + 0.0 + + + + ${topic} + ${name} + ${min_range} + ${max_range} + ${noise} + + + + + + 0 0 0 0 0 0 + false + ${hz} + + + + ${samples} + 1 + ${min_angle} + ${max_angle} + + + ${lasers} + 1 + -${30.67*M_PI/180.0} + ${10.67*M_PI/180.0} + + + + ${collision_range} + ${max_range+1} + 0.001 + + + gaussian + 0.0 + 0.0 + + + + ${topic} + ${name} + ${min_range} + ${max_range} + ${noise} + + + diff --git a/velodyne_lidar/velodyne_description/urdf/VLP-16.urdf.xacro b/velodyne_lidar/velodyne_description/urdf/VLP-16.urdf.xacro index b78717a2..25aec97d 100644 --- a/velodyne_lidar/velodyne_description/urdf/VLP-16.urdf.xacro +++ b/velodyne_lidar/velodyne_description/urdf/VLP-16.urdf.xacro @@ -1,7 +1,7 @@ - + @@ -57,44 +57,86 @@ - - 0 0 0 0 0 0 - false - ${hz} - - - - ${samples} - 1 - ${min_angle} - ${max_angle} - - - ${lasers} - 1 - -${15.0*M_PI/180.0} - ${15.0*M_PI/180.0} - - - - ${collision_range} - ${max_range+1} - 0.001 - - - gaussian - 0.0 - 0.0 - - - - ${topic} - ${name} - ${min_range} - ${max_range} - ${noise} - - + + + 0 0 0 0 0 0 + false + ${hz} + + + + ${samples} + 1 + ${min_angle} + ${max_angle} + + + ${lasers} + 1 + -${15.0*M_PI/180.0} + ${15.0*M_PI/180.0} + + + + ${collision_range} + ${max_range+1} + 0.001 + + + gaussian + 0.0 + 0.0 + + + + ${topic} + ${name} + ${min_range} + ${max_range} + ${noise} + + + + + + 0 0 0 0 0 0 + false + ${hz} + + + + ${samples} + 1 + ${min_angle} + ${max_angle} + + + ${lasers} + 1 + -${15.0*M_PI/180.0} + ${15.0*M_PI/180.0} + + + + ${collision_range} + ${max_range+1} + 0.001 + + + gaussian + 0.0 + 0.0 + + + + ${topic} + ${name} + ${min_range} + ${max_range} + ${noise} + + + diff --git a/velodyne_lidar/velodyne_description/urdf/example.urdf.xacro b/velodyne_lidar/velodyne_description/urdf/example.urdf.xacro index a5e26d2f..f958c804 100644 --- a/velodyne_lidar/velodyne_description/urdf/example.urdf.xacro +++ b/velodyne_lidar/velodyne_description/urdf/example.urdf.xacro @@ -1,5 +1,7 @@ + + @@ -31,12 +33,12 @@ - + - + diff --git a/velodyne_lidar/velodyne_gazebo_plugins/CHANGELOG.rst b/velodyne_lidar/velodyne_gazebo_plugins/CHANGELOG.rst index 90987902..c1a20c1d 100644 --- a/velodyne_lidar/velodyne_gazebo_plugins/CHANGELOG.rst +++ b/velodyne_lidar/velodyne_gazebo_plugins/CHANGELOG.rst @@ -2,6 +2,27 @@ Changelog for package velodyne_gazebo_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.9 (2019-03-08) +------------------ +* Added min_intensity parameter to support cliping of low intensity returns +* Contributors: Jonathan Wheare, Kevin Hallenbeck + +1.0.8 (2018-09-08) +------------------ +* Changed iteration order to more closely represent the live velodyne driver +* Contributors: Kevin Hallenbeck + +1.0.7 (2018-07-03) +------------------ +* Added GPU support +* Added support for Gazebo 9 +* Improved behavior of max range calculation +* Removed trailing slashes in robot namespace +* Fixed resolution of 1 not supported +* Fixed issue with only 1 vert or horiz ray +* Fixed cmake exports and warning +* Contributors: Kevin Hallenbeck, Jacob Seibert, Naoki Mizuno + 1.0.6 (2017-10-17) ------------------ * Use robotNamespace as prefix for PointCloud2 topic frame_id by default diff --git a/velodyne_lidar/velodyne_gazebo_plugins/CMakeLists.txt b/velodyne_lidar/velodyne_gazebo_plugins/CMakeLists.txt index 1c510100..92776c7f 100644 --- a/velodyne_lidar/velodyne_gazebo_plugins/CMakeLists.txt +++ b/velodyne_lidar/velodyne_gazebo_plugins/CMakeLists.txt @@ -11,10 +11,9 @@ find_package(gazebo REQUIRED) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") catkin_package( - INCLUDE_DIRS include - LIBRARIES gazebo_ros_velodyne_laser + INCLUDE_DIRS include ${GAZEBO_INCLUDE_DIRS} + LIBRARIES gazebo_ros_velodyne_laser gazebo_ros_velodyne_gpu_laser CATKIN_DEPENDS roscpp sensor_msgs gazebo_ros - DEPENDS gazebo ) include_directories( @@ -34,7 +33,15 @@ target_link_libraries(gazebo_ros_velodyne_laser RayPlugin ) -install(TARGETS gazebo_ros_velodyne_laser +add_library(gazebo_ros_velodyne_gpu_laser src/GazeboRosVelodyneLaser.cpp) +target_link_libraries(gazebo_ros_velodyne_gpu_laser + ${catkin_LIBRARIES} + ${GAZEBO_LIBRARIES} + GpuRayPlugin +) +target_compile_definitions(gazebo_ros_velodyne_gpu_laser PRIVATE GAZEBO_GPU_RAY=1) + +install(TARGETS gazebo_ros_velodyne_laser gazebo_ros_velodyne_gpu_laser LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ) diff --git a/velodyne_lidar/velodyne_gazebo_plugins/include/velodyne_gazebo_plugins/GazeboRosVelodyneLaser.h b/velodyne_lidar/velodyne_gazebo_plugins/include/velodyne_gazebo_plugins/GazeboRosVelodyneLaser.h index 1e2209c3..df3f780e 100644 --- a/velodyne_lidar/velodyne_gazebo_plugins/include/velodyne_gazebo_plugins/GazeboRosVelodyneLaser.h +++ b/velodyne_lidar/velodyne_gazebo_plugins/include/velodyne_gazebo_plugins/GazeboRosVelodyneLaser.h @@ -1,7 +1,7 @@ /********************************************************************* * Software License Agreement (BSD License) * - * Copyright (c) 2015-2017, Dataspeed Inc. + * Copyright (c) 2015-2018, Dataspeed Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -35,6 +35,11 @@ #ifndef GAZEBO_ROS_VELODYNE_LASER_H_ #define GAZEBO_ROS_VELODYNE_LASER_H_ +// Use the same source code for CPU and GPU plugins +#ifndef GAZEBO_GPU_RAY +#define GAZEBO_GPU_RAY 0 +#endif + // Custom Callback Queue #include #include @@ -48,13 +53,24 @@ #include #include #include +#if GAZEBO_GPU_RAY +#include +#else #include +#endif +#include #include #include #include #include +#if GAZEBO_GPU_RAY +#define GazeboRosVelodyneLaser GazeboRosVelodyneGpuLaser +#define RayPlugin GpuRayPlugin +#define RaySensorPtr GpuRaySensorPtr +#endif + namespace gazebo { @@ -89,6 +105,9 @@ namespace gazebo /// \brief frame transform name, should match link name private: std::string frame_name_; + /// \brief the intensity beneath which points will be filtered + private: double min_intensity_; + /// \brief Minimum range to publish private: double min_range_; diff --git a/velodyne_lidar/velodyne_gazebo_plugins/package.xml b/velodyne_lidar/velodyne_gazebo_plugins/package.xml index ff622045..db07a0ba 100644 --- a/velodyne_lidar/velodyne_gazebo_plugins/package.xml +++ b/velodyne_lidar/velodyne_gazebo_plugins/package.xml @@ -1,7 +1,7 @@ velodyne_gazebo_plugins - 1.0.6 + 1.0.9 Gazebo plugin to provide simulated data from Velodyne laser scanners. diff --git a/velodyne_lidar/velodyne_gazebo_plugins/src/GazeboRosVelodyneLaser.cpp b/velodyne_lidar/velodyne_gazebo_plugins/src/GazeboRosVelodyneLaser.cpp index 9299b2ef..27f83b41 100644 --- a/velodyne_lidar/velodyne_gazebo_plugins/src/GazeboRosVelodyneLaser.cpp +++ b/velodyne_lidar/velodyne_gazebo_plugins/src/GazeboRosVelodyneLaser.cpp @@ -1,7 +1,7 @@ /********************************************************************* * Software License Agreement (BSD License) * - * Copyright (c) 2015-2017, Dataspeed Inc. + * Copyright (c) 2015-2018, Dataspeed Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -42,7 +42,11 @@ #include #include #include +#if GAZEBO_GPU_RAY +#include +#else #include +#endif #include #include @@ -50,6 +54,15 @@ #include +#if GAZEBO_GPU_RAY +#define RaySensor GpuRaySensor +#define STR_Gpu "Gpu" +#define STR_GPU_ "GPU " +#else +#define STR_Gpu "" +#define STR_GPU_ "" +#endif + namespace gazebo { // Register this plugin with the simulator @@ -95,7 +108,7 @@ void GazeboRosVelodyneLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _s parent_ray_sensor_ = boost::dynamic_pointer_cast(_parent); #endif if (!parent_ray_sensor_) { - gzthrow("GazeboRosVelodyneLaser controller requires a Ray Sensor as its parent"); + gzthrow("GazeboRosVelodyne" << STR_Gpu << "Laser controller requires a " << STR_Gpu << "Ray Sensor as its parent"); } robot_namespace_ = "/"; @@ -124,6 +137,13 @@ void GazeboRosVelodyneLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _s max_range_ = _sdf->GetElement("max_range")->Get(); } + min_intensity_ = std::numeric_limits::lowest(); + if (!_sdf->HasElement("min_intensity")) { + ROS_INFO("Velodyne laser plugin missing , defaults to no clipping"); + } else { + min_intensity_ = _sdf->GetElement("min_intensity")->Get(); + } + if (!_sdf->HasElement("topicName")) { ROS_INFO("Velodyne laser plugin missing , defaults to /points"); topic_name_ = "/points"; @@ -151,11 +171,11 @@ void GazeboRosVelodyneLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _s // Resolve tf prefix std::string prefix; nh_->getParam(std::string("tf_prefix"), prefix); - if (robot_namespace_ == "/") { - frame_name_ = tf::resolve(prefix, frame_name_); - } else { - frame_name_ = tf::resolve(robot_namespace_, frame_name_); + if (robot_namespace_ != "/") { + prefix = robot_namespace_; } + boost::trim_right_if(prefix, boost::is_any_of("/")); + frame_name_ = tf::resolve(prefix, frame_name_); // Advertise publisher with a custom callback queue if (topic_name_ != "") { @@ -174,9 +194,9 @@ void GazeboRosVelodyneLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _s callback_laser_queue_thread_ = boost::thread( boost::bind( &GazeboRosVelodyneLaser::laserQueueThread,this ) ); #if GAZEBO_MAJOR_VERSION >= 7 - ROS_INFO("Velodyne laser plugin ready, %i lasers", parent_ray_sensor_->VerticalRangeCount()); + ROS_INFO("Velodyne %slaser plugin ready, %i lasers", STR_GPU_, parent_ray_sensor_->VerticalRangeCount()); #else - ROS_INFO("Velodyne laser plugin ready, %i lasers", parent_ray_sensor_->GetVerticalRangeCount()); + ROS_INFO("Velodyne %slaser plugin ready, %i lasers", STR_GPU_, parent_ray_sensor_->GetVerticalRangeCount()); #endif } @@ -208,22 +228,20 @@ void GazeboRosVelodyneLaser::ConnectCb() void GazeboRosVelodyneLaser::OnScan(ConstLaserScanStampedPtr& _msg) { #if GAZEBO_MAJOR_VERSION >= 7 - const math::Angle maxAngle = parent_ray_sensor_->AngleMax(); - const math::Angle minAngle = parent_ray_sensor_->AngleMin(); + const ignition::math::Angle maxAngle = parent_ray_sensor_->AngleMax(); + const ignition::math::Angle minAngle = parent_ray_sensor_->AngleMin(); const double maxRange = parent_ray_sensor_->RangeMax(); const double minRange = parent_ray_sensor_->RangeMin(); const int rayCount = parent_ray_sensor_->RayCount(); const int rangeCount = parent_ray_sensor_->RangeCount(); - assert(rayCount == rangeCount); const int verticalRayCount = parent_ray_sensor_->VerticalRayCount(); const int verticalRangeCount = parent_ray_sensor_->VerticalRangeCount(); - assert(verticalRayCount == verticalRangeCount); - const math::Angle verticalMaxAngle = parent_ray_sensor_->VerticalAngleMax(); - const math::Angle verticalMinAngle = parent_ray_sensor_->VerticalAngleMin(); + const ignition::math::Angle verticalMaxAngle = parent_ray_sensor_->VerticalAngleMax(); + const ignition::math::Angle verticalMinAngle = parent_ray_sensor_->VerticalAngleMin(); #else math::Angle maxAngle = parent_ray_sensor_->GetAngleMax(); math::Angle minAngle = parent_ray_sensor_->GetAngleMin(); @@ -233,11 +251,9 @@ void GazeboRosVelodyneLaser::OnScan(ConstLaserScanStampedPtr& _msg) const int rayCount = parent_ray_sensor_->GetRayCount(); const int rangeCount = parent_ray_sensor_->GetRangeCount(); - assert(rayCount == rangeCount); const int verticalRayCount = parent_ray_sensor_->GetVerticalRayCount(); const int verticalRangeCount = parent_ray_sensor_->GetVerticalRangeCount(); - assert(verticalRayCount == verticalRangeCount); const math::Angle verticalMaxAngle = parent_ray_sensor_->GetVerticalAngleMax(); const math::Angle verticalMinAngle = parent_ray_sensor_->GetVerticalAngleMin(); @@ -247,7 +263,8 @@ void GazeboRosVelodyneLaser::OnScan(ConstLaserScanStampedPtr& _msg) const double pDiff = verticalMaxAngle.Radian() - verticalMinAngle.Radian(); const double MIN_RANGE = std::max(min_range_, minRange); - const double MAX_RANGE = std::min(max_range_, maxRange - minRange - 0.01); + const double MAX_RANGE = std::min(max_range_, maxRange); + const double MIN_INTENSITY = min_intensity_; // Populate message fields const uint32_t POINT_STEP = 32; @@ -279,21 +296,39 @@ void GazeboRosVelodyneLaser::OnScan(ConstLaserScanStampedPtr& _msg) int i, j; uint8_t *ptr = msg.data.data(); - for (j = 0; j < verticalRangeCount; j++) { - for (i = 0; i < rangeCount; i++) { + for (i = 0; i < rangeCount; i++) { + for (j = 0; j < verticalRangeCount; j++) { + + // Range + double r = _msg->scan().ranges(i + j * rangeCount); + // Intensity + double intensity = _msg->scan().intensities(i + j * rangeCount); + // Ignore points that lay outside range bands or optionally, beneath a + // minimum intensity level. + if ((MIN_RANGE >= r) || (r >= MAX_RANGE) || (intensity < MIN_INTENSITY) ) { + continue; + } - // Range and noise - double r = std::min(_msg->scan().ranges(i + j * rangeCount), maxRange-minRange); + // Noise if (gaussian_noise_ != 0.0) { r += gaussianKernel(0,gaussian_noise_); } - // Intensity - double intensity = _msg->scan().intensities(i + j * rangeCount); - // Get angles of ray to get xyz for point - double yAngle = i * yDiff / (rayCount -1) + minAngle.Radian(); - double pAngle = j * pDiff / (verticalRayCount -1) + verticalMinAngle.Radian(); + double yAngle; + double pAngle; + + if (rangeCount > 1) { + yAngle = i * yDiff / (rangeCount -1) + minAngle.Radian(); + } else { + yAngle = minAngle.Radian(); + } + + if (verticalRayCount > 1) { + pAngle = j * pDiff / (verticalRangeCount -1) + verticalMinAngle.Radian(); + } else { + pAngle = verticalMinAngle.Radian(); + } // pAngle is rotated by yAngle: if ((MIN_RANGE < r) && (r < MAX_RANGE)) { diff --git a/velodyne_lidar/velodyne_simulator/CHANGELOG.rst b/velodyne_lidar/velodyne_simulator/CHANGELOG.rst index cb301288..2aadb0ea 100644 --- a/velodyne_lidar/velodyne_simulator/CHANGELOG.rst +++ b/velodyne_lidar/velodyne_simulator/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package velodyne_simulator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.9 (2019-03-08) +------------------ + +1.0.8 (2018-09-08) +------------------ + +1.0.7 (2018-07-03) +------------------ + 1.0.6 (2017-10-17) ------------------ diff --git a/velodyne_lidar/velodyne_simulator/package.xml b/velodyne_lidar/velodyne_simulator/package.xml index 42d1a16a..4309bacc 100644 --- a/velodyne_lidar/velodyne_simulator/package.xml +++ b/velodyne_lidar/velodyne_simulator/package.xml @@ -1,7 +1,7 @@ velodyne_simulator - 1.0.6 + 1.0.9 Metapackage allowing easy installation of Velodyne simulation components.