Unverified Commit 6f62a519 authored by Simon Thomas's avatar Simon Thomas
Browse files

Initial commit

parents
Software License Agreement (BSD License)
Copyright (c) 2020, ENSTA PARIS.
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.
# Robosense Simulator
URDF description and Gazebo plugins to simulate Robosense laser scanners. Cloned and adapted from [velodyne_simulator](https://bitbucket.org/DataspeedInc/velodyne_simulator)
![rviz screenshot](img/rviz.png)
# Features
* URDF with colored meshes
* 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:
* [RS-LiDAR-16](robosense_description/urdf/RS-16.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 ```robosense```
* ```topic``` PointCloud2 output topic name. Default ```/rs_points```
* ```hz``` Update rate in hz. Default ```20```
* ```lasers``` Number of vertical spinning lasers. Default ```RS-16: 16```
* ```samples``` Number of horizontal rotating samples. Default ```RS-16: 900```
* ```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
# Example Gazebo Robot
```roslaunch robosense_description example.launch```
# Example Gazebo Robot (with GPU)
```roslaunch robosense_description example.launch gpu:=true```
# 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
```
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package robosense_description
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
cmake_minimum_required(VERSION 2.8.3)
project(robosense_description)
find_package(catkin REQUIRED)
catkin_package()
install(DIRECTORY launch meshes rviz urdf world
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
<?xml version="1.0" ?>
<launch>
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<arg name="verbose" default="false"/>
<arg name="world_name" default="$(find robosense_description)/world/example.world"/>
<!-- Start gazebo and load the world -->
<include file="$(find gazebo_ros)/launch/empty_world.launch" >
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="gui" value="$(arg gui)"/>
<arg name="headless" value="$(arg headless)"/>
<arg name="debug" value="$(arg debug)"/>
<arg name="verbose" value="$(arg verbose)"/>
<arg name="world_name" value="$(arg world_name)"/>
</include>
<!-- Spawn the example robot -->
<arg name="gpu" default="false"/>
<param name="robot_description" command="$(find xacro)/xacro '$(find robosense_description)/urdf/example.urdf.xacro' gpu:=$(arg gpu)" />
<node pkg="gazebo_ros" type="spawn_model" name="spawn_model" args="-urdf -param /robot_description -model example"/>
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
<param name="publish_frequency" type="double" value="30.0" />
</node>
<!-- RViz -->
<arg name="rviz" default="true"/>
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="$(anon rviz)" respawn="false" output="screen" args="-d $(find robosense_description)/rviz/example.rviz" />
</launch>
This diff is collapsed.
<?xml version="1.0"?>
<package format="2">
<name>robosense_description</name>
<version>1.0.0</version>
<description>
URDF and meshes describing Robosense laser scanners.
</description>
<license>BSD</license>
<author>Thomas SIMON</author>
<maintainer email="thomas.simon@ensta-paris.fr">Thomas SIMON</maintainer>
<url type="website">http://wiki.ros.org/robosense_description</url>
<!--url type="repository"></url>
<url type="bugtracker"></url-->
<buildtool_depend>catkin</buildtool_depend>
<exec_depend>urdf</exec_depend>
<exec_depend>xacro</exec_depend>
</package>
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
Splitter Ratio: 0.5
Tree Height: 719
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: PointCloud2
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 0
Min Color: 0; 0; 0
Min Intensity: 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic: /rs_points
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Class: rviz/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: true
base_footprint:
Value: true
base_link:
Value: true
robosense:
Value: true
robosense_base_link:
Value: true
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
base_footprint:
base_link:
robosense_base_link:
robosense:
{}
Update Interval: 0
Value: true
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
base_footprint:
Alpha: 1
Show Axes: false
Show Trail: false
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robosense:
Alpha: 1
Show Axes: false
Show Trail: false
robosense_base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: base_footprint
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Theta std deviation: 0.2617993950843811
Topic: /initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 5.06912899017334
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0
Y: 0
Z: 0
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.570398211479187
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 0.6853981018066406
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1016
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000003efc0100000002fb0000000800540069006d006501000000000000073a000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005de0000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1850
X: 70
Y: 27
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="RS-16">
<xacro:property name="M_PI" value="3.1415926535897931" />
<xacro:macro name="RS-16" params="*origin parent:=base_link name:=robosense topic:=/robosense_points hz:=20 lasers:=16 samples:=900 collision_range:=0.4 min_range:=0.4 max_range:=150.0 noise:=0.008 min_angle:=-${M_PI} max_angle:=${M_PI} gpu:=false">
<joint name="${name}_base_mount_joint" type="fixed">
<xacro:insert_block name="origin" />
<parent link="${parent}"/>
<child link="${name}_base_link"/>
</joint>
<link name="${name}_base_link">
<inertial>
<mass value="0.87"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.001094695" ixy="0" ixz="0"
iyy="0.001094695" iyz="0"
izz="0.001245079"/>
</inertial>
<visual>
<origin rpy="-1.5707 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://robosense_description/meshes/robosense_16.dae" />
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder radius="0.0535" length="0.0817"/>
</geometry>
</collision>
</link>
<joint name="${name}_base_scan_joint" type="fixed" >
<origin xyz="0 0 0.039" rpy="0 0 0" />
<parent link="${name}_base_link" />
<child link="${name}"/>
</joint>
<link name="${name}">
<inertial>
<mass value="0.01"/>
<origin xyz="0 0 0"/>
<inertia ixx="1e-7" ixy="0" ixz="0" iyy="1e-7" iyz="0" izz="1e-7"/>
</inertial>
</link>
<!-- Gazebo requires the robosense_gazebo_plugins package -->
<gazebo reference="${name}">
<xacro:if value="${gpu}">
<sensor type="gpu_ray" name="${name}-RS16">
<pose>0 0 0 0 0 0</pose>
<visualize>false</visualize>
<update_rate>${hz}</update_rate>
<ray>
<scan>
<horizontal>
<samples>${samples}</samples>
<resolution>1</resolution>
<min_angle>${min_angle}</min_angle>
<max_angle>${max_angle}</max_angle>
</horizontal>
<vertical>
<samples>${lasers}</samples>
<resolution>1</resolution>
<min_angle>-${15.0*M_PI/180.0}</min_angle>
<max_angle> ${15.0*M_PI/180.0}</max_angle>
</vertical>
</scan>
<range>
<min>${collision_range}</min>
<max>${max_range}</max>
<resolution>0.001</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.0</stddev>
</noise>
</ray>
<plugin name="gazebo_ros_laser_controller" filename="libgazebo_ros_robosense_gpu_laser.so">
<topicName>${topic}</topicName>
<frameName>${name}</frameName>
<min_range>${min_range}</min_range>
<max_range>${max_range}</max_range>
<gaussianNoise>${noise}</gaussianNoise>
</plugin>
</sensor>
</xacro:if>
<xacro:unless value="${gpu}">
<sensor type="ray" name="${name}-RS16">
<pose>0 0 0 0 0 0</pose>
<visualize>false</visualize>
<update_rate>${hz}</update_rate>
<ray>
<scan>
<horizontal>
<samples>${samples}</samples>
<resolution>1</resolution>
<min_angle>${min_angle}</min_angle>
<max_angle>${max_angle}</max_angle>
</horizontal>
<vertical>
<samples>${lasers}</samples>
<resolution>1</resolution>
<min_angle>-${15.0*M_PI/180.0}</min_angle>
<max_angle> ${15.0*M_PI/180.0}</max_angle>
</vertical>
</scan>
<range>
<min>${collision_range}</min>
<max>${max_range+1}</max>
<resolution>0.001</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.0</stddev>
</noise>
</ray>
<plugin name="gazebo_ros_laser_controller" filename="libgazebo_ros_robosense_laser.so">
<topicName>${topic}</topicName>
<frameName>${name}</frameName>
<min_range>${min_range}</min_range>
<max_range>${max_range}</max_range>
<gaussianNoise>${noise}</gaussianNoise>
</plugin>
</sensor>
</xacro:unless>
</gazebo>
</xacro:macro>
</robot>
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="example">
<xacro:arg name="gpu" default="false"/>
<xacro:property name="gpu" value="$(arg gpu)" />
<!-- Base Footprint -->
<link name="base_footprint" />
<!-- Base Link -->
<joint name="footprint" type="fixed" >
<parent link="base_footprint" />
<child link="base_link" />
<origin xyz="0 0 0.05" rpy="0 0 0" />
</joint>
<link name="base_link" >
<visual>
<geometry>
<box size="0.5 0.5 0.1" />
</geometry>
</visual>
<collision>
<geometry>
<box size="0.5 0.5 0.1" />
</geometry>
</collision>
<inertial>
<origin xyz="0 0 0"/>
<mass value="10"/>
<inertia ixx="3.0" ixy="0.0" ixz="0.0"
iyy="3.0" iyz="0.0"
izz="3.0" />
</inertial>
</link>
<xacro:include filename="$(find robosense_description)/urdf/RS-16.urdf.xacro"/>
<RS-16 parent="base_link" name="robosense" topic="/rs_points" hz="20" samples="900" gpu="${gpu}">
<origin xyz="0 0 0.4" rpy="0 0 0" />
</RS-16>
</robot>
<?xml version="1.0" ?>
<sdf version="1.5">
<world name="default">
<!-- A global light source -->
<include>
<uri>model://sun</uri>
</include>
<!-- A ground plane -->
<include>
<uri>model://ground_plane</uri>
</include>
<!-- A unit box -->
<model name='unit_box_0_0'>
<pose frame=''>-1.5 2.5 0.5 0 -0 0</pose>
<link name='link'>
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.166667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.166667</iyy>
<iyz>0</iyz>
<izz>0.166667</izz>
</inertia>
</inertial>
<collision name='collision'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<material>
<script>
<name>Gazebo/Grey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
</link>
</model>
</world>
</sdf>
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package robosense_gazebo_plugins
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^