Clone velodyne_simulator.

Used to simulate VLP-16.

Branch: foxy-devel
Reference: https://bitbucket.org/DataspeedInc/velodyne_simulator/src/1cfa7b3bb562849271b9185f3a636bae714aadda/

kobuki_ws/src/velodyne_simulator/.gitignore

@@ -0,0 +1,20 @@
+syntax: glob
+
+# Files generated by Eclipse
+catkin/
+catkin_generated/
+cmake/
+CMakeFiles/
+devel/
+gtest/
+test_results/
+CMakeCache.txt
+cmake_install.cmake
+CTestTestfile.cmake
+Makefile
+.cproject
+.project
+
+# Temporary files
+*~
+

kobuki_ws/src/velodyne_simulator/LICENSE

@@ -0,0 +1,27 @@
+Software License Agreement (BSD License)
+
+Copyright (c) 2015-2021, Dataspeed Inc.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+    * Redistributions of source code must retain the above copyright notice,
+      this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright notice,
+      this list of conditions and the following disclaimer in the documentation
+      and/or other materials provided with the distribution.
+    * Neither the name of Dataspeed Inc. nor the names of its
+      contributors may be used to endorse or promote products derived from this
+      software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

kobuki_ws/src/velodyne_simulator/README.md

@@ -0,0 +1,49 @@
+# Velodyne Simulator
+URDF description and Gazebo plugins to simulate Velodyne laser scanners
+
+![rviz screenshot](img/rviz.png)
+
+# Features
+* URDF with colored meshes
+* Gazebo plugin based on [gazebo_plugins/gazebo_ros_ray_sensor](https://github.com/ros-simulation/gazebo_ros_pkgs/blob/foxy/gazebo_plugins/src/gazebo_ros_ray_sensor.cpp)
+* Publishes PointCloud2 with same structure (x, y, z, intensity, ring, time)
+* Simulated Gaussian noise
+* GPU acceleration (with known issues)
+* 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.
+* ```parent``` URDF parent link name. Default ```base_link```
+* ```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: 32```
+* ```samples``` Nuber of horizontal rotating samples. Default ```VLP-16: 1875, HDL-32E: 2187```
+* ```organize_cloud``` Organize PointCloud2 into 2D array with NaN placeholders, otherwise 1D array and leave out invlaid points. Default ```false```
+* ```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
+* When accelerated with the GPU option, ranges are heavily quantized ([image](img/gpu.png))
+    * Solution: Use CPU instead of GPU
+* 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."
+    * Solution: User can reduce number of points in urdf (same as above)
+
+# Example Gazebo Robot
+```ros2 launch velodyne_description example.launch.py```
+
+# Example Gazebo Robot (with GPU)
+```ros2 launch velodyne_description example.launch.py gpu:=true```
+

kobuki_ws/src/velodyne_simulator/bitbucket-pipelines.yml

@@ -0,0 +1,43 @@
+pipelines:
+  default:
+    - parallel:
+      - step:
+          name: foxy
+          image: ros:foxy
+          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 | tail -1` && echo $ROS_DISTRO # Source ROS environment
+            - apt update && rosdep install --from-paths src --ignore-src -y # Install dependencies missing from the docker image
+            - colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
+            - colcon test
+            - colcon test-result
+      - step:
+          name: galactic
+          image: ros:galactic
+          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 | tail -1` && echo $ROS_DISTRO # Source ROS environment
+            - apt update && rosdep install --from-paths src --ignore-src -y # Install dependencies missing from the docker image
+            - colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
+            - colcon test
+            - colcon test-result
+      - step:
+          name: humble
+          image: ros:humble
+          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 | tail -1` && echo $ROS_DISTRO # Source ROS environment
+            - apt update && rosdep install --from-paths src --ignore-src -y # Install dependencies missing from the docker image
+            - colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
+            - colcon test
+            - colcon test-result
+      - step:
+          name: rolling
+          image: ros:rolling
+          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 | tail -1` && echo $ROS_DISTRO # Source ROS environment
+            - apt update && rosdep install --from-paths src --ignore-src -y # Install dependencies missing from the docker image
+            - colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
+            - colcon test
+            - colcon test-result

kobuki_ws/src/velodyne_simulator/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
+```
+

kobuki_ws/src/velodyne_simulator/velodyne_description/CHANGELOG.rst

@@ -0,0 +1,85 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package velodyne_description
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+2.0.3 (2022-12-02)
+------------------
+* Add dummy collision to avoid moveit warnings
+* Remove gazebo_ros dependency for velodyne_description
+* Contributors: Filip Sund, Kevin Hallenbeck
+
+2.0.2 (2021-12-27)
+------------------
+* Adds tf_prefix SDF parameter
+* Contributors: Micho Radovnikovich
+
+2.0.1 (2021-10-14)
+------------------
+
+2.0.0 (2021-10-12)
+------------------
+* Merged in ROS2 Foxy support (pull request #14)
+  * Remove support for old Gazebo versions
+  * Lazy subscriber using timer in the absence of publisher connection callback
+  * Python launch file
+  * Add env-hooks for GAZEBO_MODEL_PATH
+* Contributors: Kevin Hallenbeck, Gonzalo de Pedro, Joep Tool
+
+1.0.12 (2021-03-30)
+-------------------
+
+1.0.11 (2021-02-23)
+-------------------
+* Add organize_cloud parameter to match velodyne_pointcloud
+* Contributors: Kevin Hallenbeck
+
+1.0.10 (2020-08-03)
+-------------------
+* Change PointCloud visualization type from flat squares to points in example rviz config
+* Bump minimum CMake version to 3.0.2 in all CMakeLists.txt
+* Fix xacro macro instantiation
+* Contributors: Kevin Hallenbeck, Micho Radovnikovich
+
+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
+* Contributors: Micho Radovnikovich
+
+1.0.5 (2017-09-05)
+------------------
+* Increased minimum collision range to prevent self-clipping when in motion
+* Added many URDF parameters, and set example sample count to reasonable values
+* Launch rviz with gazebo
+* Contributors: Kevin Hallenbeck
+
+1.0.4 (2017-04-24)
+------------------
+* Updated package.xml format to version 2
+* Contributors: Kevin Hallenbeck
+
+1.0.3 (2016-08-13)
+------------------
+* Contributors: Kevin Hallenbeck
+
+1.0.2 (2016-02-03)
+------------------
+* Moved M_PI property out of macro to support multiple instances
+* Materials caused problems with more than one sensors. Removed.
+* Added example urdf and gazebo
+* Changed to DAE meshes
+* Added meshes. Added HDL-32E.
+* Start from block laser
+* Contributors: Kevin Hallenbeck

kobuki_ws/src/velodyne_simulator/velodyne_description/CMakeLists.txt

@@ -0,0 +1,12 @@
+cmake_minimum_required(VERSION 3.5.0)
+project(velodyne_description)
+
+find_package(ament_cmake REQUIRED)
+
+install(DIRECTORY launch meshes rviz urdf world
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/velodyne_description.dsv.in")
+
+ament_package()

kobuki_ws/src/velodyne_simulator/velodyne_description/env-hooks/velodyne_description.dsv.in

@@ -0,0 +1 @@
+prepend-non-duplicate;GAZEBO_MODEL_PATH;share/

kobuki_ws/src/velodyne_simulator/velodyne_description/launch/example.launch.py

@@ -0,0 +1,123 @@
+#*********************************************************************
+ # Software License Agreement (BSD License)
+ #
+ #  Copyright (c) 2021, Dataspeed Inc
+ #  All rights reserved.
+ #
+ #  Redistribution and use in source and binary forms, with or without
+ #  modification, are permitted provided that the following conditions
+ #  are met:
+ #
+ #   * Redistributions of source code must retain the above copyright
+ #     notice, this list of conditions and the following disclaimer.
+ #   * Redistributions in binary form must reproduce the above
+ #     copyright notice, this list of conditions and the following
+ #     disclaimer in the documentation and/or other materials provided
+ #     with the distribution.
+ #   * Neither the name of Dataspeed Inc. nor the names of its
+ #     contributors may be used to endorse or promote products derived
+ #     from this software without specific prior written permission.
+ #
+ #  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ #  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ #  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ #  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ #  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ #  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ #  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ #  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ #  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ #  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ #  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ #  POSSIBILITY OF SUCH DAMAGE.
+ #********************************************************************
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, EmitEvent, RegisterEventHandler
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.event_handlers import OnProcessExit
+from launch.events import Shutdown
+from launch_ros.actions import Node
+from launch.conditions import IfCondition, UnlessCondition
+from launch.substitutions import Command, LaunchConfiguration, PythonExpression
+
+def generate_launch_description():
+  this_directory = get_package_share_directory('velodyne_description')
+  xacro_path = os.path.join(this_directory, 'urdf', 'example.urdf.xacro')
+  rviz_config_file = os.path.join(this_directory, 'rviz', 'example.rviz')
+  world = os.path.join(this_directory, 'world', 'example.world')
+
+  declare_gpu_cmd = DeclareLaunchArgument(
+    'gpu',
+    default_value='False',
+    description='Whether to use Gazebo gpu_ray or ray')
+  declare_organize_cloud_cmd = DeclareLaunchArgument(
+    'organize_cloud',
+    default_value='False',
+    description='Organize PointCloud2 into 2D array with NaN placeholders, otherwise 1D array and leave out invlaid points')
+  gpu = LaunchConfiguration('gpu')
+  organize_cloud = LaunchConfiguration('organize_cloud')
+  robot_description = Command(['xacro',' ', xacro_path, ' gpu:=', gpu, ' organize_cloud:=', organize_cloud])
+
+  start_robot_state_publisher_cmd = Node(
+    package='robot_state_publisher',
+    executable='robot_state_publisher',
+    name='robot_state_publisher',
+    output='screen',
+    parameters=[{
+      'use_sim_time': True,
+      'robot_description': robot_description
+    }]
+  )
+
+  spawn_example_cmd = Node(
+    package='gazebo_ros', 
+    executable='spawn_entity.py',
+    arguments=[
+      '-entity', 'example',
+      '-topic', 'robot_description',
+    ],
+    output='screen',
+  )
+
+  start_rviz_cmd = Node(
+    package='rviz2',
+    executable='rviz2',
+    arguments=['-d', rviz_config_file],
+    output='screen'
+  )
+
+  exit_event_handler = RegisterEventHandler(
+    event_handler=OnProcessExit(
+      target_action=start_rviz_cmd,
+      on_exit=EmitEvent(event=Shutdown(reason='rviz exited'))
+    )
+  )
+
+  declare_gui_cmd = DeclareLaunchArgument(
+    'gui',
+    default_value='True',
+    description='Whether to launch the Gazebo GUI or not (headless)')
+  gui = LaunchConfiguration('gui')
+  start_gazebo = IncludeLaunchDescription(
+    PythonLaunchDescriptionSource(os.path.join(
+      get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')),
+    launch_arguments={'world' : world, 'gui' : gui}.items()
+  )
+
+  ld = LaunchDescription()
+
+  # Add the actions
+  ld.add_action(declare_gpu_cmd)
+  ld.add_action(declare_organize_cloud_cmd)
+  ld.add_action(declare_gui_cmd)
+  ld.add_action(start_gazebo)
+  ld.add_action(start_robot_state_publisher_cmd)
+  ld.add_action(spawn_example_cmd)
+  ld.add_action(start_rviz_cmd)
+  ld.add_action(exit_event_handler)
+
+  return ld