Commit 6dfa15f8 authored by Johannes Bier's avatar Johannes Bier 💬
Browse files

Files from dschubba gazebo

parent 3b1f9680
devel/
logs/
build/
bin/
lib/
msg_gen/
srv_gen/
msg/*Action.msg
msg/*ActionFeedback.msg
msg/*ActionGoal.msg
msg/*ActionResult.msg
msg/*Feedback.msg
msg/*Goal.msg
msg/*Result.msg
msg/_*.py
build_isolated/
devel_isolated/
# Generated by dynamic reconfigure
*.cfgc
/cfg/cpp/
/cfg/*.py
# Ignore generated docs
*.dox
*.wikidoc
# eclipse stuff
.project
.cproject
# qcreator stuff
CMakeLists.txt.user
srv/_*.py
*.pcd
*.pyc
qtcreator-*
*.user
/planning/cfg
/planning/docs
/planning/src
*~
# Emacs
.#*
# Catkin custom files
CATKIN_IGNORE
# generated files
generated.world
.ipynb_checkpoints
# Kamaro World
Description
## Installation
This package is written for `ROS melodic` and `ROS noetic`.
Additional you'll need the following packages:
```
sudo apt install ros-melodic-gazebo-ros-pkgs \
ros-melodic-robot-state-publisher \
ros-melodic-effort-controllers \
ros-melodic-joint-state-controller \
ros-melodic-position-controllers \
ros-melodic-diff-drive-controller \
ros-melodic-rqt-robot-steering
sudo apt install ros-noetic-gazebo-ros-pkgs \
ros-noetic-robot-state-publisher \
ros-noetic-effort-controllers \
ros-noetic-joint-state-controller \
ros-noetic-position-controllers \
ros-noetic-diff-drive-controller \
ros-noetic-rqt-robot-steerin
```
## Generating pumkin worlds
This package includes a script (`scripts/generate_world.py`) that can generate randomized pumpkin worlds. All parameters are optional and have default values. You can call the script using
```bash
conda activate # this script needs a python3 environment that has jinja installed (pip install Jinja2)
rosrun dschubba_gazebo generate_world.py --num_rows_left=1 --num_rows_right=1 --num_pumpkin_pairs=20 --max_angle_variation=0.3
```
The resulting file will be placed in `worlds/generated.world`.
```
usage: generate_world.py [-h] [--pumpkin_radius PUMPKIN_RADIUS]
[--row_width ROW_WIDTH]
[--pumpkin_offset PUMPKIN_OFFSET]
[--max_angle_variation MAX_ANGLE_VARIATION]
[--num_pumpkin_pairs NUM_PUMPKIN_PAIRS]
[--num_rows_left NUM_ROWS_LEFT]
[--num_rows_right NUM_ROWS_RIGHT]
[--pumpkin_height PUMPKIN_HEIGHT]
[--pumpkin_mass PUMPKIN_MASS]
[--radius_noise_range RADIUS_NOISE_RANGE]
[--seed SEED]
generate a world full of cylinders
optional arguments:
-h, --help show this help message and exit
--pumpkin_radius PUMPKIN_RADIUS
default_value: 0.15
--row_width ROW_WIDTH
default_value: 0.7
--pumpkin_offset PUMPKIN_OFFSET
default_value: 0.6
--max_angle_variation MAX_ANGLE_VARIATION
default_value: 0.08726646259971647
--num_pumpkin_pairs NUM_PUMPKIN_PAIRS
default_value: 5
--num_rows_left NUM_ROWS_LEFT
default_value: 2
--num_rows_right NUM_ROWS_RIGHT
default_value: 3
--pumpkin_height PUMPKIN_HEIGHT
default_value: 0.3
--pumpkin_mass PUMPKIN_MASS
default_value: 5.0
--radius_noise_range RADIUS_NOISE_RANGE
default_value: 0.05
--seed SEED default_value: None
```
<launch>
<!-- these are the arguments you can pass this launch file, for example paused:=true -->
<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="vehicle"/>
<arg name="world_path" default="$(find kamaro_world)/worlds" />
<arg name="world" default="empty.world" />
<arg name="verbose" default="true" />
<arg name="vm" default="false" />
<!-- Only needed in VM -->
<env if="$(arg vm)" name="SVGA_VGPU10" value="0"/>
<!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(arg world_path)/$(arg world)" />
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="headless" value="$(arg headless)" />
<arg name="verbose" value="$(arg verbose)" />
</include>
</launch>
This diff is collapsed.
URL: https://sketchfab.com/3d-models/mini-pumpkin-vcu-3d-2280-98548dea9cd74e34be99b936b60abb14
Author: Virtual Curation Lab
License: CC BY-SA 4.0
This diff is collapsed.
URL: https://sketchfab.com/3d-models/pumpkin-5866a5b13bac4a01918d2b1eb80ad2ff
Author: Maurice Svay
License: CC BY 4.0
This source diff could not be displayed because it is too large. You can view the blob instead.
URL: https://sketchfab.com/3d-models/pumpkin-509e960db3b3443382b0a189f5abfae0
Author: wojciechmiedziocha
License: CC BY 4.0
This diff is collapsed.
#!/bin/bash
exec python$ROS_PYTHON_VERSION - << EOF
import rospy
import tf
from tf.transformations import euler_from_quaternion
from std_msgs.msg import Float64
from math import pi, atan2, sqrt
rospy.init_node('distance_sim', log_level=rospy.DEBUG)
distance_pub = rospy.Publisher("distance", Float64, queue_size=1)
tf_listener = tf.TransformListener()
wheel_left = None
wheel_right = None
distance = 0
def get_angle(q):
signum = 1 if q[1] > 0 else -1
return signum * 2 * atan2(sqrt(q[0] ** 2 + q[1] ** 2 + q[2] ** 2), q[3])
rate = rospy.Rate(12) # Hz
while not rospy.is_shutdown():
try:
wheel_left_quat = tf_listener.lookupTransform("base_link", "wheel_front_left_link", rospy.Time(0))[1]
wheel_right_quat = tf_listener.lookupTransform("base_link", "wheel_front_right_link", rospy.Time(0))[1]
wheel_left_new = get_angle(wheel_left_quat)
wheel_right_new = get_angle(wheel_right_quat)
except Exception as e:
print("Failed to get transforms: ")
print(e)
rate.sleep()
continue
if wheel_left is not None:
d_left = (((wheel_left_new - wheel_left) + pi) % (2*pi) - pi) * 0.085
print(wheel_left_new, d_left)
d_right = (((wheel_right_new - wheel_right) + pi) % (2*pi) - pi) * 0.085
distance += (d_left + d_right) / 2
wheel_left = wheel_left_new
wheel_right = wheel_right_new
distance_pub.publish(Float64(distance))
rate.sleep()
EOF
#!/usr/bin/env python3
from pumpkin_2d_generator import Pumpkin2DGenerator
import rospkg
import argparse
import inspect
import os
if __name__ == "__main__":
# get the possible arguments of the generator and default values
argspec = inspect.getfullargspec(Pumpkin2DGenerator.__init__)
possible_kwargs = argspec.args[1:]
defaults = argspec.defaults
# construct an ArgumentParser that takes these arguments
parser = argparse.ArgumentParser(description='generate a world full of cylinders')
for argname, default in zip(possible_kwargs, defaults):
# we analyze the default value's type to guess the type for that argument
parser.add_argument("--"+argname, type=type(default), help='default_value: {}'.format(default), required=False)
# get a dict representation of the arguments and call our constructor with them as kwargs
args = vars(parser.parse_args())
args = {k:v for k,v in args.items() if v is not None}
pk = Pumpkin2DGenerator(**args)
# generate the template and write it to a file
pkg_path = rospkg.RosPack().get_path('dschubba_gazebo')
template_path = os.path.join(pkg_path, "scripts/pumpkin.world.template")
template = open(template_path).read()
generated_sdf = pk.render_to_template(template)
out_path = os.path.join(pkg_path, "worlds/generated.world")
with open(out_path, "w") as f:
f.write(generated_sdf)
\ No newline at end of file
<sdf version='1.6'>
<world name='default'>
<light name='sun' type='directional'>
<cast_shadows>1</cast_shadows>
<pose frame=''>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>1</static>
<link name='link'>
<collision name='collision'>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<surface>
<contact>
<collide_bitmask>65535</collide_bitmask>
<ode/>
</contact>
<friction>
<ode>
<mu>100</mu>
<mu2>50</mu2>
</ode>
<torsional>
<ode/>
</torsional>
</friction>
<bounce/>
</surface>
<max_contacts>10</max_contacts>
</collision>
<visual name='visual'>
<cast_shadows>0</cast_shadows>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<gravity>0 0 -9.8</gravity>
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
<atmosphere type='adiabatic'/>
<physics name='default_physics' default='0' type='ode'>
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
</physics>
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
<background>0.7 0.7 0.7 1</background>
<shadows>1</shadows>
</scene>
<audio>
<device>default</device>
</audio>
<wind/>
<spherical_coordinates>
<surface_model>EARTH_WGS84</surface_model>
<latitude_deg>0</latitude_deg>
<longitude_deg>0</longitude_deg>
<elevation>0</elevation>
<heading_deg>0</heading_deg>
</spherical_coordinates>
<state world_name='default'>
<sim_time>29 706000000</sim_time>
<real_time>29 884195945</real_time>
<wall_time>1582227173 790094351</wall_time>
<iterations>29706</iterations>
<model name='ground_plane'>
<pose frame=''>0 0 0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose frame=''>0 0 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<light name='sun'>
<pose frame=''>0 0 10 0 -0 0</pose>
</light>
</state>
{% for coordinate in coordinates %}
{% if coordinate.type == 'cylinder' %}
<model name='{{ coordinate.name }}'>
<pose frame=''>{{ coordinate.x }} {{ coordinate.y }} 0.5 0 -0 0</pose>
<link name='{{ coordinate.name }}_link'>
<inertial>
<mass>{{ coordinate.mass }}</mass>
<inertia>
<ixx>{{ coordinate.inertia.ixx }}</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>{{ coordinate.inertia.iyy }}</iyy>
<iyz>0</iyz>
<izz>{{ coordinate.inertia.izz }}</izz>
</inertia>
</inertial>
<collision name='collision'>
<geometry>
<cylinder>
<radius>{{ coordinate.radius }}</radius>
<length>{{ coordinate.height }}</length>
</cylinder>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<cylinder>
<radius>{{ coordinate.radius }}</radius>
<length>{{ coordinate.height }}</length>
</cylinder>
</geometry>
<material>
<script>
<name>Gazebo/Red</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
{% elif coordinate.type == 'pumpkin_01' %}
<model name='{{ coordinate.name }}'>
<pose frame=''>{{ coordinate.x }} {{ coordinate.y }} 0.5 0 -0 0</pose>
<link name='{{ coordinate.name }}_link'>
<inertial>
<mass>{{ coordinate.mass }}</mass>
<inertia>
<ixx>{{ coordinate.inertia.ixx }}</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>{{ coordinate.inertia.iyy }}</iyy>
<iyz>0</iyz>
<izz>{{ coordinate.inertia.izz }}</izz>
</inertia>
</inertial>
<collision name='collision'>
<geometry>
<cylinder>
<radius>{{ coordinate.radius }}</radius>
<length>{{ 1.7*coordinate.radius }}</length>
</cylinder>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<uri>{{ package_path }}/meshes/pumpkins/pumpkin_01.dae</uri>
<scale>{{ 2*coordinate.radius }} {{ 2*coordinate.radius }} {{ 2*coordinate.radius }}</scale>
</mesh>
</geometry>
<material>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
{% elif coordinate.type == 'pumpkin_02' %}
<model name='{{ coordinate.name }}'>
<pose frame=''>{{ coordinate.x }} {{ coordinate.y }} 0.5 0 -0 0</pose>
<link name='{{ coordinate.name }}_link'>
<inertial>
<mass>{{ coordinate.mass }}</mass>
<inertia>
<ixx>{{ coordinate.inertia.ixx }}</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>{{ coordinate.inertia.iyy }}</iyy>
<iyz>0</iyz>
<izz>{{ coordinate.inertia.izz }}</izz>
</inertia>
</inertial>
<collision name='collision'>
<geometry>
<cylinder>
<radius>{{ coordinate.radius }}</radius>
<length>{{ 1.32*coordinate.radius }}</length>
</cylinder>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<uri>{{ package_path }}/meshes/pumpkins/pumpkin_02.dae</uri>
<scale>{{ 2*coordinate.radius }} {{ 2*coordinate.radius }} {{ 2*coordinate.radius }}</scale>
</mesh>
</geometry>
<material>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
{% elif coordinate.type == 'pumpkin_03' %}
<model name='{{ coordinate.name }}'>
<pose frame=''>{{ coordinate.x }} {{ coordinate.y }} 0.5 0 -0 0</pose>
<link name='{{ coordinate.name }}_link'>
<inertial>
<mass>{{ coordinate.mass }}</mass>
<inertia>
<ixx>{{ coordinate.inertia.ixx }}</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>{{ coordinate.inertia.iyy }}</iyy>
<iyz>0</iyz>
<izz>{{ coordinate.inertia.izz }}</izz>
</inertia>
</inertial>
<collision name='collision'>
<geometry>
<cylinder>
<radius>{{ coordinate.radius }}</radius>
<length>{{ 1.9*coordinate.radius }}</length>
</cylinder>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<uri>{{ package_path }}/meshes/pumpkins/pumpkin_03.dae</uri>
<scale>{{ 2*coordinate.radius }} {{ 2*coordinate.radius }} {{ 2*coordinate.radius }}</scale>
</mesh>
</geometry>
<material>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
{% elif coordinate.type == 'pumpkin_04' %}
<model name='{{ coordinate.name }}'>
<pose frame=''>{{ coordinate.x }} {{ coordinate.y }} 0.5 0 -0 0</pose>
<link name='{{ coordinate.name }}_link'>
<inertial>
<mass>{{ coordinate.mass }}</mass>
<inertia>
<ixx>{{ coordinate.inertia.ixx }}</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>{{ coordinate.inertia.iyy }}</iyy>
<iyz>0</iyz>
<izz>{{ coordinate.inertia.izz }}</izz>
</inertia>
</inertial>
<collision name='collision'>
<geometry>
<cylinder>
<radius>{{ coordinate.radius }}</radius>
<length>{{ 2*coordinate.radius }}</length>
</cylinder>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<uri>{{ package_path }}/meshes/pumpkins/pumpkin_04.dae</uri>
<scale>{{ 2*coordinate.radius }} {{ 2*coordinate.radius }} {{ 2*coordinate.radius }}</scale>
</mesh>
</geometry>
<material>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>