Skip to content
Snippets Groups Projects
Commit b36e1796 authored by Pascal's avatar Pascal
Browse files

[isae_simulations_package] - first commit.

Environment of room 07-009.
clone this (ros2) package in a colcon workspace, compile, source, and you can launch directly with:
ros2 launch isae_simulations_pkg load_testlabo_world_into_gazebo.launch.py

Once you have launched the simulation, you can retreive models in gazebo insert tab and add into the world.

[models] - Add model of rover and sensors (lidar and camera).

TODO:
add others models in models dir.
add others worlds in worlds dir.
parent 1b27c8b9
Branches
No related tags found
No related merge requests found
Showing
with 718 additions and 0 deletions
cmake_minimum_required(VERSION 3.8)
project(isae_simulations_package)
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)
find_package(gazebo_ros_pkgs REQUIRED)
find_package(rviz2 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
launch
models
worlds
DESTINATION
share/${PROJECT_NAME}/
)
ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/${PROJECT_NAME}.sh.in")
ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/${PROJECT_NAME}.dsv.in")
ament_package()
prepend-non-duplicate;GAZEBO_MODEL_PATH;share/@PROJECT_NAME@/models
prepend-non-duplicate;GAZEBO_RESOURCE_PATH;share/@PROJECT_NAME@
# generated from dolly_gazebo/env-hooks/dolly_gazebo.sh.in
ament_prepend_unique_value GAZEBO_MODEL_PATH "$AMENT_CURRENT_PREFIX/share/@PROJECT_NAME@/models"
ament_prepend_unique_value GAZEBO_RESOURCE_PATH "$AMENT_CURRENT_PREFIX/share/@PROJECT_NAME@"
# from Author: Addison Sears-Collins
# Date: July 06, 2021
# Description: Load isae world file into Gazebo.
import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.conditions import IfCondition, UnlessCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import Command, LaunchConfiguration, PythonExpression
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
# Set the path to the Gazebo ROS package
pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros')
# Set the path to this package.
pkg_share = FindPackageShare(package='isae_simulations_pkg').find('isae_simulations_pkg')
# Set the path to the world file
world_file_name = 'isaetesttemp.world'
world_path = os.path.join(pkg_share, 'worlds','isae_room-07009', world_file_name)
########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ##############
# Launch configuration variables specific to simulation
headless = LaunchConfiguration('headless')
use_sim_time = LaunchConfiguration('use_sim_time')
use_simulator = LaunchConfiguration('use_simulator')
world = LaunchConfiguration('world')
declare_simulator_cmd = DeclareLaunchArgument(
name='headless',
default_value='False',
description='Whether to execute gzclient')
declare_use_sim_time_cmd = DeclareLaunchArgument(
name='use_sim_time',
default_value='true',
description='Use simulation (Gazebo) clock if true')
declare_use_simulator_cmd = DeclareLaunchArgument(
name='use_simulator',
default_value='True',
description='Whether to start the simulator')
declare_world_cmd = DeclareLaunchArgument(
name='world',
default_value=world_path,
description='Full path to the world model file to load')
# Specify the actions
# Start Gazebo server
start_gazebo_server_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')),
condition=IfCondition(use_simulator),
launch_arguments={'world': world}.items())
# Start Gazebo client
start_gazebo_client_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')),
condition=IfCondition(PythonExpression([use_simulator, ' and not ', headless])))
# Create the launch description and populate
ld = LaunchDescription()
# Declare the launch options
ld.add_action(declare_simulator_cmd)
ld.add_action(declare_use_sim_time_cmd)
ld.add_action(declare_use_simulator_cmd)
ld.add_action(declare_world_cmd)
# Add any actions
ld.add_action(start_gazebo_server_cmd)
ld.add_action(start_gazebo_client_cmd)
return ld
<?xml version="1.0"?>
<model>
<name>blue_box</name>
<version>1.0</version>
<sdf version='1.6'>model.sdf</sdf>
<author>
<name>pascal</name>
<email>p.chauvin@isae-supaero.fr</email>
</author>
<description>
simple blue box.
</description>
</model>
<?xml version='1.0'?>
<sdf version='1.6'>
<model name="blue_box">
<!--pose>2.0 0 0.05 0 0 0</pose-->
<link name="link">
<collision name="collision">
<geometry>
<box>
<size>0.06 0.04 0.02</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.06 0.04 0.02</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Blue</name>
</script>
</material>
</visual>
</link>
<plugin name='planar_move' filename='libgazebo_ros_planar_move.so'>
<ros>
<namespace>/bluebox</namespace>
<argument>cmd_vel:=cmd_vel</argument>
<argument>odom:=odom_demo</argument>
</ros>
<update_rate>100</update_rate>
<publish_rate>10</publish_rate>
<!-- output -->
<publish_odom>false</publish_odom>
<publish_odom_tf>false</publish_odom_tf>
<odometry_frame>odom_demo</odometry_frame>
<robot_base_frame>link</robot_base_frame>
<covariance_x>0.0001</covariance_x>
<covariance_y>0.0001</covariance_y>
<covariance_yaw>0.01</covariance_yaw>
</plugin>
</model>
</sdf>
<?xml version='1.0'?>
<sdf version='1.6'>
<model name="camera">
<static>false</static>
<link name='cameralink'>
<collision name="collision">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
</visual>
<sensor type="camera" name="sensor_name_camera">
...
<!-- Set always_on only sensor, not on plugin -->
<always_on>0</always_on>
<!-- Set update_rate only sensor, not on plugin -->
<update_rate>1</update_rate>
<camera name="camera_name">
...
<distortion>
<k1>0.1</k1>
<k2>0.2</k2>
<k3>0.3</k3>
<p1>0.4</p1>
<p2>0.5</p2>
<center>0.5 0.5</center>
</distortion>
</camera>
<!-- Use camera, not camera_triggered -->
<plugin name="plugin_name" filename="libgazebo_ros_camera.so">
<!-- Change namespace, camera name and topics so -
* Images are published to: /custom_ns/custom_camera/custom_image
* Camera info is published to: /custom_ns/custom_camera/custom_info
* Trigger is received on: /custom_ns/custom_camera/custom_trigger
-->
<ros>
<namespace>custom_ns</namespace>
<remapping>image_raw:=custom_img</remapping>
<remapping>camera_info:=custom_info</remapping>
<remapping>image_trigger:=custom_trigger</remapping>
</ros>
<!-- Set camera name. If empty, defaults to sensor name (i.e. "sensor_name") -->
<camera_name>custom_camera</camera_name>
<!-- Set TF frame name. If empty, defaults to link name (i.e. "link_name") -->
<frame_name>custom_frame</frame_name>
<!-- Set to true to turn on triggering -->
<triggered>true</triggered>
<hack_baseline>0.07</hack_baseline>
<!-- No need to repeat distortion parameters or to set autoDistortion -->
</plugin>
</sensor>
</link>
</model>
</sdf>
<?xml version="1.0"?>
<model>
<name>camera</name>
<version>1.0</version>
<sdf version='1.6'>model.sdf</sdf>
<author>
<name>pascal</name>
<email>youremail@isae-supaero.fr</email>
</author>
<description>
simple camera.
</description>
</model>
<?xml version='1.0'?>
<sdf version='1.6'>
<model name="camera">
<static>false</static>
<!--pose> 0.11 0 0.195011 0 0 0</pose-->
<pose> 0.2 0 0.12 0 0 0</pose>
<link name='cameralink'>
<collision name="collision">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
</visual>
<!-- camera -->
<sensor type="camera" name="camera1">
<visualize>true</visualize>
<update_rate>30.0</update_rate>
<camera name="head">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>640</width>
<height>480</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame.
That pixel's noise value is added to each of its color
channels, which at that point lie in the range [0,1]. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>rrbot/camera1</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>camera_link</frameName>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</link>
</model>
</sdf>
<?xml version="1.0"?>
<model>
<name>red_box</name>
<version>1.0</version>
<sdf version='1.6'>model.sdf</sdf>
<author>
<name>pascal</name>
<email>p.chauvin@isae-supaero.fr</email>
</author>
<description>
simple red box.
</description>
</model>
<?xml version='1.0'?>
<sdf version='1.6'>
<model name="red_box">
<!--pose>2.0 0 0.05 0 0 0</pose-->
<link name="link">
<collision name="collision">
<geometry>
<box>
<size>0.06 0.04 0.02</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.06 0.04 0.02</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Red</name>
</script>
</material>
</visual>
</link>
<plugin name='planar_move' filename='libgazebo_ros_planar_move.so'>
<ros>
<namespace>/redbox</namespace>
<argument>cmd_vel:=cmd_vel</argument>
<argument>odom:=odom_demo</argument>
</ros>
<update_rate>100</update_rate>
<publish_rate>10</publish_rate>
<!-- output -->
<publish_odom>false</publish_odom>
<publish_odom_tf>false</publish_odom_tf>
<odometry_frame>odom_demo</odometry_frame>
<robot_base_frame>link</robot_base_frame>
<covariance_x>0.0001</covariance_x>
<covariance_y>0.0001</covariance_y>
<covariance_yaw>0.01</covariance_yaw>
</plugin>
</model>
</sdf>
<?xml version="1.0"?>
<model>
<name>Rover</name>
<version>1.0</version>
<sdf version='1.6'>model.sdf</sdf>
<author>
<name>Your name</name>
<email>youremail@isae-supaero.fr</email>
</author>
<description>
My awesome robot.
</description>
</model>
<?xml version='1.0'?>
<sdf version='1.6'>
<model name="rover">
<static>false</static>
<pose>0 0 0.05 0 0 0</pose>
<link name='chassis'>
<pose>0 0 0.05 0 0 0</pose>
<collision name="collision">
<geometry>
<box>
<size>0.4 0.2 0.1</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.4 0.2 0.1</size>
</box>
</geometry>
</visual>
</link>
<link name="left_wheel">
<pose>.09 0.1 0 1.57 0 0</pose>
<collision name="collision">
<geometry>
<cylinder>
<radius>.06</radius>
<length>.03</length>
</cylinder>
</geometry>
</collision>
<visual name="visual">
<geometry>
<cylinder>
<radius>.06</radius>
<length>.03</length>
</cylinder>
</geometry>
</visual>
</link>
<link name="left_wheel2">
<pose>-.09 0.1 0 1.57 0 0</pose>
<collision name="collision">
<geometry>
<cylinder>
<radius>.06</radius>
<length>.03</length>
</cylinder>
</geometry>
</collision>
<visual name="visual">
<geometry>
<cylinder>
<radius>.06</radius>
<length>.03</length>
</cylinder>
</geometry>
</visual>
</link>
<link name="right_wheel">
<pose>.09 -0.1 0 1.57 0 0</pose>
<collision name="collision">
<geometry>
<cylinder>
<radius>.06</radius>
<length>.03</length>
</cylinder>
</geometry>
</collision>
<visual name="visual">
<geometry>
<cylinder>
<radius>.06</radius>
<length>.03</length>
</cylinder>
</geometry>
</visual>
</link>
<link name="right_wheel2">
<pose>-.09 -0.1 0 1.57 0 0</pose>
<collision name="collision">
<geometry>
<cylinder>
<radius>.06</radius>
<length>.03</length>
</cylinder>
</geometry>
</collision>
<visual name="visual">
<geometry>
<cylinder>
<radius>.06</radius>
<length>.03</length>
</cylinder>
</geometry>
</visual>
</link>
<joint type="revolute" name="left_wheel_hinge">
<pose>0 0 0 0 0 0</pose>
<child>left_wheel</child>
<parent>chassis</parent>
<axis>
<xyz>0 0 1</xyz>
</axis>
</joint>
<joint type="revolute" name="left_wheel_hinge2">
<pose>0 0 0 0 0 0</pose>
<child>left_wheel2</child>
<parent>chassis</parent>
<axis>
<xyz>0 0 1</xyz>
</axis>
</joint>
<joint type="revolute" name="right_wheel_hinge">
<pose>0 0 0 0 0 0</pose>
<child>right_wheel</child>
<parent>chassis</parent>
<axis>
<xyz>0 0 1</xyz>
</axis>
</joint>
<joint type="revolute" name="right_wheel_hinge2">
<pose>0 0 0 0 0 0</pose>
<child>right_wheel2</child>
<parent>chassis</parent>
<axis>
<xyz>0 0 1</xyz>
</axis>
</joint>
<plugin name="object_controller" filename="libgazebo_ros_planar_move.so">
<commandTopic>cmd_vel</commandTopic>
<odometryTopic>odom</odometryTopic>
<odometryFrame>odom</odometryFrame>
<odometryRate>20.0</odometryRate>
<robotBaseFrame>base_footprint</robotBaseFrame>
</plugin>
</model>
</sdf>
<?xml version="1.0"?>
<model>
<name>rovercamera</name>
<version>1.0</version>
<sdf version='1.6'>model.sdf</sdf>
<author>
<name>Your name</name>
<email>youremail@isae-supaero.fr</email>
</author>
<description>
My awesome Robot with his telemeter.
</description>
</model>
<?xml version="1.0" ?>
<sdf version="1.6">
<model name="RoverCamera">
<include>
<uri>model://rover</uri>
</include>
<include>
<uri>model://camera</uri>
</include>
<joint type="fixed" name="camera_joint">
<pose>0 0 0 0 0 0</pose>
<child>camera::cameralink</child>
<parent>rover::chassis</parent>
</joint>
</model>
</sdf>
<?xml version="1.0"?>
<model>
<name>RoverSensor</name>
<version>1.0</version>
<sdf version='1.6'>model.sdf</sdf>
<author>
<name>Your name</name>
<email>youremail@isae-supaero.fr</email>
</author>
<description>
My awesome Robot with his telemeter.
</description>
</model>
<?xml version="1.0" ?>
<sdf version="1.6">
<model name="RoverSensor">
<include>
<uri>model://rover</uri>
</include>
<include>
<uri>model://sensor</uri>
</include>
<joint type="fixed" name="telemeter_joint">
<pose>0 0 0 0 0 0</pose>
<child>sensor::head</child>
<parent>rover::chassis</parent>
</joint>
</model>
</sdf>
<?xml version="1.0"?>
<model>
<name>Sensor</name>
<version>1.0</version>
<sdf version='1.6'>model.sdf</sdf>
<author>
<name>Your name</name>
<email>youremail@isae-supaero.fr</email>
</author>
<description>
My awesome telemeter.
</description>
</model>
<?xml version='1.0'?>
<sdf version='1.6'>
<model name="sensor">
<static>false</static>
<pose> 0.15 0 0.185011 0 0 0</pose>
<link name='head'>
<collision name="collision">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
</visual>
<sensor name="laser" type="ray">
<pose>0 0 0 0 0 0</pose>
<ray>
<scan>
<horizontal>
<samples>181</samples>
<resolution>1</resolution>
<min_angle>-0.5</min_angle>
<max_angle>0.5</max_angle>
</horizontal>
</scan>
<range>
<min>0.08</min>
<max>10</max>
<resolution>0.05</resolution>
</range>
</ray>
<always_on>1</always_on>
<update_rate>10</update_rate>
<visualize>true</visualize>
<plugin name='laser' filename='libgazebo_ros_ray_sensor.so'>
<ros>
<namespace>/demo</namespace>
<argument>--ros-args --remap ~/out:=scan</argument>
</ros>
<output_type>sensor_msgs/LaserScan</output_type>
</plugin>
</sensor>
</link>
</model>
</sdf>
<?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>isae_simulations_package</name>
<version>0.0.0</version>
<description>isae simulations package</description>
<maintainer email="p.chauvin@isae-supaero.fr">pascal</maintainer>
<license>ISAE_2022</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>gazebo_ros_pkgs</depend>
<!--depend>rviz2</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% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment