Added first version of Sensorsim
This commit is contained in:
wittenator
parent
784dca286b
commit
97443604b7
|
|
@ -0,0 +1,49 @@
|
|||
cmake_minimum_required(VERSION 3.5)
|
||||
project(sensor_sim)
|
||||
|
||||
# Default to C99
|
||||
if(NOT CMAKE_C_STANDARD)
|
||||
set(CMAKE_C_STANDARD 99)
|
||||
endif()
|
||||
|
||||
# Default to C++14
|
||||
if(NOT CMAKE_CXX_STANDARD)
|
||||
set(CMAKE_CXX_STANDARD 14)
|
||||
endif()
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
# uncomment the following section in order to fill in
|
||||
# further dependencies manually.
|
||||
# find_package(<dependency> REQUIRED)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# uncomment the line when a copyright and license is not present in all source files
|
||||
#set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# uncomment the line when this package is not in a git repo
|
||||
#set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
install(
|
||||
DIRECTORY description launch worlds src
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_python_install_package(${PROJECT_NAME})
|
||||
|
||||
# Install Python executables
|
||||
install(PROGRAMS
|
||||
src/lidar_visualizer.py
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/${PROJECT_NAME}.dsv.in")
|
||||
ament_package()
|
||||
|
|
@ -0,0 +1,201 @@
|
|||
Apache License
|
||||
Version 2.0, January 2004
|
||||
http://www.apache.org/licenses/
|
||||
|
||||
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
|
||||
|
||||
1. Definitions.
|
||||
|
||||
"License" shall mean the terms and conditions for use, reproduction,
|
||||
and distribution as defined by Sections 1 through 9 of this document.
|
||||
|
||||
"Licensor" shall mean the copyright owner or entity authorized by
|
||||
the copyright owner that is granting the License.
|
||||
|
||||
"Legal Entity" shall mean the union of the acting entity and all
|
||||
other entities that control, are controlled by, or are under common
|
||||
control with that entity. For the purposes of this definition,
|
||||
"control" means (i) the power, direct or indirect, to cause the
|
||||
direction or management of such entity, whether by contract or
|
||||
otherwise, or (ii) ownership of fifty percent (50%) or more of the
|
||||
outstanding shares, or (iii) beneficial ownership of such entity.
|
||||
|
||||
"You" (or "Your") shall mean an individual or Legal Entity
|
||||
exercising permissions granted by this License.
|
||||
|
||||
"Source" form shall mean the preferred form for making modifications,
|
||||
including but not limited to software source code, documentation
|
||||
source, and configuration files.
|
||||
|
||||
"Object" form shall mean any form resulting from mechanical
|
||||
transformation or translation of a Source form, including but
|
||||
not limited to compiled object code, generated documentation,
|
||||
and conversions to other media types.
|
||||
|
||||
"Work" shall mean the work of authorship, whether in Source or
|
||||
Object form, made available under the License, as indicated by a
|
||||
copyright notice that is included in or attached to the work
|
||||
(an example is provided in the Appendix below).
|
||||
|
||||
"Derivative Works" shall mean any work, whether in Source or Object
|
||||
form, that is based on (or derived from) the Work and for which the
|
||||
editorial revisions, annotations, elaborations, or other modifications
|
||||
represent, as a whole, an original work of authorship. For the purposes
|
||||
of this License, Derivative Works shall not include works that remain
|
||||
separable from, or merely link (or bind by name) to the interfaces of,
|
||||
the Work and Derivative Works thereof.
|
||||
|
||||
"Contribution" shall mean any work of authorship, including
|
||||
the original version of the Work and any modifications or additions
|
||||
to that Work or Derivative Works thereof, that is intentionally
|
||||
submitted to Licensor for inclusion in the Work by the copyright owner
|
||||
or by an individual or Legal Entity authorized to submit on behalf of
|
||||
the copyright owner. For the purposes of this definition, "submitted"
|
||||
means any form of electronic, verbal, or written communication sent
|
||||
to the Licensor or its representatives, including but not limited to
|
||||
communication on electronic mailing lists, source code control systems,
|
||||
and issue tracking systems that are managed by, or on behalf of, the
|
||||
Licensor for the purpose of discussing and improving the Work, but
|
||||
excluding communication that is conspicuously marked or otherwise
|
||||
designated in writing by the copyright owner as "Not a Contribution."
|
||||
|
||||
"Contributor" shall mean Licensor and any individual or Legal Entity
|
||||
on behalf of whom a Contribution has been received by Licensor and
|
||||
subsequently incorporated within the Work.
|
||||
|
||||
2. Grant of Copyright License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
copyright license to reproduce, prepare Derivative Works of,
|
||||
publicly display, publicly perform, sublicense, and distribute the
|
||||
Work and such Derivative Works in Source or Object form.
|
||||
|
||||
3. Grant of Patent License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
(except as stated in this section) patent license to make, have made,
|
||||
use, offer to sell, sell, import, and otherwise transfer the Work,
|
||||
where such license applies only to those patent claims licensable
|
||||
by such Contributor that are necessarily infringed by their
|
||||
Contribution(s) alone or by combination of their Contribution(s)
|
||||
with the Work to which such Contribution(s) was submitted. If You
|
||||
institute patent litigation against any entity (including a
|
||||
cross-claim or counterclaim in a lawsuit) alleging that the Work
|
||||
or a Contribution incorporated within the Work constitutes direct
|
||||
or contributory patent infringement, then any patent licenses
|
||||
granted to You under this License for that Work shall terminate
|
||||
as of the date such litigation is filed.
|
||||
|
||||
4. Redistribution. You may reproduce and distribute copies of the
|
||||
Work or Derivative Works thereof in any medium, with or without
|
||||
modifications, and in Source or Object form, provided that You
|
||||
meet the following conditions:
|
||||
|
||||
(a) You must give any other recipients of the Work or
|
||||
Derivative Works a copy of this License; and
|
||||
|
||||
(b) You must cause any modified files to carry prominent notices
|
||||
stating that You changed the files; and
|
||||
|
||||
(c) You must retain, in the Source form of any Derivative Works
|
||||
that You distribute, all copyright, patent, trademark, and
|
||||
attribution notices from the Source form of the Work,
|
||||
excluding those notices that do not pertain to any part of
|
||||
the Derivative Works; and
|
||||
|
||||
(d) If the Work includes a "NOTICE" text file as part of its
|
||||
distribution, then any Derivative Works that You distribute must
|
||||
include a readable copy of the attribution notices contained
|
||||
within such NOTICE file, excluding those notices that do not
|
||||
pertain to any part of the Derivative Works, in at least one
|
||||
of the following places: within a NOTICE text file distributed
|
||||
as part of the Derivative Works; within the Source form or
|
||||
documentation, if provided along with the Derivative Works; or,
|
||||
within a display generated by the Derivative Works, if and
|
||||
wherever such third-party notices normally appear. The contents
|
||||
of the NOTICE file are for informational purposes only and
|
||||
do not modify the License. You may add Your own attribution
|
||||
notices within Derivative Works that You distribute, alongside
|
||||
or as an addendum to the NOTICE text from the Work, provided
|
||||
that such additional attribution notices cannot be construed
|
||||
as modifying the License.
|
||||
|
||||
You may add Your own copyright statement to Your modifications and
|
||||
may provide additional or different license terms and conditions
|
||||
for use, reproduction, or distribution of Your modifications, or
|
||||
for any such Derivative Works as a whole, provided Your use,
|
||||
reproduction, and distribution of the Work otherwise complies with
|
||||
the conditions stated in this License.
|
||||
|
||||
5. Submission of Contributions. Unless You explicitly state otherwise,
|
||||
any Contribution intentionally submitted for inclusion in the Work
|
||||
by You to the Licensor shall be under the terms and conditions of
|
||||
this License, without any additional terms or conditions.
|
||||
Notwithstanding the above, nothing herein shall supersede or modify
|
||||
the terms of any separate license agreement you may have executed
|
||||
with Licensor regarding such Contributions.
|
||||
|
||||
6. Trademarks. This License does not grant permission to use the trade
|
||||
names, trademarks, service marks, or product names of the Licensor,
|
||||
except as required for reasonable and customary use in describing the
|
||||
origin of the Work and reproducing the content of the NOTICE file.
|
||||
|
||||
7. Disclaimer of Warranty. Unless required by applicable law or
|
||||
agreed to in writing, Licensor provides the Work (and each
|
||||
Contributor provides its Contributions) on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
|
||||
implied, including, without limitation, any warranties or conditions
|
||||
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
|
||||
PARTICULAR PURPOSE. You are solely responsible for determining the
|
||||
appropriateness of using or redistributing the Work and assume any
|
||||
risks associated with Your exercise of permissions under this License.
|
||||
|
||||
8. Limitation of Liability. In no event and under no legal theory,
|
||||
whether in tort (including negligence), contract, or otherwise,
|
||||
unless required by applicable law (such as deliberate and grossly
|
||||
negligent acts) or agreed to in writing, shall any Contributor be
|
||||
liable to You for damages, including any direct, indirect, special,
|
||||
incidental, or consequential damages of any character arising as a
|
||||
result of this License or out of the use or inability to use the
|
||||
Work (including but not limited to damages for loss of goodwill,
|
||||
work stoppage, computer failure or malfunction, or any and all
|
||||
other commercial damages or losses), even if such Contributor
|
||||
has been advised of the possibility of such damages.
|
||||
|
||||
9. Accepting Warranty or Additional Liability. While redistributing
|
||||
the Work or Derivative Works thereof, You may choose to offer,
|
||||
and charge a fee for, acceptance of support, warranty, indemnity,
|
||||
or other liability obligations and/or rights consistent with this
|
||||
License. However, in accepting such obligations, You may act only
|
||||
on Your own behalf and on Your sole responsibility, not on behalf
|
||||
of any other Contributor, and only if You agree to indemnify,
|
||||
defend, and hold each Contributor harmless for any liability
|
||||
incurred by, or claims asserted against, such Contributor by reason
|
||||
of your accepting any such warranty or additional liability.
|
||||
|
||||
END OF TERMS AND CONDITIONS
|
||||
|
||||
APPENDIX: How to apply the Apache License to your work.
|
||||
|
||||
To apply the Apache License to your work, attach the following
|
||||
boilerplate notice, with the fields enclosed by brackets "[]"
|
||||
replaced with your own identifying information. (Don't include
|
||||
the brackets!) The text should be enclosed in the appropriate
|
||||
comment syntax for the file format. We also recommend that a
|
||||
file or class name and description of purpose be included on the
|
||||
same "printed page" as the copyright notice for easier
|
||||
identification within third-party archives.
|
||||
|
||||
Copyright [yyyy] [name of copyright owner]
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
File diff suppressed because one or more lines are too long
|
|
@ -0,0 +1,52 @@
|
|||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" >
|
||||
|
||||
<joint name="camera_joint" type="fixed">
|
||||
<parent link="laser_frame"/>
|
||||
<child link="camera_link"/>
|
||||
<origin xyz="0 -0.2 0" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="camera_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.025 0.09 0.025"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
|
||||
<joint name="camera_optical_joint" type="fixed">
|
||||
<parent link="camera_link"/>
|
||||
<child link="camera_link_optical"/>
|
||||
<origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}"/>
|
||||
</joint>
|
||||
|
||||
<link name="camera_link_optical"></link>
|
||||
|
||||
|
||||
|
||||
<gazebo reference="camera_link">
|
||||
<material>Gazebo/Black</material>
|
||||
<sensor name="segmentation_camera" type="segmentation_camera">
|
||||
<ignition_frame_id>camera_link_optical</ignition_frame_id>
|
||||
<topic>semantic</topic>
|
||||
<camera>
|
||||
<horizontal_fov>1.047</horizontal_fov>
|
||||
<image>
|
||||
<width>640</width>
|
||||
<height>480</height>
|
||||
</image>
|
||||
<clip>
|
||||
<near>0.01</near>
|
||||
<far>60</far>
|
||||
</clip>
|
||||
</camera>
|
||||
<always_on>1</always_on>
|
||||
<update_rate>20</update_rate>
|
||||
<visualize>false</visualize>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,34 @@
|
|||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
|
||||
<material name="orange">
|
||||
<color rgba="1 0.3 0.1 1"/>
|
||||
</material>
|
||||
|
||||
<material name="blue">
|
||||
<color rgba="0.2 0.2 1 1"/>
|
||||
</material>
|
||||
|
||||
<material name="black">
|
||||
<color rgba="0 0 0 1"/>
|
||||
</material>
|
||||
|
||||
<material name="red">
|
||||
<color rgba="1 0 0 1"/>
|
||||
</material>
|
||||
|
||||
<material name="gray">
|
||||
<color rgba="0.753 0.753 0.753 1"/>
|
||||
</material>
|
||||
|
||||
<material name="grey">
|
||||
<color rgba="0.753 0.753 0.753 1"/>
|
||||
</material>
|
||||
|
||||
<material name="silver">
|
||||
<color rgba="0.88 0.88 0.88 1"/>
|
||||
</material>
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,77 @@
|
|||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" >
|
||||
|
||||
<!-- Specify some standard inertial calculations https://en.wikipedia.org/wiki/List_of_moments_of_inertia -->
|
||||
<!-- These make use of xacro's mathematical functionality -->
|
||||
|
||||
<xacro:macro name="inertial_sphere" params="mass radius *origin">
|
||||
<inertial>
|
||||
<xacro:insert_block name="origin"/>
|
||||
<mass value="${mass}" />
|
||||
<inertia ixx="${(2/5) * mass * (radius*radius)}" ixy="0.0" ixz="0.0"
|
||||
iyy="${(2/5) * mass * (radius*radius)}" iyz="0.0"
|
||||
izz="${(2/5) * mass * (radius*radius)}" />
|
||||
</inertial>
|
||||
</xacro:macro>
|
||||
|
||||
|
||||
<xacro:macro name="inertial_box" params="mass x y z *origin">
|
||||
<inertial>
|
||||
<xacro:insert_block name="origin"/>
|
||||
<mass value="${mass}" />
|
||||
<inertia ixx="${(1/12) * mass * (y*y+z*z)}" ixy="0.0" ixz="0.0"
|
||||
iyy="${(1/12) * mass * (x*x+z*z)}" iyz="0.0"
|
||||
izz="${(1/12) * mass * (x*x+y*y)}" />
|
||||
</inertial>
|
||||
</xacro:macro>
|
||||
|
||||
|
||||
<xacro:macro name="inertial_cylinder" params="mass length radius *origin">
|
||||
<inertial>
|
||||
<xacro:insert_block name="origin"/>
|
||||
<mass value="${mass}" />
|
||||
<inertia ixx="${(1/12) * mass * (3*radius*radius + length*length)}" ixy="0.0" ixz="0.0"
|
||||
iyy="${(1/12) * mass * (3*radius*radius + length*length)}" iyz="0.0"
|
||||
izz="${(1/2) * mass * (radius*radius)}" />
|
||||
</inertial>
|
||||
</xacro:macro>
|
||||
|
||||
<!-- Math constants -->
|
||||
<xacro:property name="math_pi" value="3.141592653589793" />
|
||||
<xacro:property name="math_pi_over_2" value="1.5707963267948966" />
|
||||
<xacro:property name="math_pi_over_4" value="0.785398163397448" />
|
||||
|
||||
<!-- Inertial for solid cuboid with dimensions x y z -->
|
||||
<xacro:macro name="solid_cuboid_inertial" params="rpy xyz mass x y z">
|
||||
<inertial>
|
||||
<origin rpy="${rpy}" xyz="${xyz}"/>
|
||||
<mass value="${mass}" />
|
||||
<inertia
|
||||
ixx="${mass * (y * y + z * z) / 12.0}" ixy="0.0" ixz="0.0"
|
||||
iyy="${mass * (x * x + z * z) / 12.0}" iyz="0.0"
|
||||
izz="${mass * (x * x + y * y) / 12.0}" />
|
||||
</inertial>
|
||||
</xacro:macro>
|
||||
|
||||
<!-- Inertial for solid cylinder with radius and length aligned to z-axis -->
|
||||
<xacro:macro name="solid_cylinder_inertial" params="rpy xyz mass radius length">
|
||||
<inertial>
|
||||
<origin rpy="${rpy}" xyz="${xyz}"/>
|
||||
<mass value="${mass}" />
|
||||
<inertia
|
||||
ixx="${mass * (3.0 * radius * radius + length * length) / 12.0}" ixy="0.0" ixz="0.0"
|
||||
iyy="${mass * (3.0 * radius * radius + length * length) / 12.0}" iyz="0.0"
|
||||
izz="${mass * (radius * radius) / 2.0}" />
|
||||
</inertial>
|
||||
</xacro:macro>
|
||||
|
||||
<!-- A null inertial - used in placeholder links to esure the model will work in Gazebo -->
|
||||
<xacro:macro name="null_inertial">
|
||||
<inertial>
|
||||
<mass value="0.001"/>
|
||||
<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
|
||||
</inertial>
|
||||
</xacro:macro>
|
||||
|
||||
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,67 @@
|
|||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" >
|
||||
|
||||
<xacro:include filename="inertial_macros.xacro"/>
|
||||
|
||||
<joint name="laser_joint" type="fixed">
|
||||
<parent link="chassis"/>
|
||||
<child link="laser_frame"/>
|
||||
<origin xyz="-1.1 0.0 1.0" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="laser_frame">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder radius="0.05" length="0.04"/>
|
||||
</geometry>
|
||||
<material name="red"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder radius="0.05" length="0.04"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<xacro:inertial_cylinder mass="0.1" length="0.04" radius="0.05">
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
</xacro:inertial_cylinder>
|
||||
</link>
|
||||
|
||||
|
||||
|
||||
<gazebo reference="laser_frame">
|
||||
<material>Gazebo/Red</material>
|
||||
<sensor name='gpu_lidar' type='gpu_lidar'>
|
||||
<topic>lidar</topic>
|
||||
<update_rate>5</update_rate>
|
||||
<ignition_frame_id>laser_frame</ignition_frame_id>
|
||||
<ray>
|
||||
<scan>
|
||||
<horizontal>
|
||||
<samples>640</samples>
|
||||
<resolution>1</resolution>
|
||||
<min_angle>${-90*deg_to_rad}</min_angle>
|
||||
<max_angle>${90*deg_to_rad}</max_angle>
|
||||
</horizontal>
|
||||
<vertical>
|
||||
<samples>64</samples>
|
||||
<resolution>1</resolution>
|
||||
<min_angle>${-45*deg_to_rad}</min_angle>
|
||||
<max_angle>${0*deg_to_rad}</max_angle>
|
||||
</vertical>
|
||||
</scan>
|
||||
<range>
|
||||
<min>0.08</min>
|
||||
<max>60.0</max>
|
||||
<resolution>0.01</resolution>
|
||||
</range>
|
||||
<noise>
|
||||
<type>gaussian</type>
|
||||
<mean>0.0</mean>
|
||||
<stddev>0.01</stddev>
|
||||
</noise>
|
||||
</ray>
|
||||
<alwaysOn>true</alwaysOn>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,24 @@
|
|||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="sensor_sim">
|
||||
|
||||
<!-- Define parameters-->
|
||||
<xacro:arg name="lidar_vertical_fov_upper" default="45"/>
|
||||
<xacro:arg name="lidar_vertical_fov_lower" default="-45"/>
|
||||
<xacro:arg name="lidar_horizontal_fov_upper" default="60"/>
|
||||
<xacro:arg name="lidar_horizontal_fov_lower" default="-60"/>
|
||||
<!-- Math constants -->
|
||||
<xacro:property name="deg_to_rad" value="0.01745329251994329577"/>
|
||||
|
||||
|
||||
|
||||
|
||||
<!-- Include the colours -->
|
||||
<xacro:include filename="colours.xacro" />
|
||||
<!-- Include the inertial macros -->
|
||||
<xacro:include filename="inertial_macros.xacro"/>
|
||||
|
||||
<xacro:include filename="robot_core.xacro" />
|
||||
<xacro:include filename="bbox_camera.xacro" />
|
||||
<xacro:include filename="lidar.xacro" />
|
||||
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,45 @@
|
|||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<link name="base_link">
|
||||
</link>
|
||||
|
||||
<!-- CHASSIS LINK -->
|
||||
|
||||
<joint name="chassis_joint" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="chassis"/>
|
||||
<origin xyz="0 0 0" rpy="0 0 ${-pi}"/>
|
||||
</joint>
|
||||
|
||||
<link name="chassis">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="file://$(find sensor_sim)/description/assets/ft24_asm.dae"/>
|
||||
</geometry>
|
||||
<material name="grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="2.0 2.0 0.1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1.0"/>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
|
||||
</link>
|
||||
|
||||
<gazebo reference="chassis">
|
||||
<material>Gazebo/White</material>
|
||||
<visual>
|
||||
<laser_retro>255</laser_retro>
|
||||
<plugin filename="ignition-gazebo-label-system" name="ignition::gazebo::systems::Label">
|
||||
<label>10</label>
|
||||
</plugin>
|
||||
</visual>
|
||||
</gazebo>
|
||||
|
||||
</robot>
|
||||
|
|
@ -0,0 +1 @@
|
|||
prepend-non-duplicate;IGN_GAZEBO_RESOURCE_PATH;share/@PROJECT_NAME@/worlds
|
||||
|
|
@ -0,0 +1,103 @@
|
|||
import os
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.actions import IncludeLaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch_ros.actions import Node
|
||||
from launch.substitutions import Command
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
# Check if we're told to use sim time
|
||||
lidar_vertical_fov_upper = LaunchConfiguration('lidar_vertical_fov_upper')
|
||||
lidar_vertical_fov_lower = LaunchConfiguration('lidar_vertical_fov_lower')
|
||||
lidar_horizontal_fov_upper = LaunchConfiguration('lidar_horizontal_fov_upper')
|
||||
lidar_horizontal_fov_lower = LaunchConfiguration('lidar_horizontal_fov_lower')
|
||||
|
||||
|
||||
# Process the URDF file
|
||||
package_name='sensor_sim' #<--- CHANGE ME
|
||||
pkg_path = os.path.join(get_package_share_directory(package_name))
|
||||
xacro_file = os.path.join(pkg_path,'description','robot.urdf.xacro')
|
||||
robot_description_config = Command(['xacro ', xacro_file, ' lidar_vertical_fov_upper:=', lidar_vertical_fov_upper, ' lidar_vertical_fov_lower:=', lidar_vertical_fov_lower, ' lidar_horizontal_fov_upper:=', lidar_horizontal_fov_upper, ' lidar_horizontal_fov_lower:=', lidar_horizontal_fov_lower])
|
||||
world_path = os.path.join(pkg_path,'worlds')
|
||||
|
||||
# Create a robot_state_publisher node
|
||||
params = {'robot_description': robot_description_config, 'use_sim_time': True, 'publish_frequency': 0.0001, 'use_tf_static': False}
|
||||
node_robot_state_publisher = Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
parameters=[params]
|
||||
)
|
||||
|
||||
node_joint_state_publisher = Node(
|
||||
package='joint_state_publisher',
|
||||
executable='joint_state_publisher',
|
||||
parameters=[{'use_sim_time': True}]
|
||||
)
|
||||
|
||||
gazebo = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([os.path.join(
|
||||
get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py')]),
|
||||
launch_arguments=[('gz_args', [f" -r -v 0 {world_path}/test_world.sdf"])],
|
||||
)
|
||||
|
||||
# Run the spawner node from the gazebo_ros package. The entity name doesn't really matter if you only have a single robot.
|
||||
spawn_entity = Node(package='ros_gz_sim', executable='create',
|
||||
arguments=[
|
||||
'-topic', 'robot_description',
|
||||
'-name', 'spawn_robot',
|
||||
'-z', '5',
|
||||
'-x', '0',
|
||||
],
|
||||
output='screen'
|
||||
)
|
||||
|
||||
# Bridge
|
||||
bridge = Node(
|
||||
package='ros_gz_bridge',
|
||||
executable='parameter_bridge',
|
||||
arguments=[
|
||||
'/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock',
|
||||
'/lidar/points@sensor_msgs/msg/PointCloud2@ignition.msgs.PointCloudPacked',
|
||||
],
|
||||
output='screen'
|
||||
)
|
||||
|
||||
lidar_visualizer = Node(
|
||||
package='sensor_sim',
|
||||
executable='lidar_visualizer.py',
|
||||
name='lidar_visualizer',
|
||||
output='screen'
|
||||
)
|
||||
|
||||
# Launch!
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument(
|
||||
'lidar_vertical_fov_upper',
|
||||
default_value='45',
|
||||
description='Upper limit of FOV of lidar'),
|
||||
DeclareLaunchArgument(
|
||||
'lidar_vertical_fov_lower',
|
||||
default_value='-45',
|
||||
description='Lower limit of FOV of lidar'),
|
||||
DeclareLaunchArgument(
|
||||
'lidar_horizontal_fov_upper',
|
||||
default_value='45',
|
||||
description='Upper limit of FOV of lidar'),
|
||||
DeclareLaunchArgument(
|
||||
'lidar_horizontal_fov_lower',
|
||||
default_value='-45',
|
||||
description='Lower limit of FOV of lidar'),
|
||||
node_robot_state_publisher,
|
||||
node_joint_state_publisher,
|
||||
gazebo,
|
||||
spawn_entity,
|
||||
bridge,
|
||||
lidar_visualizer
|
||||
])
|
||||
|
|
@ -0,0 +1,28 @@
|
|||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>sensor_sim </name>
|
||||
<version>0.0.0</version>
|
||||
<description>This package houses all ros2_control related code</description>
|
||||
<maintainer email="fake@email.com">Tim Korjakow</maintainer>
|
||||
<license>Todo</license>
|
||||
|
||||
<depend>ros2-control</depend>
|
||||
<depend>ros2-controllers</depend>
|
||||
<depend>ros-gz</depend>
|
||||
<depend>robot-localization</depend>
|
||||
<depend>joint-state-publisher</depend>
|
||||
<depend>twist-mux</depend>
|
||||
<depend>gz-ros2-control</depend>
|
||||
<depend>state-publisher</depend>
|
||||
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
|
|
@ -0,0 +1,53 @@
|
|||
#!/usr/bin/env python3
|
||||
|
||||
from time import sleep
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
|
||||
from std_msgs.msg import String
|
||||
from sensor_msgs.msg import PointCloud2
|
||||
import numpy as np
|
||||
from sensor_msgs_py import point_cloud2
|
||||
from numpy.lib.recfunctions import structured_to_unstructured, unstructured_to_structured
|
||||
from PIL import Image
|
||||
|
||||
|
||||
|
||||
class LidarSubscriber(Node):
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('lidar_visualizer')
|
||||
self.subscription = self.create_subscription(
|
||||
PointCloud2,
|
||||
'/lidar/points',
|
||||
self.listener_callback,
|
||||
10)
|
||||
self.subscription # prevent unused variable warning
|
||||
|
||||
def listener_callback(self, msg):
|
||||
# read intensity data from PointCloud2 efficiently to numpy array
|
||||
intensities = point_cloud2.read_points(msg, field_names=['intensity'], reshape_organized_cloud=True)
|
||||
intensities = structured_to_unstructured(intensities)
|
||||
|
||||
print(f"The number of points hitting the car is {(intensities>254).sum()}. This is a ratio of {(intensities>254).sum()/len(intensities)}%")
|
||||
exit()
|
||||
|
||||
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
|
||||
minimal_subscriber = LidarSubscriber()
|
||||
|
||||
rclpy.spin(minimal_subscriber)
|
||||
|
||||
# Destroy the node explicitly
|
||||
# (optional - otherwise it will be done automatically
|
||||
# when the garbage collector destroys the node object)
|
||||
minimal_subscriber.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
|
@ -0,0 +1,188 @@
|
|||
<?xml version="1.0" ?>
|
||||
<sdf version="1.6">
|
||||
<world name="empty">
|
||||
<plugin
|
||||
filename="ignition-gazebo-physics-system"
|
||||
name="ignition::gazebo::systems::Physics">
|
||||
</plugin>
|
||||
<plugin
|
||||
filename="ignition-gazebo-user-commands-system"
|
||||
name="ignition::gazebo::systems::UserCommands">
|
||||
</plugin>
|
||||
<plugin
|
||||
filename="ignition-gazebo-scene-broadcaster-system"
|
||||
name="ignition::gazebo::systems::SceneBroadcaster">
|
||||
</plugin>
|
||||
<plugin
|
||||
filename="ignition-gazebo-contact-system"
|
||||
name="ignition::gazebo::systems::Contact">
|
||||
</plugin>
|
||||
<plugin
|
||||
filename="ignition-gazebo-sensors-system"
|
||||
name="ignition::gazebo::systems::Sensors">
|
||||
<render_engine>ogre2</render_engine>
|
||||
</plugin>
|
||||
|
||||
<gui fullscreen="0">
|
||||
|
||||
<!-- 3D scene -->
|
||||
<plugin filename="MinimalScene" name="3D View">
|
||||
<ignition-gui>
|
||||
<title>3D View</title>
|
||||
<property type="bool" key="showTitleBar">false</property>
|
||||
<property type="string" key="state">docked</property>
|
||||
</ignition-gui>
|
||||
|
||||
<engine>ogre2</engine>
|
||||
<scene>scene</scene>
|
||||
<ambient_light>0.4 0.4 0.4</ambient_light>
|
||||
<background_color>0.8 0.8 0.8</background_color>
|
||||
<camera_pose>-6 0 6 0 0.5 0</camera_pose>
|
||||
</plugin>
|
||||
|
||||
<!-- Plugins that add functionality to the scene -->
|
||||
<plugin filename="EntityContextMenuPlugin" name="Entity context menu">
|
||||
<ignition-gui>
|
||||
<property key="state" type="string">floating</property>
|
||||
<property key="width" type="double">5</property>
|
||||
<property key="height" type="double">5</property>
|
||||
<property key="showTitleBar" type="bool">false</property>
|
||||
</ignition-gui>
|
||||
</plugin>
|
||||
<plugin filename="GzSceneManager" name="Scene Manager">
|
||||
<ignition-gui>
|
||||
<property key="resizable" type="bool">false</property>
|
||||
<property key="width" type="double">5</property>
|
||||
<property key="height" type="double">5</property>
|
||||
<property key="state" type="string">floating</property>
|
||||
<property key="showTitleBar" type="bool">false</property>
|
||||
</ignition-gui>
|
||||
</plugin>
|
||||
<plugin filename="InteractiveViewControl" name="Interactive view control">
|
||||
<ignition-gui>
|
||||
<property key="resizable" type="bool">false</property>
|
||||
<property key="width" type="double">5</property>
|
||||
<property key="height" type="double">5</property>
|
||||
<property key="state" type="string">floating</property>
|
||||
<property key="showTitleBar" type="bool">false</property>
|
||||
</ignition-gui>
|
||||
</plugin>
|
||||
<plugin filename="CameraTracking" name="Camera Tracking">
|
||||
<ignition-gui>
|
||||
<property key="resizable" type="bool">false</property>
|
||||
<property key="width" type="double">5</property>
|
||||
<property key="height" type="double">5</property>
|
||||
<property key="state" type="string">floating</property>
|
||||
<property key="showTitleBar" type="bool">false</property>
|
||||
</ignition-gui>
|
||||
</plugin>
|
||||
<!-- World control -->
|
||||
<plugin filename="WorldControl" name="World control">
|
||||
<ignition-gui>
|
||||
<title>World control</title>
|
||||
<property type="bool" key="showTitleBar">false</property>
|
||||
<property type="bool" key="resizable">false</property>
|
||||
<property type="double" key="height">72</property>
|
||||
<property type="double" key="width">121</property>
|
||||
<property type="double" key="z">1</property>
|
||||
|
||||
<property type="string" key="state">floating</property>
|
||||
<anchors target="3D View">
|
||||
<line own="left" target="left"/>
|
||||
<line own="bottom" target="bottom"/>
|
||||
</anchors>
|
||||
</ignition-gui>
|
||||
|
||||
<play_pause>true</play_pause>
|
||||
<step>true</step>
|
||||
<start_paused>true</start_paused>
|
||||
<use_event>true</use_event>
|
||||
|
||||
</plugin>
|
||||
|
||||
<!-- World statistics -->
|
||||
<plugin filename="WorldStats" name="World stats">
|
||||
<ignition-gui>
|
||||
<title>World stats</title>
|
||||
<property type="bool" key="showTitleBar">false</property>
|
||||
<property type="bool" key="resizable">false</property>
|
||||
<property type="double" key="height">110</property>
|
||||
<property type="double" key="width">290</property>
|
||||
<property type="double" key="z">1</property>
|
||||
|
||||
<property type="string" key="state">floating</property>
|
||||
<anchors target="3D View">
|
||||
<line own="right" target="right"/>
|
||||
<line own="bottom" target="bottom"/>
|
||||
</anchors>
|
||||
</ignition-gui>
|
||||
|
||||
<sim_time>true</sim_time>
|
||||
<real_time>true</real_time>
|
||||
<real_time_factor>true</real_time_factor>
|
||||
<iterations>true</iterations>
|
||||
</plugin>
|
||||
|
||||
<!-- Inspector -->
|
||||
<plugin filename="ComponentInspector" name="Component inspector">
|
||||
<ignition-gui>
|
||||
<property type="string" key="state">docked_collapsed</property>
|
||||
</ignition-gui>
|
||||
</plugin>
|
||||
|
||||
<!-- Entity tree -->
|
||||
<plugin filename="EntityTree" name="Entity tree">
|
||||
<ignition-gui>
|
||||
<property type="string" key="state">docked_collapsed</property>
|
||||
</ignition-gui>
|
||||
</plugin>
|
||||
|
||||
<!-- Image Display Plugins for visualization -->
|
||||
<plugin filename="ImageDisplay" name="Image Display">
|
||||
<ignition-gui>
|
||||
</ignition-gui>
|
||||
<topic>semantic/colored_map</topic>
|
||||
</plugin>
|
||||
</gui>
|
||||
|
||||
<light type="directional" name="sun">
|
||||
<pose>0 0 10 0 0 0</pose>
|
||||
<diffuse>0.8 0.8 0.8 1</diffuse>
|
||||
<specular>0.2 0.2 0.2 1</specular>
|
||||
<attenuation>
|
||||
<range>1000</range>
|
||||
<constant>0.9</constant>
|
||||
<linear>0.01</linear>
|
||||
<quadratic>0.001</quadratic>
|
||||
</attenuation>
|
||||
<direction>-0.5 0.1 -0.9</direction>
|
||||
</light>
|
||||
|
||||
<model name="ground_plane">
|
||||
<static>true</static>
|
||||
<link name="link">
|
||||
<collision name="collision">
|
||||
<geometry>
|
||||
<plane>
|
||||
<normal>0 0 1</normal>
|
||||
<size>100 100</size>
|
||||
</plane>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name="visual">
|
||||
<geometry>
|
||||
<plane>
|
||||
<normal>0 0 1</normal>
|
||||
<size>100 100</size>
|
||||
</plane>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>0.8 0.8 0.8 1</ambient>
|
||||
<diffuse>0.8 0.8 0.8 1</diffuse>
|
||||
<specular>0.8 0.8 0.8 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
</world>
|
||||
</sdf>
|
||||
Loading…
Reference in New Issue