Showing preview only (342K chars total). Download the full file or copy to clipboard to get everything.
Repository: RGring/drl_local_planner_ros_stable_baselines
Branch: master
Commit: 2ce7aa56c878
Files: 137
Total size: 307.1 KB
Directory structure:
gitextract_wiq5yc1c/
├── .gitignore
├── .rosinstall
├── LICENSE
├── README.md
├── docker/
│ └── Dockerfile
├── example_agents/
│ ├── ppo2_1_img_disc_0/
│ │ └── ppo2_1_img_disc_0.pkl
│ ├── ppo2_1_raw_data_cont_0/
│ │ └── ppo2_1_raw_data_cont_0.pkl
│ ├── ppo2_1_raw_data_disc_0/
│ │ └── ppo2_1_raw_data_disc_0.pkl
│ └── ppo2_3_raw_data_disc_0/
│ └── ppo2_3_raw_data_disc_0.pkl
├── flatland_setup/
│ ├── CMakeLists.txt
│ ├── maps/
│ │ ├── complex_map_1/
│ │ │ ├── map.yaml
│ │ │ ├── pedsim_scenario.xml
│ │ │ └── world.yaml
│ │ ├── complex_map_2/
│ │ │ ├── map.yaml
│ │ │ ├── pedsim_scenario.xml
│ │ │ └── world.yaml
│ │ ├── complex_map_3/
│ │ │ ├── map.yaml
│ │ │ ├── pedsim_scenario.xml
│ │ │ └── world.yaml
│ │ ├── corridor_1/
│ │ │ ├── map.yaml
│ │ │ ├── pedsim_scenario.xml
│ │ │ └── world.yaml
│ │ ├── corridor_2/
│ │ │ ├── map.yaml
│ │ │ ├── pedsim_scenario.xml
│ │ │ └── world.yaml
│ │ ├── corridor_3/
│ │ │ ├── map.yaml
│ │ │ ├── pedsim_scenario.xml
│ │ │ └── world.yaml
│ │ ├── map_middle_complexity/
│ │ │ ├── map.yaml
│ │ │ ├── pedsim_scenario.xml
│ │ │ └── world.yaml
│ │ ├── no_map/
│ │ │ └── map.yaml
│ │ ├── open_field_1/
│ │ │ ├── map.yaml
│ │ │ ├── pedsim_scenario.xml
│ │ │ └── world.yaml
│ │ ├── open_field_2/
│ │ │ ├── map.yaml
│ │ │ ├── pedsim_scenario.xml
│ │ │ └── world.yaml
│ │ └── open_field_3/
│ │ ├── agent_scenario.xml
│ │ ├── map.yaml
│ │ ├── pedsim_scenario.xml
│ │ └── world.yaml
│ ├── objects/
│ │ ├── cylinder.model.yaml
│ │ ├── palett.model.yaml
│ │ ├── person_single_circle.model.yaml
│ │ ├── person_two_legged.model.yaml
│ │ └── wagon.model.yaml
│ ├── package.xml
│ └── robot/
│ └── robot1.model.yaml
├── requirements.txt
├── rl_agent/
│ ├── CMakeLists.txt
│ ├── include/
│ │ └── rl_agent/
│ │ ├── laser_scan_merger.h
│ │ └── tf_python.h
│ ├── launch/
│ │ ├── run_1_img_disc.launch
│ │ ├── run_1_raw_cont.launch
│ │ ├── run_1_raw_disc.launch
│ │ ├── run_3_raw_disc.launch
│ │ └── run_ppo_agent.launch
│ ├── package.xml
│ ├── scripts/
│ │ ├── evaluation/
│ │ │ ├── analysis.py
│ │ │ ├── evaluate_agent.py
│ │ │ ├── qualitative_plot.py
│ │ │ └── save_test_episodes.py
│ │ ├── run_scripts/
│ │ │ └── run_ppo.py
│ │ └── train_scripts/
│ │ └── train_ppo.py
│ ├── setup.py
│ └── src/
│ ├── __init__.py
│ ├── laser_scan_merger.cpp
│ ├── rl_agent/
│ │ ├── __init__.py
│ │ ├── common_custom_policies.py
│ │ ├── env_utils/
│ │ │ ├── __init__.py
│ │ │ ├── debug_ros_env.py
│ │ │ ├── reward_container.py
│ │ │ ├── state_collector.py
│ │ │ └── task_generator.py
│ │ ├── env_wrapper/
│ │ │ ├── __init__.py
│ │ │ ├── ros_env.py
│ │ │ ├── ros_env_cont_img.py
│ │ │ ├── ros_env_cont_img_vel.py
│ │ │ ├── ros_env_cont_raw_data.py
│ │ │ ├── ros_env_cont_raw_scan_prep_wp.py
│ │ │ ├── ros_env_disc_img.py
│ │ │ ├── ros_env_disc_img_vel.py
│ │ │ ├── ros_env_disc_raw_data.py
│ │ │ ├── ros_env_disc_raw_scan_prep_wp.py
│ │ │ ├── ros_env_img.py
│ │ │ ├── ros_env_img_vel.py
│ │ │ ├── ros_env_raw_data.py
│ │ │ └── ros_env_raw_scan_prep_wp.py
│ │ └── evaluation/
│ │ ├── Analysis_eval.py
│ │ ├── Evaluation.py
│ │ └── __init__.py
│ └── tf_python.cpp
├── rl_bringup/
│ ├── CMakeLists.txt
│ ├── config/
│ │ ├── costmap_common_params.yaml
│ │ ├── dwa_params.yaml
│ │ ├── global_costmap_params.yaml
│ │ ├── local_costmap_params.yaml
│ │ ├── movebase_params.yaml
│ │ ├── path_config.ini
│ │ ├── path_config_docker.ini
│ │ ├── rl_common.yaml
│ │ ├── rl_params_img.yaml
│ │ └── rl_params_scan.yaml
│ ├── launch/
│ │ ├── rviz.launch
│ │ ├── setup.launch
│ │ └── sub_launch/
│ │ ├── dummy_localization.launch
│ │ ├── flatland.launch
│ │ ├── move_base.launch
│ │ ├── navigation.launch
│ │ ├── pedestrians_only.launch
│ │ ├── pedsim_visualizer.launch
│ │ └── simulation.launch
│ ├── package.xml
│ ├── rviz/
│ │ └── robot_navigation.rviz
│ └── src/
│ └── toggle_setup_init.cpp
├── rl_local_planner/
│ ├── CMakeLists.txt
│ ├── blp_plugin.xml
│ ├── include/
│ │ └── rl_local_planner/
│ │ ├── image_generator.h
│ │ ├── rl_local_planner.h
│ │ └── wp_generator.h
│ ├── package.xml
│ └── src/
│ ├── image_generator.cpp
│ ├── rl_local_planner.cpp
│ └── wp_generator.cpp
├── rl_msgs/
│ ├── CMakeLists.txt
│ ├── msg/
│ │ └── Waypoint.msg
│ ├── package.xml
│ └── srv/
│ ├── MergeScans.srv
│ ├── SetPath.srv
│ └── StateImageGenerationSrv.srv
└── start_scripts/
├── entrypoint_ppo2.sh
├── ppo2_training.sh
├── ros.sh
├── start_roscore.sh
└── training_params/
├── ppo2_params.csv
└── training_maps.csv
================================================
FILE CONTENTS
================================================
================================================
FILE: .gitignore
================================================
*.png
*__pycache__*
*.idea*
*.pyc
*screenlog*
================================================
FILE: .rosinstall
================================================
- git:
local-name: drl_local_planner_forks/flatland
uri: https://github.com/RGring/flatland.git
version: pedsim_integration
- git:
local-name: drl_local_planner_forks/pedsim
uri: https://github.com/RGring/pedsim_ros.git
version: rl_features
- git:
local-name: drl_local_planner_forks/navigation
uri: https://github.com/RGring/navigation.git
version: flatland_integration
- git:
local-name: drl_local_planner_forks/stable-baselines
uri: https://github.com/RGring/stable-baselines.git
version: added_1d_conv_layer
================================================
FILE: LICENSE
================================================
BSD 3-Clause License
Copyright (c) 2019, RGring
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 the copyright holder 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.
================================================
FILE: README.md
================================================
# What is this repository for?
* Setup to train a local planner with reinforcement learning approaches from [stable baselines](https://github.com/hill-a/stable-baselines) integrated ROS
* Training in a simulator fusion of [Flatland](https://github.com/avidbots/flatland) and [pedsim_ros](https://github.com/srl-freiburg/pedsim_ros)
* local planner has been trained on static and dynamic obstacles: [video](https://www.youtube.com/watch?v=nHvpO0hVnAg)
* Link to [IROS Paper](http://ras.papercept.net/images/temp/IROS/files/0122.pdf)
* Link to [Master Thesis](https://tams.informatik.uni-hamburg.de/publications/2019/MSc_Ronja_Gueldenring.pdf) for more in depth information.
# Installation (Else: Docker below)
1. Standart ROS setup (Code has been tested with ROS-kinetic on Ubuntu 16.04)
2. Install additional packages
```
apt-get update && apt-get install -y \
libqt4-dev \
libopencv-dev \
liblua5.2-dev \
virtualenv \
screen \
python3-dev \
ros-kinetic-tf2-geometry-msgs \
ros-kinetic-navigation \
ros-kinetic-rviz
```
3. Setup repository:
* Clone this repository in your src-folder of your catkin workspace
```
cd <path_to_catkin_ws>/src/drl_local_planner_ros_stable_baselines
cp .rosinstall ../
cd ..
rosws update
cd <path_to_catkin_ws>
catkin_make -DCMAKE_BUILD_TYPE=Release
```
(please install missing packages)
4. Setup virtual environment to be able to use python3 with ros (consider also requirements.txt)
```
virtualenv <path_to_venv>/venv_p3 --python=python3
source <path_to_venv>/venv_p3/bin/activate
<path_to_venv>/venv_p3/bin/pip install \
pyyaml \
rospkg \
catkin_pkg \
exception \
numpy \
tensorflow=="1.13.1" \
gym \
pyquaternion \
mpi4py \
matplotlib
cd <path_to_catkin_ws>/src/drl_local_planner_forks/stable_baselines/
<path_to_venv>/venv_p3/bin/pip install -e path_to_catkin_ws>/src/drl_local_planner_forks/stable-baselines/
```
5. Set system-relevant variables
* Modify all relevant pathes rl_bringup/config/path_config.ini
# Example usage
1. Train agent
* Open first terminal (roscore):
```
roscore
```
* Open second terminal (simulationI:
```
roslaunch rl_bringup setup.launch ns:="sim1" rl_params:="rl_params_scan"
```
* Open third terminal (DRL-agent):
```
source <path_to_venv>/bin/activate
python rl_agent/scripts/train_scripts/train_ppo.py
```
* Open fourth terminal (Visualization):
```
roslaunch rl_bringup rviz.launch ns:="sim1"
```
2. Execute self-trained ppo-agent
* Copy your trained agent in your "path_to_models"
* Open first terminal:
```
roscore
```
* Open second terminal:
```
roslaunch rl_bringup setup.launch ns:="sim1" rl_params:="rl_params_scan"
```
* Open third terminal:
```
source <path_to_venv>/venv_p3/bin/activate
roslaunch rl_agent run_ppo_agent.launch mode:="train"
```
* Open fourth terminal:
```
roslaunch rl_bringup rviz.launch ns:="sim1"
```
* Set 2D Navigation Goal in rviz
# Run pretrained Agents
Note: To be able to load the pretrained agents, you need to install numpy version 1.17.0.
```
<path_to_venv>/venv_p3/bin/pip install numpy==1.17
```
### Run agent trained on raw data, discrete action space, stack size 1
1. Copy the example_agents in your "path_to_models"
2. Open first terminal:
```
roscore
```
3. Open second terminal for visualization:
```
roslaunch rl_bringup rviz.launch ns:="sim1"
```
4. Open third terminal:
```
roslaunch rl_bringup setup.launch ns:="sim1" rl_params:="rl_params_scan"
```
5. Open fourth terminal:
```
source <path_to_venv>/venv_p3/bin/activate
roslaunch rl_agent run_1_raw_disc.launch mode:="train"
```
### Run agent trained on raw data, discrete action space, stack size 3
1. Step 1 - 4 are the same like in the first example
2. Open fourth terminal:
```
source <path_to_venv>/venv_p3/bin/activate
roslaunch rl_agent run_3_raw_disc.launch mode:="train"
```
### Run agent trained on raw data, continuous action space, stack size 1
1. Step 1 - 4 are the same like in the first example
2. Open fourth terminal:
```
source <path_to_venv>/venv_p3/bin/activate
roslaunch rl_agent run_1_raw_cont.launch mode:="train"
```
### Run agent trained on image data, discrete action space, stack size 1
1. Step 1 - 3 are the same like in the first example
4. Open third terminal:
```
roslaunch rl_bringup setup.launch ns:="sim1" rl_params:="rl_params_img"
```
5. Open fourth terminal:
```
source <path_to_venv>/venv_p3/bin/activate
roslaunch rl_agent run_1_img_disc.launch mode:="train"
```
# Training in Docker
I set up a docker image, that allows you to train a DRL-agent in parallel simulation environments. Furthermore, it simplifies the deployment on a server. Using docker you don't need to follow the steps in the Installation section.
0. Build the Docker image (This will unfortunately take about 15 minutes):
```
cd drl_local_planner_ros_stable_baselines/docker
```
```
docker build -t ros-drl_local_planner .
```
## Training from scratch
1. In start_scripts/training_params/ppo2_params, define the agents training parameters.
| Parameter | Desctiption |
|-------------------------|--------------|
| agent_name | Number of timestamps the agent will be trained. |
| total_timesteps | Number of timestamps the agent will be trained. |
| policy | see [PPO2 Doc](https://stable-baselines.readthedocs.io/en/master/modules/ppo2.html) |
| gamma | see [PPO2 Doc](https://stable-baselines.readthedocs.io/en/master/modules/ppo2.html) |
| n_steps | see [PPO2 Doc](https://stable-baselines.readthedocs.io/en/master/modules/ppo2.html) |
| ent_coef | see [PPO2 Doc](https://stable-baselines.readthedocs.io/en/master/modules/ppo2.html) |
| learning_rate | see [PPO2 Doc](https://stable-baselines.readthedocs.io/en/master/modules/ppo2.html) |
| vf_coef | see [PPO2 Doc](https://stable-baselines.readthedocs.io/en/master/modules/ppo2.html) |
| max_grad_norm | see [PPO2 Doc](https://stable-baselines.readthedocs.io/en/master/modules/ppo2.html) |
| lam | see [PPO2 Doc](https://stable-baselines.readthedocs.io/en/master/modules/ppo2.html) |
| nminibatches | see [PPO2 Doc](https://stable-baselines.readthedocs.io/en/master/modules/ppo2.html) |
| noptepochs | see [PPO2 Doc](https://stable-baselines.readthedocs.io/en/master/modules/ppo2.html) |
| cliprange | see [PPO2 Doc](https://stable-baselines.readthedocs.io/en/master/modules/ppo2.html) |
| robot_radius | The radius if the robot footprint |
| rew_func | The reward functions that should be used. They can be found and defined in rl_agent/src/rl_agent/env_utils/reward_container.py. |
| num_stacks | State representation includes the current observation and (num_stacks - 1) previous observation. |
| stack_offset | The number of timestamps between each stacked observation. |
| disc_action_space | 0, if continuous action space. 1, if discrete action space. |
| normalize | 0, if input should not be normalized. 1, if input should be normalized. |
| stage | stage number of your training. It is supposed to be 0, if you train for the first time. If it is > 0, it loads the agent of the "pretrained_model_path" and continues training. |
| pretrained_model_name | If stage > 0 this agent will be loaded and training can be continued. |
| task_mode | - "ped" for training on pedestrians only; "static" for training on static objects only; "ped_static" for training on both, static |
2. There are some predefined agents. As example I will use the ppo2_1_raw_data_disc_0 in the training session.
```
docker run --rm -d \
-v <folder_to_save_data>:/data \
-v drl_local_planner_ros_stable_baselines/start_scripts/training_params:/usr/catkin_ws/src/drl_local_planner_ros_stable_baselines/start_scripts/training_params \
-e AGENT_NAME=ppo2_1_raw_data_disc_0 \
-e NUM_SIM_ENVS=4 \
ros-drl_local_planner
```
3. If you want to display the training in Rviz, run the docker container in the hosts network. In order to use rviz, the relevant packages need to be compiled on your machine.
```
docker run --rm -d \
-v <folder_to_save_data>:/data \
-v drl_local_planner_ros_stable_baselines/start_scripts/training_params:/usr/catkin_ws/src/drl_local_planner_ros_stable_baselines/start_scripts/training_params \
-e AGENT_NAME=ppo2_1_raw_data_disc_0 \
-e NUM_SIM_ENVS=4 \
--net=host \
ros-drl_local_planner
```
Now you can display the different simulation environments:
* Simulation 1:
```
roslaunch rl_bringup rviz.launch ns:="sim1"
```
* Simulation 2:
```
roslaunch rl_bringup rviz.launch ns:="sim2"
```
* etc. ...
## Train with pre-trained agents
### Run agent trained on raw data, discrete action space, stack size 1
```
docker run --rm -d \
-v drl_local_planner_ros_stable_baselines/example_agents:/data/agents \
-v drl_local_planner_ros_stable_baselines/start_scripts/training_params:/usr/catkin_ws/src/drl_local_planner_ros_stable_baselines/start_scripts/training_params \
-e AGENT_NAME=ppo2_1_raw_data_disc_0_pretrained \
-e NUM_SIM_ENVS=4 \
--net=host \
ros-drl_local_planner
```
### Run agent trained on image data, discrete action space, stack size 1
```
docker run --rm -d \
-v drl_local_planner_ros_stable_baselines/example_agents:/data/agents \
-v drl_local_planner_ros_stable_baselines/start_scripts/training_params:/usr/catkin_ws/src/drl_local_planner_ros_stable_baselines/start_scripts/training_params \
-e AGENT_NAME=ppo2_1_img_disc_1_pretrained \
-e NUM_SIM_ENVS=4 \
--net=host \
ros-drl_local_planner
```
================================================
FILE: docker/Dockerfile
================================================
FROM ros:kinetic-robot-xenial
RUN apt-get update && apt-get install -y \
libqt4-dev \
libopencv-dev \
liblua5.2-dev \
virtualenv \
screen \
python3-dev \
git \
ros-kinetic-tf2-geometry-msgs \
ros-kinetic-navigation \
ros-kinetic-rviz
# Building catkin_ws
RUN mkdir -p /usr/catkin_ws/src
WORKDIR /usr/catkin_ws/src
RUN git clone https://github.com/RGring/drl_local_planner_ros_stable_baselines
RUN cp drl_local_planner_ros_stable_baselines/.rosinstall .
RUN rosws update
RUN /bin/bash -c '. /opt/ros/kinetic/setup.bash; cd /usr/catkin_ws; catkin_make'
#Creating virtualenv for python3
RUN virtualenv /venv_p3 --python=python3
RUN /venv_p3/bin/pip install \
pyyaml \
rospkg \
catkin_pkg \
exception \
numpy \
tensorflow=="1.13.1" \
gym \
pyquaternion \
mpi4py \
matplotlib
RUN /venv_p3/bin/pip install -e /usr/catkin_ws/src/drl_local_planner_forks/stable-baselines
RUN mv /usr/catkin_ws/src/drl_local_planner_ros_stable_baselines/rl_bringup/config/path_config_docker.ini \
/usr/catkin_ws/src/drl_local_planner_ros_stable_baselines/rl_bringup/config/path_config.ini
WORKDIR /usr/catkin_ws/src/drl_local_planner_ros_stable_baselines/start_scripts/
ENTRYPOINT ./entrypoint_ppo2.sh "$AGENT_NAME" "$NUM_SIM_ENVS"
================================================
FILE: flatland_setup/CMakeLists.txt
================================================
cmake_minimum_required(VERSION 2.8.3)
project(flatland_setup)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# std_msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES flatland_setup
# CATKIN_DEPENDS roscpp rospy std_msgs
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/flatland_setup.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/flatland_setup_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_flatland_setup.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
================================================
FILE: flatland_setup/maps/complex_map_1/map.yaml
================================================
image: complex_map_1.png
resolution: 0.05
# origin: [-1.1, -9.05, 0.0]
# origin: [-0.5, -1.45, 0.0]
origin: [0.0, 0.0, 0.0]
occupied_thresh: 0.65
free_thresh: 0.196
negate: 0
#mode: scale
================================================
FILE: flatland_setup/maps/complex_map_1/pedsim_scenario.xml
================================================
<?xml version="1.0" encoding="UTF-8"?>
<scenario>
<waypoint id="robot_goal" x="22" y="27" r="2"/> -->
<waypoint id="robot_start" x="4" y="4" r="2"/>
<!--Robot Agents-->
<agent x="8" y="8" n="1" dx="0" dy="0" type="2">
<addwaypoint id="robot_start"/>
<!-- <addwaypoint id="robot_goal"/> -->
</agent>
</scenario>
================================================
FILE: flatland_setup/maps/complex_map_1/world.yaml
================================================
properties:
# optional, defaults to 10, number of velocity iterations for the Box2D
# physics solver
velocity_iterations: 10
# optional, defaults to 10, number of position iterations for the Box2D
# physics solver
position_iterations: 10
layers: # Support for arbitrary number of layers
# - name: "robot" # So that sensors can trigger on just the robot
- name: "static"
map: "map.yaml" # leading / denotes absolute file path, otherwise relative
color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary
- name: "ped"
# map: "../no_map/map.yaml" # leading / denotes absolute file path, otherwise relative
color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary
# - name: "2d" # layer 0 named "2d"
# map: "map.yaml" # leading / denotes absolute file path, otherwise relative
# color: [0, 1, 0, 1] # List of floats [r,g,b,a] to color debug boundary
# - name: "3d"
# map: "map.yaml" # leading / denotes absolute file path, otherwise relative
# color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary
================================================
FILE: flatland_setup/maps/complex_map_2/map.yaml
================================================
image: complex_map_2.png
resolution: 0.05
# origin: [-1.1, -9.05, 0.0]
# origin: [-0.5, -1.45, 0.0]
origin: [0.0, 0.0, 0.0]
occupied_thresh: 0.65
free_thresh: 0.196
negate: 0
#mode: scale
================================================
FILE: flatland_setup/maps/complex_map_2/pedsim_scenario.xml
================================================
<?xml version="1.0" encoding="UTF-8"?>
<scenario>
<waypoint id="robot_goal" x="22" y="27" r="2"/> -->
<waypoint id="robot_start" x="4" y="4" r="2"/>
<!--Robot Agents-->
<agent x="8" y="8" n="1" dx="0" dy="0" type="2">
<addwaypoint id="robot_start"/>
<!-- <addwaypoint id="robot_goal"/> -->
</agent>
</scenario>
================================================
FILE: flatland_setup/maps/complex_map_2/world.yaml
================================================
properties:
# optional, defaults to 10, number of velocity iterations for the Box2D
# physics solver
velocity_iterations: 10
# optional, defaults to 10, number of position iterations for the Box2D
# physics solver
position_iterations: 10
layers: # Support for arbitrary number of layers
# - name: "robot" # So that sensors can trigger on just the robot
- name: "static"
map: "map.yaml" # leading / denotes absolute file path, otherwise relative
color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary
- name: "ped"
# map: "../no_map/map.yaml" # leading / denotes absolute file path, otherwise relative
color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary
# - name: "2d" # layer 0 named "2d"
# map: "map.yaml" # leading / denotes absolute file path, otherwise relative
# color: [0, 1, 0, 1] # List of floats [r,g,b,a] to color debug boundary
# - name: "3d"
# map: "map.yaml" # leading / denotes absolute file path, otherwise relative
# color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary
================================================
FILE: flatland_setup/maps/complex_map_3/map.yaml
================================================
image: complex_map_3.png
resolution: 0.05
# origin: [-1.1, -9.05, 0.0]
# origin: [-0.5, -1.45, 0.0]
origin: [0.0, 0.0, 0.0]
occupied_thresh: 0.65
free_thresh: 0.196
negate: 0
#mode: scale
================================================
FILE: flatland_setup/maps/complex_map_3/pedsim_scenario.xml
================================================
<?xml version="1.0" encoding="UTF-8"?>
<scenario>
<waypoint id="robot_goal" x="22" y="27" r="2"/> -->
<waypoint id="robot_start" x="4" y="4" r="2"/>
<!--Robot Agents-->
<agent x="8" y="8" n="1" dx="0" dy="0" type="2">
<addwaypoint id="robot_start"/>
<!-- <addwaypoint id="robot_goal"/> -->
</agent>
</scenario>
================================================
FILE: flatland_setup/maps/complex_map_3/world.yaml
================================================
properties:
# optional, defaults to 10, number of velocity iterations for the Box2D
# physics solver
velocity_iterations: 10
# optional, defaults to 10, number of position iterations for the Box2D
# physics solver
position_iterations: 10
layers: # Support for arbitrary number of layers
# - name: "robot" # So that sensors can trigger on just the robot
- name: "static"
map: "map.yaml" # leading / denotes absolute file path, otherwise relative
color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary
- name: "ped"
# map: "../no_map/map.yaml" # leading / denotes absolute file path, otherwise relative
color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary
# - name: "2d" # layer 0 named "2d"
# map: "map.yaml" # leading / denotes absolute file path, otherwise relative
# color: [0, 1, 0, 1] # List of floats [r,g,b,a] to color debug boundary
# - name: "3d"
# map: "map.yaml" # leading / denotes absolute file path, otherwise relative
# color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary
================================================
FILE: flatland_setup/maps/corridor_1/map.yaml
================================================
image: corridor_1.png
resolution: 0.05
# origin: [-1.1, -9.05, 0.0]
# origin: [-0.5, -1.45, 0.0]
origin: [0.0, 0.0, 0.0]
occupied_thresh: 0.65
free_thresh: 0.196
negate: 0
#mode: scale
================================================
FILE: flatland_setup/maps/corridor_1/pedsim_scenario.xml
================================================
<?xml version="1.0" encoding="UTF-8"?>
<scenario>
<waypoint id="robot_goal" x="22" y="27" r="2"/> -->
<waypoint id="robot_start" x="4" y="4" r="2"/>
<!--Robot Agents-->
<agent x="8" y="8" n="1" dx="0" dy="0" type="2">
<addwaypoint id="robot_start"/>
<!-- <addwaypoint id="robot_goal"/> -->
</agent>
</scenario>
================================================
FILE: flatland_setup/maps/corridor_1/world.yaml
================================================
properties:
# optional, defaults to 10, number of velocity iterations for the Box2D
# physics solver
velocity_iterations: 10
# optional, defaults to 10, number of position iterations for the Box2D
# physics solver
position_iterations: 10
layers: # Support for arbitrary number of layers
# - name: "robot" # So that sensors can trigger on just the robot
- name: "static"
map: "map.yaml" # leading / denotes absolute file path, otherwise relative
color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary
- name: "ped"
# map: "../no_map/map.yaml" # leading / denotes absolute file path, otherwise relative
color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary
# - name: "2d" # layer 0 named "2d"
# map: "map.yaml" # leading / denotes absolute file path, otherwise relative
# color: [0, 1, 0, 1] # List of floats [r,g,b,a] to color debug boundary
# - name: "3d"
# map: "map.yaml" # leading / denotes absolute file path, otherwise relative
# color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary
================================================
FILE: flatland_setup/maps/corridor_2/map.yaml
================================================
image: corridor_2.png
resolution: 0.05
# origin: [-1.1, -9.05, 0.0]
# origin: [-0.5, -1.45, 0.0]
origin: [0.0, 0.0, 0.0]
occupied_thresh: 0.65
free_thresh: 0.196
negate: 0
#mode: scale
================================================
FILE: flatland_setup/maps/corridor_2/pedsim_scenario.xml
================================================
<?xml version="1.0" encoding="UTF-8"?>
<scenario>
<waypoint id="robot_goal" x="22" y="27" r="2"/> -->
<waypoint id="robot_start" x="4" y="4" r="2"/>
<!--Robot Agents-->
<agent x="8" y="8" n="1" dx="0" dy="0" type="2">
<addwaypoint id="robot_start"/>
<!-- <addwaypoint id="robot_goal"/> -->
</agent>
</scenario>
================================================
FILE: flatland_setup/maps/corridor_2/world.yaml
================================================
properties:
# optional, defaults to 10, number of velocity iterations for the Box2D
# physics solver
velocity_iterations: 10
# optional, defaults to 10, number of position iterations for the Box2D
# physics solver
position_iterations: 10
layers: # Support for arbitrary number of layers
# - name: "robot" # So that sensors can trigger on just the robot
- name: "static"
map: "map.yaml" # leading / denotes absolute file path, otherwise relative
color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary
- name: "ped"
# map: "../no_map/map.yaml" # leading / denotes absolute file path, otherwise relative
color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary
# - name: "2d" # layer 0 named "2d"
# map: "map.yaml" # leading / denotes absolute file path, otherwise relative
# color: [0, 1, 0, 1] # List of floats [r,g,b,a] to color debug boundary
# - name: "3d"
# map: "map.yaml" # leading / denotes absolute file path, otherwise relative
# color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary
================================================
FILE: flatland_setup/maps/corridor_3/map.yaml
================================================
image: corridor_3.png
resolution: 0.05
# origin: [-1.1, -9.05, 0.0]
# origin: [-0.5, -1.45, 0.0]
origin: [0.0, 0.0, 0.0]
occupied_thresh: 0.65
free_thresh: 0.196
negate: 0
#mode: scale
================================================
FILE: flatland_setup/maps/corridor_3/pedsim_scenario.xml
================================================
<?xml version="1.0" encoding="UTF-8"?>
<scenario>
<waypoint id="robot_goal" x="22" y="27" r="2"/> -->
<waypoint id="robot_start" x="4" y="4" r="2"/>
<!--Robot Agents-->
<agent x="8" y="8" n="1" dx="0" dy="0" type="2">
<addwaypoint id="robot_start"/>
<!-- <addwaypoint id="robot_goal"/> -->
</agent>
</scenario>
================================================
FILE: flatland_setup/maps/corridor_3/world.yaml
================================================
properties:
# optional, defaults to 10, number of velocity iterations for the Box2D
# physics solver
velocity_iterations: 10
# optional, defaults to 10, number of position iterations for the Box2D
# physics solver
position_iterations: 10
layers: # Support for arbitrary number of layers
# - name: "robot" # So that sensors can trigger on just the robot
- name: "static"
map: "map.yaml" # leading / denotes absolute file path, otherwise relative
color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary
- name: "ped"
# map: "../no_map/map.yaml" # leading / denotes absolute file path, otherwise relative
color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary
# - name: "2d" # layer 0 named "2d"
# map: "map.yaml" # leading / denotes absolute file path, otherwise relative
# color: [0, 1, 0, 1] # List of floats [r,g,b,a] to color debug boundary
# - name: "3d"
# map: "map.yaml" # leading / denotes absolute file path, otherwise relative
# color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary
================================================
FILE: flatland_setup/maps/map_middle_complexity/map.yaml
================================================
image: map_middle_complexity.png
resolution: 0.05
origin: [0.0, 0.0, 0.0]
occupied_thresh: 0.65
free_thresh: 0.196
negate: 0
================================================
FILE: flatland_setup/maps/map_middle_complexity/pedsim_scenario.xml
================================================
<?xml version="1.0" encoding="UTF-8"?>
<scenario>
<obstacle x1="1.4" y1="24.6" x2="16.9" y2="24.6"/>
<obstacle x1="16.9" y1="24.6" x2="18.2" y2="22.6"/>
<obstacle x1="18.2" y1="22.6" x2="24" y2="24.6"/>
<obstacle x1="24" y1="24.6" x2="29" y2="24.6"/>
<obstacle x1="29" y1="24.6" x2="29" y2="20.2"/>
<obstacle x1="29" y1="20.2" x2="31.8" y2="20.2"/>
<obstacle x1="31.8" y1="20.2" x2="31.8" y2="1.4"/>
<obstacle x1="31.8" y1="1.4" x2="1.4" y2="1.4"/>
<obstacle x1="1.4" y1="1.4" x2="1.4" y2="8.1"/>
<obstacle x1="1.4" y1="8.1" x2="3.0" y2="10.9"/>
<obstacle x1="3.0" y1="10.9" x2="3.15" y2="15.4"/>
<obstacle x1="3.15" y1="15.4" x2="1.4" y2="16.87"/>
<obstacle x1="1.4" y1="16.87" x2="1.4" y2="24.6"/>
<obstacle x1="8.25" y1="8.2" x2="8.25" y2="13.9"/>
<obstacle x1="8.25" y1="13.9" x2="11" y2="13.9"/>
<obstacle x1="11.0" y1="13.9" x2="11" y2="8.2"/>
<obstacle x1="11.0" y1="8.2" x2="8.25" y2="8.2"/>
<obstacle x1="12.1" y1="19.0" x2="5.25" y2="19.0"/>
<obstacle x1="5.25" y1="19.0" x2="5.25" y2="21.5"/>
<obstacle x1="5.25" y1="21.5" x2="12.0" y2="21.5"/>
<obstacle x1="5.25" y1="21.5" x2="12.0" y2="21.5"/>
<obstacle x1="12.0" y1="21.5" x2="12.0" y2="19.0"/>
<obstacle x1="26.5" y1="17.0" x2="20.2" y2="17.0"/>
<obstacle x1="20.2" y1="17.0" x2="20.2" y2="18.0"/>
<obstacle x1="20.2" y1="18.0" x2="26.5" y2="18.0"/>
<obstacle x1="26.5" y1="18.0" x2="26.5" y2="17.0"/>
<obstacle x1="5.6" y1="4.5" x2="6.4" y2="4.5"/>
<obstacle x1="6.4" y1="4.5" x2="6.4" y2="2.1"/>
<obstacle x1="6.4" y1="2.1" x2="5.6" y2="2.1"/>
<obstacle x1="5.6" y1="2.1" x2="5.6" y2="4.5"/>
<obstacle x1="13.6" y1="6.0" x2="19.6" y2="6.0"/>
<obstacle x1="19.6" y1="6.0" x2="19.6" y2="3.4"/>
<obstacle x1="19.6" y1="3.4" x2="13.6" y2="3.4"/>
<obstacle x1="13.6" y1="3.4" x2="13.6" y2="6.0"/>
<obstacle x1="18.0" y1="13.6" x2="21.8" y2="13.6"/>
<obstacle x1="21.8" y1="13.6" x2="21.8" y2="10.7"/>
<obstacle x1="21.8" y1="10.7" x2="18.0" y2="10.7"/>
<obstacle x1="18.0" y1="10.7" x2="18.0" y2="13.6"/>
<obstacle x1="23.3" y1="7.4" x2="25.9" y2="10.0"/>
<obstacle x1="25.9" y1="10.0" x2="27.3" y2="8.8"/>
<obstacle x1="27.3" y1="8.8" x2="27.1" y2="7.24"/>
<obstacle x1="27.1" y1="7.24" x2="25.35" y2="7.0"/>
<obstacle x1="25.35" y1="7.0" x2="25.75" y2="6.0"/>
<obstacle x1="25.75" y1="6.0" x2="24.9" y2="4.9"/>
<obstacle x1="24.9" y1="4.9" x2="23.3" y2="7.4"/>
<waypoint id="robot_goal" x="22" y="27" r="2"/> -->
<waypoint id="robot_start" x="4" y="4" r="2"/>
<!--Robot Agents-->
<agent x="8" y="8" n="1" dx="0" dy="0" type="2">
<addwaypoint id="robot_start"/>
<!-- <addwaypoint id="robot_goal"/> -->
</agent>
</scenario>
================================================
FILE: flatland_setup/maps/map_middle_complexity/world.yaml
================================================
properties:
# optional, defaults to 10, number of velocity iterations for the Box2D
# physics solver
velocity_iterations: 10
# optional, defaults to 10, number of position iterations for the Box2D
# physics solver
position_iterations: 10
layers: # Support for arbitrary number of layers
- name: "ped"
color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary
- name: "static"
map: "map.yaml" # leading / denotes absolute file path, otherwise relative
color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary
================================================
FILE: flatland_setup/maps/no_map/map.yaml
================================================
# image: no_map.png
image: no_map.png
resolution: 0.05
origin: [-0.5, -1.45, 0.0]
occupied_thresh: 0.65
free_thresh: 0.196
negate: 0
#mode: scale
================================================
FILE: flatland_setup/maps/open_field_1/map.yaml
================================================
image: open_field_1.png
resolution: 0.05
# origin: [-1.1, -9.05, 0.0]
# origin: [-0.5, -1.45, 0.0]
origin: [0.0, 0.0, 0.0]
occupied_thresh: 0.65
free_thresh: 0.196
negate: 0
#mode: scale
================================================
FILE: flatland_setup/maps/open_field_1/pedsim_scenario.xml
================================================
<?xml version="1.0" encoding="UTF-8"?>
<scenario>
<waypoint id="robot_goal" x="22" y="27" r="2"/> -->
<waypoint id="robot_start" x="4" y="4" r="2"/>
<!--Robot Agents-->
<agent x="8" y="8" n="1" dx="0" dy="0" type="2">
<addwaypoint id="robot_start"/>
<!-- <addwaypoint id="robot_goal"/> -->
</agent>
</scenario>
================================================
FILE: flatland_setup/maps/open_field_1/world.yaml
================================================
properties:
# optional, defaults to 10, number of velocity iterations for the Box2D
# physics solver
velocity_iterations: 10
# optional, defaults to 10, number of position iterations for the Box2D
# physics solver
position_iterations: 10
layers: # Support for arbitrary number of layers
# - name: "robot" # So that sensors can trigger on just the robot
- name: "static"
map: "map.yaml" # leading / denotes absolute file path, otherwise relative
color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary
- name: "ped"
# map: "../no_map/map.yaml" # leading / denotes absolute file path, otherwise relative
color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary
# - name: "2d" # layer 0 named "2d"
# map: "map.yaml" # leading / denotes absolute file path, otherwise relative
# color: [0, 1, 0, 1] # List of floats [r,g,b,a] to color debug boundary
# - name: "3d"
# map: "map.yaml" # leading / denotes absolute file path, otherwise relative
# color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary
================================================
FILE: flatland_setup/maps/open_field_2/map.yaml
================================================
image: open_field_2.png
resolution: 0.05
# origin: [-1.1, -9.05, 0.0]
# origin: [-0.5, -1.45, 0.0]
origin: [0.0, 0.0, 0.0]
occupied_thresh: 0.65
free_thresh: 0.196
negate: 0
#mode: scale
================================================
FILE: flatland_setup/maps/open_field_2/pedsim_scenario.xml
================================================
<?xml version="1.0" encoding="UTF-8"?>
<scenario>
<waypoint id="robot_goal" x="22" y="27" r="2"/> -->
<waypoint id="robot_start" x="4" y="4" r="2"/>
<!--Robot Agents-->
<agent x="8" y="8" n="1" dx="0" dy="0" type="2">
<addwaypoint id="robot_start"/>
<!-- <addwaypoint id="robot_goal"/> -->
</agent>
</scenario>
================================================
FILE: flatland_setup/maps/open_field_2/world.yaml
================================================
properties:
# optional, defaults to 10, number of velocity iterations for the Box2D
# physics solver
velocity_iterations: 10
# optional, defaults to 10, number of position iterations for the Box2D
# physics solver
position_iterations: 10
layers: # Support for arbitrary number of layers
# - name: "robot" # So that sensors can trigger on just the robot
- name: "static"
map: "map.yaml" # leading / denotes absolute file path, otherwise relative
color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary
- name: "ped"
# map: "../no_map/map.yaml" # leading / denotes absolute file path, otherwise relative
color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary
# - name: "2d" # layer 0 named "2d"
# map: "map.yaml" # leading / denotes absolute file path, otherwise relative
# color: [0, 1, 0, 1] # List of floats [r,g,b,a] to color debug boundary
# - name: "3d"
# map: "map.yaml" # leading / denotes absolute file path, otherwise relative
# color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary
================================================
FILE: flatland_setup/maps/open_field_3/agent_scenario.xml
================================================
<waypoint id="Entry1" x="6" y="26" r="2"/>
<waypoint id="Entry2" x="2" y="5" r="2"/>
<waypoint id="Entry3" x="2" y="15" r="2"/>
<waypoint id="Coffee" x="17" y="18" r="2"/>
<waypoint id="WorkStation1" x="5.5" y="3" r="2"/>
<waypoint id="KloenSchnack" x="3" y="23.5" r="2"/>
<waypoint id="WorkStation2" x="12" y="8" r="2"/>
<waypoint id="WorkStation3" x="10.5" y="17.5" r="2"/>
<waypoint id="TempWP1" x="8" y="14.5" r="2"/>
<waypoint id="TempWP2" x="14.5" y="3" r="2"/>
<waypoint id="TempWP3" x="15" y="23" r="2"/>
<agent x="6" y="26" n="1" dx="0" dy="0" type="0">
<addwaypoint id="Entry1"/>
<addwaypoint id="TempWP1"/>
<addwaypoint id="Entry2"/>
<addwaypoint id="TempWP1"/>
</agent>
<agent x="2" y="30" n="15" dx="0" dy="0" type="0">
<addwaypoint id="Entry3"/>
<addwaypoint id="WorkStation3"/>
</agent>
<agent x="3" y="23.5" n="15" dx="0" dy="0" type="0">
</agent>
<agent x="2" y="30" n="15" dx="0" dy="0" type="0">
<addwaypoint id="Coffee"/>
<addwaypoint id="TempWP2"/>
<addwaypoint id="WorkStation1"/>
<addwaypoint id="TempWP2"/>
</agent>
<agent x="2" y="30" n="15" dx="0" dy="0" type="0">
<addwaypoint id="Coffee"/>
<addwaypoint id="TempWP3"/>
<addwaypoint id="KloenSchnack"/>
<addwaypoint id="TempWP3"/>
</agent>
================================================
FILE: flatland_setup/maps/open_field_3/map.yaml
================================================
image: open_field_3.png
resolution: 0.05
# origin: [-1.1, -9.05, 0.0]
# origin: [-0.5, -1.45, 0.0]
origin: [0.0, 0.0, 0.0]
occupied_thresh: 0.65
free_thresh: 0.196
negate: 0
#mode: scale
================================================
FILE: flatland_setup/maps/open_field_3/pedsim_scenario.xml
================================================
<?xml version="1.0" encoding="UTF-8"?>
<scenario>
<waypoint id="robot_goal" x="22" y="27" r="2"/> -->
<waypoint id="robot_start" x="4" y="4" r="2"/>
<!--Robot Agents-->
<agent x="8" y="8" n="1" dx="0" dy="0" type="2">
<addwaypoint id="robot_start"/>
<!-- <addwaypoint id="robot_goal"/> -->
</agent>
</scenario>
================================================
FILE: flatland_setup/maps/open_field_3/world.yaml
================================================
properties:
# optional, defaults to 10, number of velocity iterations for the Box2D
# physics solver
velocity_iterations: 10
# optional, defaults to 10, number of position iterations for the Box2D
# physics solver
position_iterations: 10
layers: # Support for arbitrary number of layers
# - name: "robot" # So that sensors can trigger on just the robot
- name: "static"
map: "map.yaml" # leading / denotes absolute file path, otherwise relative
color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary
- name: "ped"
# map: "../no_map/map.yaml" # leading / denotes absolute file path, otherwise relative
color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary
# - name: "2d" # layer 0 named "2d"
# map: "map.yaml" # leading / denotes absolute file path, otherwise relative
# color: [0, 1, 0, 1] # List of floats [r,g,b,a] to color debug boundary
# - name: "3d"
# map: "map.yaml" # leading / denotes absolute file path, otherwise relative
# color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary
================================================
FILE: flatland_setup/objects/cylinder.model.yaml
================================================
# Round obstacle
bodies: # List of named bodies
- name: cylinder
pose: [0, 0, 0]
type: dynamic
color: [0.2, 0.8, 0.2, 0.75]
footprints:
- type: circle
radius: 0.5
center: [0, 0]
density: 0
layers: ["static"]
collision: false
- type: circle
radius: 0.02
center: [0.2, 0.1]
density: 0
layers: ["static"]
collision: false
- type: circle
radius: 0.02
center: [0.2, -0.1]
density: 0
layers: ["static"]
collision: false
- type: circle
radius: 0.02
center: [-0.2, 0.1]
density: 0
layers: ["static"]
collision: false
- type: circle
radius: 0.02
center: [-0.2, -0.1]
density: 0
layers: ["static"]
collision: false
================================================
FILE: flatland_setup/objects/palett.model.yaml
================================================
bodies: # List of named bodies
- name: pallet
pose: [0, 0, 0]
type: dynamic
color: [0.2, 0.8, 0.2, 0.75]
footprints:
- type: polygon
points: [[-0.6, -0.4], [-0.6, 0.4], [0.6, 0.4], [0.6, -0.4]]
density: 0.0
layers: ["static"]
collision: false
- name: dummy_for_laser
pose: [0, 0, 0]
type: dynamic
color: [1.0, 1.0, 1.0, 0.75]
footprints:
- type: circle
radius: 0.02
center: [0.25, 0.15]
density: 0.0
layers: ["static"]
collision: false
- type: circle
radius: 0.02
center: [0.25, -0.15]
density: 0.0
layers: ["static"]
collision: false
- type: circle
radius: 0.02
center: [-0.25, 0.15]
density: 0.0
layers: ["static"]
collision: false
- type: circle
radius: 0.02
center: [-0.25, -0.15]
density: 0.0
layers: ["static"]
collision: false
================================================
FILE: flatland_setup/objects/person_single_circle.model.yaml
================================================
# Person
bodies: # List of named bodies
- name: base
pose: [0, 0, 0]
type: dynamic
color: [1.0, 1.0, 1.0, 0.75]
footprints:
- type: circle
radius: 0.001
center: [0, 0]
density: 0
layers: []
collision: false
- name: left_leg
pose: [0, 0, 0]
type: dynamic
color: [0.66, 0.0, 0, 0.75]
footprints:
- type: circle
radius: 0.1
center: [0.0, 0]
density: 1
# layers: ["2d", "ped"]
layers: ["ped"]
collision: false
- name: right_leg
pose: [0, 0, 0]
type: dynamic
color: [0.66, 0.0, 0, 0.75]
footprints:
- type: circle
radius: 0.1
center: [-0.0, 0]
density: 1
# layers: ["2d", "ped"]
layers: ["ped"]
collision: false
plugins:
- type: PedsimMovement
name: pedsim_movement
agent_topic: /pedsim_simulator/simulated_agents
base_body: base
left_leg_body: left_leg
right_leg_body: right_leg
update_rate: 40
toggle_leg_movement: false
leg_offset: 0.0
var_leg_offset: 0.0
step_time: 0.3 #(in sec)
var_step_time: 0.1
leg_radius: 0.2
var_leg_radius: 0.03
enabled: true
# - type: ModelTfPublisher
# name: tf_publisher_
# exclude: ["left_leg", "right_leg"]
# publish_tf_world: true
================================================
FILE: flatland_setup/objects/person_two_legged.model.yaml
================================================
# Person
bodies: # List of named bodies
- name: base
pose: [0, 0, 0]
type: dynamic
color: [1.0, 1.0, 1.0, 0.75]
footprints:
- type: circle
radius: 0.001
center: [0, 0]
density: 0
layers: []
collision: false
- name: left_leg
pose: [0, 0, 0]
type: dynamic
color: [0.66, 0.0, 0, 0.75]
footprints:
- type: circle
radius: 0.1
center: [0.0, 0]
density: 1
# layers: ["2d", "ped"]
layers: ["ped"]
collision: false
- name: right_leg
pose: [0, 0, 0]
type: dynamic
color: [0.66, 0.0, 0, 0.75]
footprints:
- type: circle
radius: 0.1
center: [-0.0, 0]
density: 1
# layers: ["2d", "ped"]
layers: ["ped"]
collision: false
# joints:
# - type: weld
# name: left_leg_weld
# bodies:
# - name: base
# anchor: [0, 1]
# - name: left_leg
# anchor: [0, 0]
# - type: weld
# name: right_leg_weld
# bodies:
# - name: base
# anchor: [0, -1]
# - name: right_leg
# anchor: [0, 0]
plugins:
- type: PedsimMovement
name: pedsim_movement
agent_topic: /pedsim_simulator/simulated_agents
base_body: base
left_leg_body: left_leg
right_leg_body: right_leg
update_rate: 40
toggle_leg_movement: true
leg_offset: 0.3
var_leg_offset: 0.05
step_time: 0.6 #(in sec)
var_step_time: 0.1
leg_radius: 0.1
var_leg_radius: 0.02
# - type: ModelTfPublisher
# name: tf_publisher_
# exclude: ["left_leg", "right_leg"]
# publish_tf_world: true
================================================
FILE: flatland_setup/objects/wagon.model.yaml
================================================
# Round obstacle
bodies: # List of named bodies
- name: foot1
pose: [0, 0, 0]
type: dynamic
color: [0.2, 0.8, 0.2, 0.75]
footprints:
- type: circle
radius: 0.1
center: [0.25, 0.4]
density: 0
layers: ["static"]
collision: false
- name: foot2
pose: [0, 0, 0]
type: dynamic
color: [0.2, 0.8, 0.2, 0.75]
footprints:
- type: circle
radius: 0.1
center: [-0.25, 0.4]
density: 0
layers: ["static"]
collision: false
- name: foot3
pose: [0, 0, 0]
type: dynamic
color: [0.2, 0.8, 0.2, 0.75]
footprints:
- type: circle
radius: 0.1
center: [-0.25, -0.4]
density: 0
layers: ["static"]
collision: false
- name: foot4
pose: [0, 0, 0]
type: dynamic
color: [0.2, 0.8, 0.2, 0.75]
footprints:
- type: circle
radius: 0.1
center: [0.25, -0.4]
density: 0
layers: ["static"]
collision: false
================================================
FILE: flatland_setup/package.xml
================================================
<?xml version="1.0"?>
<package format="2">
<name>flatland_setup</name>
<version>0.0.0</version>
<description>The flatland_setup package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="ronja@todo.todo">ronja</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/flatland_setup</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
================================================
FILE: flatland_setup/robot/robot1.model.yaml
================================================
bodies: # List of named bodies
- name: base_footprint
pose: [0, 0, 0]
type: dynamic
color: [0.5, 0.5, 0.5, 1.0]
footprints:
- type: polygon
sensor: true
points: [[-0.445, -0.29], [-0.445, 0.29], [0.445, 0.29], [0.445, -0.29]]
density: 0
layers: ["static"]
collision: false
plugins:
- type: DiffDrive
name: diff_drive
body: base_footprint
odom_frame_id: odom
odom_pub: odom # topic odom is published on
pub_rate: 10
- type: Laser
name: static_laser
frame: static_laser_link
topic: static_laser
body: base_footprint
broadcast_tf: true
origin: [0.0, 0.0, 0.0]
range: 10
# angle: {min: -3.14159265359, max: 3.14159265359, increment: 0.05235987755}
angle: {min: -3.14159265359, max: 3.14159265359, increment: 0.06981317007}
noise_std_dev: 0.015
update_rate: 10
layers: ["static"]
- type: Laser
name: ped_laser
frame: ped_laser_link
topic: ped_laser
body: base_footprint
broadcast_tf: true
origin: [0.0, 0.0, 0.0]
range: 10
# angle: {min: -3.14159265359, max: 3.14159265359, increment: 0.05235987755}
angle: {min: -3.14159265359, max: 3.14159265359, increment: 0.06981317007}
noise_std_dev: 0.015
update_rate: 10
layers: ["ped"]
- type: ModelTfPublisher
name: tf_publisher
publish_tf_world: false
# exclude: ["approx_footprint"]
================================================
FILE: requirements.txt
================================================
absl-py==0.7.1
alabaster==0.7.12
argh==0.26.2
astor==0.7.1
atari-py==0.1.7
attrs==19.1.0
Babel==2.6.0
catkin-pkg==0.4.11
certifi==2019.3.9
chardet==3.0.4
Click==7.0
cloudpickle==0.8.1
configparser==4.0.2
coverage==4.5.3
cycler==0.10.0
dill==0.2.9
docutils==0.14
exception==0.1.0
future==0.17.1
gast==0.2.2
glob2==0.6
grpcio==1.19.0
gym==0.12.1
h5py==2.9.0
idna==2.8
imagesize==1.1.0
Jinja2==2.10.1
joblib==0.13.2
Keras-Applications==1.0.7
Keras-Preprocessing==1.0.9
kiwisolver==1.0.1
livereload==2.6.0
Markdown==3.1
MarkupSafe==1.1.1
matplotlib==3.0.3
mock==2.0.0
more-itertools==7.0.0
mpi4py==3.0.1
numpy==1.17.0
opencv-python==4.0.0.21
packaging==19.0
pandas==0.24.2
pathtools==0.1.2
pbr==5.1.3
Pillow==6.0.0
pkg-resources==0.0.0
pluggy==0.6.0
port-for==0.3.1
progressbar2==3.39.3
protobuf==3.7.1
py==1.8.0
pyglet==1.3.2
Pygments==2.3.1
PyOpenGL==3.1.0
pyparsing==2.4.0
pyquaternion==0.9.5
pytest==3.5.1
pytest-cov==2.6.1
python-dateutil==2.8.0
python-utils==2.3.0
pytz==2018.9
PyYAML==5.1
pyzmq==18.0.1
requests==2.21.0
rospkg==1.1.7
scipy==1.2.1
seaborn==0.9.0
six==1.12.0
snowballstemmer==1.2.1
Sphinx==2.0.1
sphinx-autobuild==0.7.1
sphinx-rtd-theme==0.4.3
sphinxcontrib-applehelp==1.0.1
sphinxcontrib-devhelp==1.0.1
sphinxcontrib-htmlhelp==1.0.1
sphinxcontrib-jsmath==1.0.1
sphinxcontrib-qthelp==1.0.2
sphinxcontrib-serializinghtml==1.1.3
-e git+git@github.com:RGring/stable-baselines.git@94ccad6d2da0c715d671f2f8f7dad22ef320cd5c#egg=stable_baselines
tensorboard==1.13.1
tensorflow==1.13.1
tensorflow-estimator==1.13.0
termcolor==1.1.0
tornado==6.0.2
tqdm==4.31.1
urllib3==1.24.1
watchdog==0.9.0
Werkzeug==0.15.2
zmq==0.0.0
================================================
FILE: rl_agent/CMakeLists.txt
================================================
cmake_minimum_required(VERSION 2.8.3)
project(rl_agent)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
# Get the information about this package's buildtime dependencies
find_package(catkin REQUIRED
COMPONENTS
rospy
roscpp
message_generation
std_msgs
sensor_msgs
laser_geometry
geometry_msgs
nav_msgs
rl_msgs
tf
pcl_ros
)
find_package(PCL REQUIRED)
catkin_python_setup()
# Declare that this catkin package's runtime dependencies
catkin_package(
CATKIN_DEPENDS
stable_baselines
message_runtime
std_msgs
rl_msgs
sensor_msgs
geometry_msgs
)
include_directories(
${catkin_INCLUDE_DIRS}
include
scripts
)
link_directories(${catkin_LIBRARY_DIRS})
add_executable(laser_scan_merger
src/laser_scan_merger.cpp
)
add_dependencies(laser_scan_merger
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
)
target_link_libraries(laser_scan_merger
${catkin_LIBRARIES}
)
add_executable(tf_python
src/tf_python.cpp
)
add_dependencies(tf_python
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
)
target_link_libraries(tf_python
${catkin_LIBRARIES}
)
catkin_install_python(PROGRAMS
scripts/train_scripts/train_ppo.py
scripts/run_scripts/run_ppo.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
install(DIRECTORY launch/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch/
)
install(PROGRAMS
start_scripts/run_launch_in_venv.sh
start_scripts/run_launch_in_venv_robot.sh
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/start_scripts/
)
install(
TARGETS
laser_scan_merger
tf_python
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
================================================
FILE: rl_agent/include/rl_agent/laser_scan_merger.h
================================================
/*
* @name laser_scan_merger.h
* @brief Merges several scans to a single one with coordinate system in robot_frame
* @author Ronja Gueldenring
* @date 2019/04/05
* @comment Code snippets are taken from: https://github.com/iralabdisco/ira_laser_tools
**/
#ifndef LASER_SCAN_MERGER_H
#define LASER_SCAN_MERGER_H
#include <rl_msgs/MergeScans.h>
#include <ros/ros.h>
#include <laser_geometry/laser_geometry.h>
#include <tf/transform_listener.h>
#include <pcl/point_cloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <pcl/conversions.h>
#include <sensor_msgs/PointCloud.h>
#include <pcl_ros/transforms.h>
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/point_cloud2_iterator.h>
namespace rl_agent {
/**
* This class merges several laserscan to one single laser scan.
*/
class LaserScanMerger{
public:
LaserScanMerger(const ros::NodeHandle& node_handle);
private:
ros::NodeHandle nh_;
laser_geometry::LaserProjection projector_;
tf::TransformListener listener_;
/**
*
* @brief Generates a Laserscan from a pointcloud
*
**/
sensor_msgs::LaserScanPtr pointcloud_to_laserscan(Eigen::MatrixXf points, pcl::PCLPointCloud2 *merged_cloud);
/**
* merge_service_ + merge_scan_callback
* @brief Merges several laserscan to one.
*
**/
ros::ServiceServer merge_service_;
bool merge_scan_callback(rl_msgs::MergeScans::Request& request, rl_msgs::MergeScans::Response& response);
double angle_increment_, time_increment_, range_min_, range_max_, scan_time_;
double max_height_, min_height_;
std::string robot_frame_;
};
};
#endif /* LASER_SCAN_MERGER_H */
================================================
FILE: rl_agent/include/rl_agent/tf_python.h
================================================
/**
*
* Ronja Gueldenring
* his class is needed, because rospy and python3.5 is not compatible regarding
* the tf-package.
* The class provides all transformation needed by rl_agent in python
*
**/
#ifndef TF_PYTHON_H
#define TF_PYTHON_H
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/transform_listener.h>
namespace rl_agent {
/**
* @class TFPython
* @brief this class is needed, because rospy and python3.5 is not compatible regarding
* the tf-package.
* The class provides all transformation needed by rl_agent in python
*
*/
class TFPython{
public:
TFPython(const ros::NodeHandle& node_handle);
void robot_to_goal_transform();
private:
ros::NodeHandle nh_;
geometry_msgs::PoseStamped goal_;
ros::Subscriber goal_sub_;
void goal_callback(const geometry_msgs::PoseStamped& goal);
ros::Publisher transformed_goal_pub_;
tf::TransformListener listener_;
};
};
#endif /* TF_PYTHON_H */
================================================
FILE: rl_agent/launch/run_1_img_disc.launch
================================================
<launch>
<arg name="mode" />
<node pkg="rl_agent" type="run_ppo.py" name="run_ppo.py" output="screen" args="ppo2_1_img_disc_0 CnnPolicy $(arg mode) 1 0 1 ped 1" />
</launch>
================================================
FILE: rl_agent/launch/run_1_raw_cont.launch
================================================
<launch>
<arg name="mode" />
<node pkg="rl_agent" type="run_ppo.py" name="run_ppo.py" output="screen" args="ppo2_1_raw_data_cont_0 CNN1DPolicy_multi_input $(arg mode) 1 0 0 ped 1" />
</launch>
================================================
FILE: rl_agent/launch/run_1_raw_disc.launch
================================================
<launch>
<arg name="mode" />
<node pkg="rl_agent" type="run_ppo.py" name="run_ppo.py" output="screen" args="ppo2_1_raw_data_disc_0 CNN1DPolicy_multi_input $(arg mode) 1 0 1 ped 1" />
</launch>
================================================
FILE: rl_agent/launch/run_3_raw_disc.launch
================================================
<launch>
<arg name="mode" />
<node pkg="rl_agent" type="run_ppo.py" name="run_ppo.py" output="screen" args="ppo2_3_raw_data_disc_0 CNN1DPolicy_multi_input $(arg mode) 1 0 1 ped 3" />
</launch>
================================================
FILE: rl_agent/launch/run_ppo_agent.launch
================================================
<launch>
<arg name="mode" />
<node pkg="rl_agent" type="run_ppo.py" name="run_ppo.py" output="screen" args="ppo2_foo CNN1DPolicy_multi_input $(arg mode) 1 0 0 ped 3" />
</launch>
================================================
FILE: rl_agent/package.xml
================================================
<?xml version="1.0"?>
<package format="2">
<name>rl_agent</name>
<version>0.0.0</version>
<description>The rl_agent package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="ronja@todo.todo">ronja</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/rl_agent</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>rospy</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>rl_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>stable_baselines</build_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>rl_msgs</exec_depend>
<exec_depend>message_runtime</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>stable_baselines</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
================================================
FILE: rl_agent/scripts/evaluation/analysis.py
================================================
'''
@name: analysis.py
@brief: Analysis of evaluation data
@author: Ronja Gueldenring
@version: 3.5
@date: 2019/04/05
'''
import os
home = os.path.expanduser("~")
import rospkg
from rl_agent.evaluation.Analysis_eval import Analysis
import pickle
import configparser
def analyse(complexity, evaluation_file_path, reward_file_path, save_path):
analysis = Analysis()
results = analysis.load_results(evaluation_file_path)
print("loaded data: %s"%evaluation_file_path)
timesteps = analysis.get_timestep_list(results)
# timesteps = analysis.reconstruct_timestep_array(timesteps)
success, time_exceeded, collision = analysis.get_scores(results)
if (complexity == "train"):
[timesteps_tb, reward] = analysis.get_reward(reward_file_path)
else:
perc_success_drive = analysis.get_percentual_success_drive(results)
path_ratio = analysis.get_path_length_ratio(results)
speed = analysis.get_speed(results)
print("saving results...")
# Collecting all results in a dict
analysis_results = {}
analysis_results["timesteps"] = timesteps
analysis_results["success"] = success
analysis_results["time_exceeded"] = time_exceeded
analysis_results["collision"] = collision
# Saving results
if complexity == "train":
analysis_results["timesteps_tb"] = timesteps_tb
analysis_results["reward"] = reward
else:
analysis_results["perc_success_drive"] = perc_success_drive
analysis_results["path_ratio"] = path_ratio
analysis_results["speed"] = speed
with open('%s.pickle' % (save_path), 'wb') as handle:
pickle.dump(analysis_results, handle, protocol=pickle.HIGHEST_PROTOCOL)
if __name__ == '__main__':
complexity = "complex_map_3" # train, simple, average, complex, follow_path,
task_type = "ped" # static or ped
no_episodes = 700
agent_names = ["ppo2_109", "ppo2_99", "ppo2_107", "ppo2_108"]
rospack = rospkg.RosPack()
rl_bringup_path = rospack.get_path('rl_bringup')
config = configparser.ConfigParser()
config.read('%s/config/path_config.ini' % rl_bringup_path)
path_to_eval_data_train = config['PATHES']['path_to_eval_data_train']
path_to_eval_data_test = config['PATHES']['path_to_eval_data_test']
for agent_name in agent_names:
if complexity == "train":
evaluation_file_path = "%s/%s_training"%(path_to_eval_data_train, agent_name)
elif complexity == "follow_path":
evaluation_file_path = "%s/%s_following_path"%(path_to_eval_data_test, agent_name)
else:
evaluation_file_path = "%s/%s_%s_eval_set_%s_%d" % (path_to_eval_data_test, agent_name, task_type, complexity, no_episodes)
reward_file_path = "%s/run_%s_reward" % (path_to_eval_data_train, agent_name)
# Saving results
if complexity == "train":
save_path = "%s/%s_analysis" % (path_to_eval_data_train, agent_name)
elif complexity == "follow_path":
save_path = "%s/%s_analysis_follow_path" % (path_to_eval_data_test, agent_name)
else:
save_path = "%s/%s_%s_analysis_eval_set_%s_%s" % (
path_to_eval_data_test, agent_name, task_type, complexity, no_episodes)
analyse(complexity, evaluation_file_path, reward_file_path, save_path)
================================================
FILE: rl_agent/scripts/evaluation/evaluate_agent.py
================================================
'''
@name: evaluate_agent.py
@brief: Evaluates an agent according to a test set.
@author: Ronja Gueldenring
@version: 3.5
@date: 2019/04/05
'''
import os
home = os.path.expanduser("~")
import rospy
import rospkg
from multiprocessing import Process
from scripts.run_scripts.run_ppo import run_ppo
from rl_agent.env_utils.state_collector import StateCollector
from rl_agent.evaluation.Evaluation import Evaluation
import time
import configparser
def evaluate(ns, sc, evaluation_set_path, save_path):
rospy.init_node("evaluate_node", anonymous=True)
eval = Evaluation(sc, ns)
time.sleep(2)
eval.evaluate_set(evaluation_set_path, save_path)
if __name__ == '__main__':
task_type = "ped" # static, ped
complexity = "complex_map_3" # simple, average, complex
no_episodes = 700
ns = "sim1"
approach = "PPO2" # PPO1, PPO2
policy = ["CnnPolicy", "CNN1DPolicy_multi_input", "CnnPolicy", "CnnPolicy"]
disc_action_space = [False, True, True, True]
agent_names = ["ppo2_109", "ppo2_99", "ppo2_107", "ppo2_108"]
num_stacks = [1,1,1,3]
stack_offset = 15
rospack = rospkg.RosPack()
rl_bringup_path = rospack.get_path('rl_bringup')
config = configparser.ConfigParser()
config.read('%s/config/path_config.ini' % rl_bringup_path)
path_to_eval_sets = config['PATHES']['path_to_eval_sets']
path_to_eval_data_test = config['PATHES']['path_to_eval_data_test']
evaluation_set_name = "%s_eval_set_%s_%d"%(task_type, complexity, no_episodes)
evaluation_set_path = "%s/%s"%(path_to_eval_sets, evaluation_set_name)
mode = "exec"
for i, agent_name in enumerate(agent_names):
save_path = "%s/%s_%s" % (path_to_eval_data_test, agent_name, evaluation_set_name)
sc = StateCollector(ns, "eval")
p = Process(target=run_ppo, args=(config, sc, approach, agent_name , policy[i] , mode, task_type,
stack_offset, num_stacks[i], True, False, disc_action_space[i], ns))
p.start()
print("Starting evaluation of agent %s with set %s"%(agent_name, evaluation_set_name))
print("--------------------------------------------------------------------------------------")
p_eval = Process(target=evaluate, args=(ns, sc, evaluation_set_path, save_path))
p_eval.start()
p_eval.join()
p.terminate()
================================================
FILE: rl_agent/scripts/evaluation/qualitative_plot.py
================================================
'''
@name: qualitative_plot.py
@brief: Generates a situation in rviz, that shows the agents qualitative behaviour.
@author: Ronja Gueldenring
@version: 3.5
@date: 2019/04/05
'''
import os
home = os.path.expanduser("~")
import rospy
import rospkg
from rl_agent.evaluation.Analysis_eval import Analysis
from rl_agent.evaluation.Evaluation import Evaluation
from rl_agent.env_utils.state_collector import StateCollector
import time
import configparser
if __name__ == '__main__':
rospy.init_node("qualitative_image_generation_node", anonymous=True)
##########################################
#### Training/Testin setup properties ####
#### Please adjust. ####
##########################################
ns = "sim1"
complexity = "average" # simple, average, complex
task_type = "static" # static, ped
no_episodes = 3
agent_name_1 = "ppo2_foo_512"
agent_name_2 = ""
rospack = rospkg.RosPack()
rl_bringup_path = rospack.get_path('rl_bringup')
config = configparser.ConfigParser()
config.read('%s/config/path_config.ini' % rl_bringup_path)
path_to_eval_sets = config['PATHES']['path_to_eval_sets']
path_to_eval_data_test = config['PATHES']['path_to_eval_data_test']
path_to_agent_1_results = "%s/%s_static_eval_set_%s_%d" % (path_to_eval_data_test, agent_name_1, complexity, no_episodes)
path_to_agent_2_results = "%s/%s_static_eval_set_%s_%d" % (path_to_eval_data_test, agent_name_2, complexity, no_episodes)
path_to_evaluation_set = "%s/%s_eval_set_%s_%d"%(path_to_eval_sets, task_type, complexity, no_episodes)
print("Loading sets...")
analysis = Analysis()
results_agent_2 = [] # Empty, if no second results are needed.
results_agent_1 = analysis.load_results(path_to_agent_1_results)
if(task_type == "static"):
if agent_name_2 != "":
results_agent_2 = analysis.load_results(path_to_agent_1_results)
eval = Evaluation(StateCollector(ns, "eval"), ns)
time.sleep(2)
print("Printing set.")
len_eval_set = eval.load_evaluation_set(path_to_evaluation_set)
for i in range(0, len_eval_set):
episode_pos = 10
while episode_pos >= 0:
if (task_type == "static"):
eval.generate_qualitative_static_image_rviz(results_agent_1, results_agent_2, i, episode_pos)
elif (task_type == "ped"):
eval.generate_qualitative_ped_image_rviz(results_agent_1, i, episode_pos)
episode_pos = int(input("Pos of episode %d ? (-1 for plotting next episode)" % i) or "50")
================================================
FILE: rl_agent/scripts/evaluation/save_test_episodes.py
================================================
'''
@name: save_test_episodes.py
@brief: For saving n different episodes. Each episode needs to be rewiewed by the user and the
following key press, trigger the followin action:
[Y] Episode is added to the evaluation set
[n] Episode is rejected
[s] set is saved
[e] quit program
@author: Ronja Gueldenring
@version: 3.5
@date: 2019/04/05
'''
import os
home = os.path.expanduser("~")
import rospy
import rospkg
import pickle
from rl_agent.env_utils.task_generator import TaskGenerator
from rl_agent.env_utils.state_collector import StateCollector
import math
from operator import itemgetter
import time
import configparser
if __name__ == '__main__':
rospy.init_node("episode_collector", anonymous=True)
##########################################
#### Training/Testin setup properties ####
#### Please adjust. ####
##########################################
ns = "sim1"
name = ""
task_type = "ped" # static, ped
complexity = "complex_map_1" # simple, average, complex
rospack = rospkg.RosPack()
rl_bringup_path = rospack.get_path('rl_bringup')
config = configparser.ConfigParser()
config.read('%s/config/path_config.ini' % rl_bringup_path)
path_to_eval_sets = config['PATHES']['path_to_eval_sets']
save_path = "%s/%s_eval_set_%s"%(path_to_eval_sets, task_type, complexity)
state_collector = StateCollector(ns, "train")
t_g = TaskGenerator(ns, state_collector, 0.56)
if name != "":
with open('%s/%s.pickle' % (path_to_eval_sets, name), 'rb') as handle:
episodes = pickle.load(handle)
count = len(episodes)
else:
episodes = []
count = 0
# Sleeping so that py-Publisher has time to setup!
time.sleep(1)
while True:
print("Episode details:")
if (task_type == "static"):
d = t_g.set_random_static_task()
print("\t -Start: (%d, %d)"%(d["start"][0], d["start"][1]))
print("\t -Goal: (%d, %d)"%(d["goal"][0], d["goal"][1]))
print("\t -Obstacles")
for i in range(len(d["static_objects"]["x"])):
print("\t \t *%s: (%f, %f)"% (d["static_objects"]["model_name"][i], d["static_objects"]["x"][i], d["static_objects"]["y"][i]))
print("\n")
elif (task_type == "ped"):
d = t_g.set_random_ped_task()
for i in range(10):
t_g.take_sim_step()
time.sleep(0.05)
print("\t -Start: (%d, %d)" % (d["start"][0], d["start"][1]))
print("\t -Goal: (%d, %d)" % (d["goal"][0], d["goal"][1]))
print("\t -Peds")
ped_count = 0
for ped in d["peds"]:
ped_poses = []
for i in ped[2]:
ped_poses.append(i)
print("\t\t*Ped%d"%ped_count)
for i in ped_poses:
print("\t\t ", i)
ped_count +=1
print("\n")
else:
print("wrong task_type!")
break
path_length = 0
if (len(d["path"].poses) <=2):
print("Path length <= 2")
continue
old_pose = d["path"].poses[0].pose.position
for pose in d["path"].poses[1:]:
new_pose = pose.pose.position
path_length += math.sqrt(math.pow((new_pose.x - old_pose.x), 2) + math.pow((new_pose.y - old_pose.y), 2))
old_pose = new_pose
d["path_length"] = path_length
print("path_length: %f"%path_length)
print("path_start: (%f, %f)"%(d["path"].poses[0].pose.position.x, d["path"].poses[0].pose.position.y))
char = str(input("Episode valid? [Y][n]"))
if (char == "n"):
print("Episode rejected!")
elif (char == "" or char == "y" or char == "Y"):
episodes.append(d)
print("Added episode %d"%count)
count+=1
elif (char == "s"):
print("Saving episodes...")
episodes = sorted(episodes, key=itemgetter('path_length'))
with open('%s_%d.pickle'%(save_path, count), 'wb') as handle:
pickle.dump(episodes, handle, protocol=pickle.HIGHEST_PROTOCOL)
elif (char == "e"):
break
================================================
FILE: rl_agent/scripts/run_scripts/run_ppo.py
================================================
#! /usr/bin/env python
'''
@name: run_ppo.py
@brief: Trained agent is loaded and executed.
@author: Ronja Gueldenring
@version: 3.5
@date: 2019/04/05
'''
import os
import sys
import rospy
import rospkg
import configparser
import time
import numpy as np
from rl_agent.env_wrapper.ros_env_cont_img import RosEnvContImg
from rl_agent.env_wrapper.ros_env_cont_raw_data import RosEnvContRaw
from rl_agent.env_wrapper.ros_env_disc_raw_data import RosEnvDiscRaw
from rl_agent.env_wrapper.ros_env_cont_raw_scan_prep_wp import RosEnvContRawScanPrepWp
from rl_agent.env_wrapper.ros_env_disc_raw_scan_prep_wp import RosEnvDiscRawScanPrepWp
from rl_agent.env_wrapper.ros_env_cont_img_vel import RosEnvContImgVel
from rl_agent.env_wrapper.ros_env_disc_img_vel import RosEnvDiscImgVel
from rl_agent.env_wrapper.ros_env_disc_img import RosEnvDiscImg
from rl_agent.env_utils.state_collector import StateCollector
from stable_baselines.common.vec_env import DummyVecEnv, VecNormalize, VecFrameStack
from stable_baselines.ppo2.ppo2 import PPO2
from stable_baselines.ppo1.pposgd_simple import PPO1
def load_train_env(ns, state_collector, robot_radius, rew_fnc, num_stacks,
stack_offset, debug, task_mode, rl_mode, policy, disc_action_space, normalize):
# Choosing environment wrapper according to the policy
if policy == "CnnPolicy" or policy == "CnnLnLstmPolicy" or policy == "CnnLstmPolicy":
if disc_action_space:
env_temp = RosEnvDiscImg
else:
env_temp = RosEnvContImg
elif policy in ["CNN1DPolicy", "CNN1DPolicy2", "CNN1DPolicy3"]:
if disc_action_space:
env_temp = RosEnvDiscRawScanPrepWp
else:
env_temp = RosEnvContRawScanPrepWp
elif policy == "CNN1DPolicy_multi_input":
if disc_action_space:
env_temp = RosEnvDiscRaw
else:
env_temp = RosEnvContRaw
elif policy == "CnnPolicy_multi_input_vel" or policy == "CnnPolicy_multi_input_vel2":
if disc_action_space:
env_temp = RosEnvDiscImgVel
else:
env_temp = RosEnvContImgVel
env_raw = DummyVecEnv([lambda: env_temp(ns, state_collector, stack_offset, num_stacks, robot_radius, rew_fnc, debug, rl_mode, task_mode)])
if normalize:
env = VecNormalize(env_raw, training=True, norm_obs=True, norm_reward=False, clip_obs=100.0, clip_reward=10.0,
gamma=0.99, epsilon=1e-08)
else:
env = env_raw
# Stack of data?
if num_stacks > 1:
env = VecFrameStack(env, n_stack=num_stacks, n_offset=stack_offset)
return env
def run_ppo(config, state_collector, agent_name ="ppo_99_8507750", policy ="CnnPolicy", mode="train", task_mode="static",
stack_offset=15, num_stacks=1, debug=True, normalize = True, disc_action_space = False, ns=""):
path_to_models = config['PATHES']['path_to_models']
# Loading agent
model = PPO2.load("%s/%s/%s.pkl" % (path_to_models, agent_name, agent_name))
print("Loaded %s" % agent_name)
print("--------------------------------------------------")
print("Normalize: ", normalize)
print("Policy: %s" % policy)
print("Discrete action space: ", disc_action_space)
print("Observation space size: ", model.observation_space.shape)
print("Debug: ", debug)
print("Number of stacks: %d, stack offset: %d" % ( model.observation_space.shape[2], stack_offset))
print("\n")
#Loading environment
env = load_train_env(ns, state_collector, 0.46, 19, num_stacks, stack_offset, debug, task_mode, mode, policy, disc_action_space, normalize)
# Resetting environment
if mode == "train" or mode == "eval":
obs = env.reset()
if mode == "exec" or mode == "exec_rw":
if disc_action_space:
obs, rewards, dones, info = env.step([5])
else:
obs, rewards, dones, info = env.step([[0.0, 0.0]])
if debug:
#Cummulative reward.
cum_reward = 0
while True:
#Determining action vor given observation
action, _states = model.predict(obs)
# Clipping actions
if not disc_action_space:
action = np.maximum(np.minimum(model.action_space.high, action), model.action_space.low)
#Executing action in environment
obs, rewards, dones, info = env.step(action)
if debug:
cum_reward += rewards
# Episode over?
if dones:
print("Episode finished with reward of %f."% cum_reward)
cum_reward = 0
time.sleep(0.0001)
if rospy.is_shutdown():
print('shutdown')
break
if __name__ == '__main__':
rospack = rospkg.RosPack()
rl_bringup_path = rospack.get_path('rl_bringup')
config = configparser.ConfigParser()
config.read('%s/config/path_config.ini'%rl_bringup_path)
# for running from terminal (e.g. launch-file)
if (len(sys.argv) > 1):
ns = "sim1"
sc = StateCollector(ns, str(sys.argv[3]))
run_ppo(config, sc,
ns=ns,
agent_name=str(sys.argv[1]),
policy=str(sys.argv[2]),
mode=str(sys.argv[3]),
debug=bool(int(sys.argv[4])),
normalize=bool(int(sys.argv[5])),
disc_action_space=bool(int(sys.argv[6])),
task_mode=str(sys.argv[7]),
num_stacks=int(sys.argv[8]))
# for quick testing
else:
mode = "train"
ns = "sim1"
policy = "CnnPolicy_multi_input_vel2"
agent_name = "ppo2_35_8001000"
sc = StateCollector(ns, mode)
run_ppo(config, sc,
agent_name= agent_name,
policy = policy,
mode = mode,
debug = True,
normalize=False,
disc_action_space=True,
task_mode="ped",
num_stacks=4,
stack_offset = 15,
ns=ns)
================================================
FILE: rl_agent/scripts/train_scripts/train_ppo.py
================================================
'''
@name: train_ppo.py
@brief: Starts a ppo2-training process. It is expected that move_base, simulation etc is started.(roslaunch rl_setup_bringup setup.launch)
@author: Ronja Gueldenring
@version: 3.5
@date: 2019/04/05
'''
import os
import sys
import rospy
import rospkg
import configparser
from rl_agent.env_wrapper.ros_env_cont_img import RosEnvContImg
from rl_agent.env_wrapper.ros_env_cont_raw_data import RosEnvContRaw
from rl_agent.env_wrapper.ros_env_disc_raw_data import RosEnvDiscRaw
from rl_agent.env_wrapper.ros_env_cont_img_vel import RosEnvContImgVel
from rl_agent.env_wrapper.ros_env_disc_img_vel import RosEnvDiscImgVel
from rl_agent.env_wrapper.ros_env_cont_raw_scan_prep_wp import RosEnvContRawScanPrepWp
from rl_agent.env_wrapper.ros_env_disc_raw_scan_prep_wp import RosEnvDiscRawScanPrepWp
from rl_agent.env_wrapper.ros_env_disc_img import RosEnvDiscImg
from rl_agent.env_utils.state_collector import StateCollector
from stable_baselines.common.vec_env import VecNormalize, SubprocVecEnv, VecFrameStack
from rl_agent.evaluation.Evaluation import Evaluation
from multiprocessing import Process
import random
from rl_agent.common_custom_policies import *
from stable_baselines.common.policies import *
from stable_baselines.ppo2 import PPO2
from stable_baselines.bench import Monitor
from stable_baselines.results_plotter import load_results, ts2xy
import numpy as np
best_mean_reward, n_callback = -np.inf, 0
agent_name = ""
path_to_models = ""
def train_callback(_locals, _globals):
"""
Callback called at each step (for DQN an others) or after n steps (see ACER or PPO2)
:param _locals: (dict)
:param _globals: (dict)
"""
global n_callback, best_mean_reward, agent_name, path_to_models
# Print stats every 1000 calls
if (n_callback + 1) % 10 == 0:
# Evaluate policy performance
x, y = ts2xy(load_results('%s/%s/'%(path_to_models, agent_name)), 'timesteps')
if len(x) > 0:
mean_reward = np.mean(y[-100:])
print(x[-1], 'timesteps')
print("Best mean reward: {:.2f} - Last mean reward per episode: {:.2f}".format(best_mean_reward, mean_reward))
# New best model, you could save the agent here
if mean_reward > best_mean_reward:
best_mean_reward = mean_reward
# Example for saving best model
print("Saving new best model")
_locals['self'].save(path_to_models + '/%s/%s.pkl' % (agent_name, agent_name))
n_callback += 1
return True
def load_train_env(num_envs, robot_radius, rew_fnc, num_stacks, stack_offset, debug, task_mode, policy, disc_action_space, normalize):
# Choosing environment wrapper according to the policy
if policy == "CnnPolicy" or policy == "CnnLnLstmPolicy" or policy == "CnnLstmPolicy":
if disc_action_space:
env_temp = RosEnvDiscImg
else:
env_temp = RosEnvContImg
elif policy == "CNN1DPolicy":
if disc_action_space:
env_temp = RosEnvDiscRawScanPrepWp
else:
env_temp = RosEnvContRawScanPrepWp
elif policy == "CNN1DPolicy_multi_input":
if disc_action_space:
env_temp = RosEnvDiscRaw
else:
env_temp = RosEnvContRaw
elif policy == "CnnPolicy_multi_input_vel" or policy == "CnnPolicy_multi_input_vel2":
if disc_action_space:
env_temp = RosEnvDiscImgVel
else:
env_temp = RosEnvContImgVel
env = SubprocVecEnv([lambda k=k: Monitor(env_temp("sim%d" % (k+1), StateCollector("sim%s"%(k+1), "train") , stack_offset, num_stacks, robot_radius, rew_fnc, debug, "train", task_mode), '%s/%s/sim_%d'%(path_to_models, agent_name, k+1), allow_early_resets=True) for k in range(num_envs)])
# Normalizing?
if normalize:
env = VecNormalize(env, training=True, norm_obs=True, norm_reward=False, clip_obs=100.0, clip_reward=10.0,
gamma=0.99, epsilon=1e-08)
else:
env = env
# Stack of data?
if num_stacks > 1:
env = VecFrameStack(env, n_stack=num_stacks, n_offset=stack_offset)
return env
def train_agent_ppo2(config, agent_name, total_timesteps, policy,
gamma=0.99, n_steps=128, ent_coef=0.01, learning_rate=0.00025,
vf_coef=0.5, max_grad_norm=0.5, lam=0.95, nminibatches=4, noptepochs=4,
cliprange=0.2, num_envs=1, robot_radius = 0.46, rew_fnc=3, num_stacks=1, stack_offset=15, disc_action_space = False,
debug=False, normalize=False,
stage=0, pretrained_model_name="", task_mode="static"):
# Setting seed
seed = random.randint(0,1000)
np.random.seed(seed)
tf.random.set_random_seed(seed)
random.seed(seed)
# Define pathes to store things
path_to_tensorboard_log = config['PATHES']['path_to_tensorboard_log']
global path_to_models
path_to_models = config['PATHES']['path_to_models']
agent_dir='%s/%s'%(path_to_models, agent_name)
if not os.path.exists(agent_dir):
os.makedirs(agent_dir)
# Loading simulation environment
env = load_train_env(num_envs,
robot_radius,
rew_fnc,
num_stacks,
stack_offset,
debug,
task_mode,
policy,
disc_action_space,
normalize)
if stage==0:
model = PPO2(eval(policy), env, gamma=gamma,
n_steps=n_steps, ent_coef=ent_coef,
learning_rate=learning_rate, vf_coef=vf_coef, max_grad_norm=max_grad_norm,
lam=lam, nminibatches=nminibatches, noptepochs=noptepochs,
cliprange=cliprange, verbose=1,
tensorboard_log='%s' % (path_to_tensorboard_log))
else:
# Pretrained model is loaded to continue training.
model = PPO2.load("%s/%s/%s.pkl" % (path_to_models, pretrained_model_name, pretrained_model_name), env,
tensorboard_log='%s'%(path_to_tensorboard_log))
# Document agent
print("Starting PPO2 Training of agent: %s" %(agent_name))
print("------------------------------------------------------")
print("gamma \t\t\t\t %f" %model.gamma)
print("n_steps \t\t\t %d" %model.n_steps)
print("ent_coef \t\t\t %f" %model.ent_coef)
print("learning_rate \t\t\t %f" %learning_rate)
print("vf_coef \t\t\t %f" %model.vf_coef)
print("max_grad_norm \t\t\t %f" %model.max_grad_norm)
print("lam \t\t\t\t %f" %model.lam)
print("nminibatches \t\t\t %d" %model.nminibatches)
print("noptepochs \t\t\t %d" %model.noptepochs)
print("cliprange \t\t\t %f" %cliprange)
print("total_timesteps \t\t %d" %total_timesteps)
print("Policy \t\t\t\t %s" %policy)
print("reward_fnc \t\t\t %d" %rew_fnc)
print("Normalized state: %d" % normalize)
print("discrete action space %d" % disc_action_space)
print("Number of stacks: %d, stack offset: %d" % (num_stacks, stack_offset))
print("\n")
# Starting training
reset_num_timesteps = False
if stage==0:
reset_num_timesteps = True
model.learn(total_timesteps=total_timesteps, log_interval=100, callback=train_callback, tb_log_name=agent_name, reset_num_timesteps=reset_num_timesteps)
# Saving final model
model.save("%s/%s/%s" % (path_to_models, agent_name, "%s_stage_%d" % (agent_name, stage)))
print("Training finished.")
env.close()
def evaluate_during_training(ns, save_path, robot_radius):
rospy.init_node("evaluate_node", anonymous=True)
eval = Evaluation(StateCollector(ns, "train"), ns, robot_radius=robot_radius)
eval.evaluate_training(save_path)
if __name__ == '__main__':
record_evaluation_data = False
rospack = rospkg.RosPack()
rl_bringup_path = rospack.get_path('rl_bringup')
config = configparser.ConfigParser()
config.read('%s/config/path_config.ini'%rl_bringup_path)
path_to_eval_data_train = config['PATHES']['path_to_eval_data_train']
# for running via ./entrypoint_ppo2.sh
if (len(sys.argv) > 1):
agent_name = str(sys.argv[1])
stage = int(sys.argv[20])
record_processes = []
if record_evaluation_data:
save_path = "%s/%s_training" % (path_to_eval_data_train, str(sys.argv[1]))
for i in range(int(sys.argv[23])):
p = Process(target=evaluate_during_training, args=("sim%d" % (i + 1), save_path, float(sys.argv[14])))
p.start()
record_processes.append(p)
train_agent_ppo2(config, agent_name, int(sys.argv[2]), str(sys.argv[3]), gamma=float(sys.argv[4]),
n_steps=int(sys.argv[5]), ent_coef=float(sys.argv[6]),
learning_rate=float(sys.argv[7]), vf_coef=float(sys.argv[8]),
max_grad_norm=float(sys.argv[9]), lam=float(sys.argv[10]),
nminibatches=int(sys.argv[11]), noptepochs=int(sys.argv[12]),
cliprange=float(sys.argv[13]), robot_radius=float(sys.argv[14]),
rew_fnc=float(sys.argv[15]), num_stacks=int(sys.argv[16]),
stack_offset=int(sys.argv[17]),
disc_action_space=bool(int(sys.argv[18])), normalize=bool(int(sys.argv[19])),
stage=stage,
pretrained_model_name = str(sys.argv[21]),
task_mode=str(sys.argv[22]), num_envs=int(sys.argv[23]))
for p in record_processes:
p.terminate()
# for quick testing
else:
num_envs = 1
stage = 0
agent_name = "ppo2_foo"
robot_radius = 0.5
record_processes = []
if record_evaluation_data:
save_path = "%s/%s_training" % (path_to_eval_data_train, agent_name)
for i in range(num_envs):
p = Process(target=evaluate_during_training, args=("sim%d"%(i+1), save_path, robot_radius))
p.start()
record_processes.append(p)
train_agent_ppo2(config,
agent_name,
gamma=0.99,
n_steps=128,
ent_coef=0.005,
learning_rate=0.00025,
cliprange=0.2,
total_timesteps=10000000,
policy="CNN1DPolicy_multi_input",
num_envs=num_envs,
nminibatches=1,
noptepochs=1,
debug=True,
rew_fnc = 19,
num_stacks= 3,
stack_offset=5,
disc_action_space=False,
robot_radius = robot_radius,
stage=stage,
pretrained_model_name="ppo2_foo",
task_mode="ped")
for p in record_processes:
p.terminate()
================================================
FILE: rl_agent/setup.py
================================================
## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
# fetch values from package.xml
setup_args = generate_distutils_setup(
packages=['rl_agent',
'rl_agent.env_wrapper',
'rl_agent.env_utils',
'rl_agent.evaluation'],
package_dir={'': 'src'},
)
setup(**setup_args)
================================================
FILE: rl_agent/src/__init__.py
================================================
================================================
FILE: rl_agent/src/laser_scan_merger.cpp
================================================
/*
* @name laser_scan_merger.cpp
* @brief Merges several scans to a single one with coordinate system in robot_frame
* @author Ronja Gueldenring
* @date 2019/04/05
* @comment Code snippets are taken from: https://github.com/iralabdisco/ira_laser_tools
**/
#include <rl_agent/laser_scan_merger.h>
namespace rl_agent {
LaserScanMerger::LaserScanMerger(const ros::NodeHandle& node_handle): nh_(node_handle){
std::string merge_service_name_ = ros::this_node::getName() + "/merge_scans";
merge_service_ = nh_.advertiseService("merge_scans", &LaserScanMerger::merge_scan_callback, this);
nh_.getParam("rl_agent/robot_frame", robot_frame_);
min_height_ = 0.0;
max_height_ = 0.25;
}
bool LaserScanMerger::merge_scan_callback(rl_msgs::MergeScans::Request& request,
rl_msgs::MergeScans::Response& response){
int num_laser_scan = request.scans.size();
if (num_laser_scan <= 0){
return false;
}
//Scan properties of first scan are saved for merged scan
this->angle_increment_ = request.scans[0].angle_increment;
this->time_increment_ = request.scans[0].time_increment;
this->scan_time_ = request.scans[0].scan_time;
this->range_min_ = request.scans[0].range_min;
this->range_max_ = request.scans[0].range_max;
// Each scan is transformed to a pointcloud
pcl::PCLPointCloud2 clouds[num_laser_scan];
for (int i = 0; i < num_laser_scan; i++){
sensor_msgs::LaserScan scan_in = request.scans[i];
if(!listener_.waitForTransform(scan_in.header.frame_id, robot_frame_,
scan_in.header.stamp + ros::Duration().fromSec(scan_in.ranges.size()*scan_in.time_increment),
ros::Duration(1.0))){
ROS_WARN("Duration exceeded");
return false;
}
sensor_msgs::PointCloud cloud_temp_1;
sensor_msgs::PointCloud2 cloud_temp_2;
projector_.transformLaserScanToPointCloud(robot_frame_ , scan_in , cloud_temp_1 , listener_);
sensor_msgs::convertPointCloudToPointCloud2(cloud_temp_1 , cloud_temp_2);
pcl_conversions::toPCL(cloud_temp_2, clouds[i]);
}
// All pointclouds are concatenated
pcl::PCLPointCloud2 merged_cloud = clouds[0];
for(int i=1; i<num_laser_scan; ++i)
{
pcl::concatenatePointCloud(merged_cloud, clouds[i], merged_cloud);
}
// From the concatenated pointcloud, a laserscan is generated.
Eigen::MatrixXf points;
getPointCloudAsEigen(merged_cloud , points);
sensor_msgs::LaserScanPtr merged_scan = pointcloud_to_laserscan(points, &merged_cloud);
response.merged_scan = *merged_scan;
return true;
}//merge_scan_callback
sensor_msgs::LaserScanPtr LaserScanMerger::pointcloud_to_laserscan(Eigen::MatrixXf points, pcl::PCLPointCloud2 *merged_cloud){
sensor_msgs::LaserScanPtr output(new sensor_msgs::LaserScan());
output->header = pcl_conversions::fromPCL(merged_cloud->header);
output->header.frame_id = robot_frame_;
output->angle_min = -M_PI;
output->angle_max = M_PI;
output->angle_increment = this->angle_increment_;
output->time_increment = this->time_increment_;
output->scan_time = this->scan_time_;
output->range_min = this->range_min_;
output->range_max = this->range_max_;
uint32_t ranges_size = std::ceil((output->angle_max - output->angle_min) / output->angle_increment);
output->ranges.assign(ranges_size, output->range_max + 1.0);
for(int i=0; i<points.cols(); i++)
{
const float &x = points(0,i);
const float &y = points(1,i);
const float &z = points(2,i);
if ( std::isnan(x) || std::isnan(y) || std::isnan(z) )
{
ROS_DEBUG("rejected for nan in point(%f, %f, %f)\n", x, y, z);
continue;
}
double range_sq = y*y+x*x;
double range_min_sq_ = output->range_min * output->range_min;
if (range_sq < range_min_sq_) {
ROS_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range_sq, range_min_sq_, x, y, z);
continue;
}
double angle = atan2(y, x);
if (angle < output->angle_min || angle > output->angle_max)
{
ROS_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, output->angle_min, output->angle_max);
continue;
}
int index = (angle - output->angle_min) / output->angle_increment;
if (output->ranges[index] * output->ranges[index] > range_sq)
output->ranges[index] = sqrt(range_sq);
}
return output;
}//pointcloud_to_laserscan
}; // namespace rl_agent
int main(int argc, char** argv){
ros::init(argc, argv, "laser_scan_merger");
ros::NodeHandle node;
rl_agent::LaserScanMerger ls(node);
while (ros::ok()) {
ros::spin();
}
return 0;
};
================================================
FILE: rl_agent/src/rl_agent/__init__.py
================================================
================================================
FILE: rl_agent/src/rl_agent/common_custom_policies.py
================================================
'''
@name: state_collector.py
@brief: This class provides custom policies used in stable-baselines library
@author: Ronja Gueldenring
@version: 3.5
@date: 2019/04/05
'''
from stable_baselines.common.policies import *
import stable_baselines.common.policies as common
import rospy
import numpy as np
NS="sim1"
######################################
###### CNN1DPolicy_multi_input #######
######################################
def laser_cnn_multi_input(state, **kwargs):
"""
1D Conv Network
:param state: (TensorFlow Tensor) state input placeholder
:param kwargs: (dict) Extra keywords parameters for the convolutional layers of the CNN
:return: (TensorFlow Tensor) The CNN output layer
"""
# scan = tf.squeeze(state[:, : , 0:kwargs['laser_scan_len'] , :], axis=1)
scan = tf.squeeze(state[:, : , 0:kwargs['laser_scan_len'] , :], axis=1)
wps = tf.squeeze(state[:, :, kwargs['laser_scan_len']:, -1], axis=1)
# goal = tf.math.multiply(goal, 6)
kwargs_conv = {}
activ = tf.nn.relu
layer_1 = activ(conv1d(scan, 'c1d_1', n_filters=32, filter_size=5, stride=2, init_scale=np.sqrt(2), **kwargs_conv))
layer_2 = activ(conv1d(layer_1, 'c1d_2', n_filters=64, filter_size=3, stride=2, init_scale=np.sqrt(2), **kwargs_conv))
layer_2 = conv_to_fc(layer_2)
layer_3 = activ(linear(layer_2, 'fc1', n_hidden=256, init_scale=np.sqrt(2)))
temp = tf.concat([layer_3, wps], 1)
layer_4 = activ(linear(temp, 'fc2', n_hidden=128, init_scale=np.sqrt(2)))
return layer_4
class CNN1DPolicy_multi_input(common.FeedForwardPolicy):
"""
This class provides a 1D convolutional network for the Raw Data Representation
"""
def __init__(self, *args, **kwargs):
kwargs["laser_scan_len"] = rospy.get_param("%s/rl_agent/scan_size"%NS, 360)
super(CNN1DPolicy_multi_input, self).__init__(*args, **kwargs, cnn_extractor=laser_cnn_multi_input, feature_extraction="cnn")
##########################
###### CNN1DPolicy #######
##########################
def laser_cnn_stack(state, **kwargs):
"""
1D Conv Network
:param state: (TensorFlow Tensor) Scan input placeholder
:param kwargs: (dict) Extra keywords parameters for the convolutional layers of the CNN
:return: (TensorFlow Tensor) The CNN output layer
"""
scan = state[:, : , 0 , :]
wps = tf.expand_dims(state[:, :, 1, -1], -1)
scan_con_wps = tf.concat([scan, wps], -1)
# goal = tf.math.multiply(goal, 6)
kwargs_conv = {}
activ = tf.nn.relu
layer_1 = activ(conv1d(scan_con_wps, 'c1d_1', n_filters=32, filter_size=5, stride=2, init_scale=np.sqrt(2), **kwargs_conv))
layer_2 = activ(conv1d(layer_1, 'c1d_2', n_filters=64, filter_size=3, stride=2, init_scale=np.sqrt(2), **kwargs_conv))
layer_2 = conv_to_fc(layer_2)
layer_3 = activ(linear(layer_2, 'fc1', n_hidden=256, init_scale=np.sqrt(2)))
# print_goal = tf.print(wps)
# with tf.control_dependencies([print_goal]):
layer_4 = activ(linear(layer_3, 'fc2', n_hidden=128, init_scale=np.sqrt(2)))
return layer_4
class CNN1DPolicy(common.FeedForwardPolicy):
"""
This class provides a 1D convolutional network for the Polar Representation
"""
def __init__(self, *args, **kwargs):
super(CNN1DPolicy, self).__init__(*args, **kwargs, cnn_extractor=laser_cnn_stack, feature_extraction="cnn")
########################################
###### CnnPolicy_multi_input_vel #######
########################################
def nature_cnn_multi_input(state, **kwargs):
"""
CNN from Nature paper. state[0:-1, :, :] is the input image, while state[-1, :, :]
provides additional information, that is concenated later with the output pf layer 3.
It can be additional non-image information.
:param scaled_images: (TensorFlow Tensor) Image input placeholder
:param kwargs: (dict) Extra keywords parameters for the convolutional layers of the CNN
:return: (TensorFlow Tensor) The CNN output layer
"""
img = state[:, 0:-1 , : , :]
vel = state[:, -1, 0:kwargs['multi_input_size'], 0]
vel = tf.math.multiply(vel, kwargs['max_image_value'])
kwargs_conv = {}
activ = tf.nn.relu
layer_1 = activ(conv(img, 'c1', n_filters=32, filter_size=8, stride=4, init_scale=np.sqrt(2), **kwargs_conv))
layer_2 = activ(conv(layer_1, 'c2', n_filters=64, filter_size=4, stride=2, init_scale=np.sqrt(2), **kwargs_conv))
layer_3 = activ(conv(layer_2, 'c3', n_filters=64, filter_size=3, stride=1, init_scale=np.sqrt(2), **kwargs_conv))
layer_3 = conv_to_fc(layer_3)
concat_img_goal = tf.concat([layer_3, vel], 1)
# For printing uncomment: velocity
# print_vel = tf.print(vel)
# with tf.control_dependencies([print_vel]):
layer_4 = activ(linear(concat_img_goal, 'fc1', n_hidden=512, init_scale=np.sqrt(2)))
return layer_4
class CnnPolicy_multi_input_vel(common.FeedForwardPolicy):
"""
This class provides a 2D convolutional network for the X-Image Speed Representation
"""
def __init__(self, *args, **kwargs):
kwargs["multi_input_size"] = 2
kwargs["max_image_value"] = 100
super(CnnPolicy_multi_input_vel, self).__init__(*args, **kwargs, cnn_extractor=nature_cnn_multi_input, feature_extraction="cnn")
#########################################
###### CnnPolicy_multi_input_vel2 #######
#########################################
def cnn_multi_input(state, **kwargs):
"""
CNN from Nature paper. state[0:-1, :, :] is the input image, while state[-1, :, :]
provides additional information, that is concenated later with the output pf layer 3.
It can be additional non-image information.
:param scaled_images: (TensorFlow Tensor) Image input placeholder
:param kwargs: (dict) Extra keywords parameters for the convolutional layers of the CNN
:return: (TensorFlow Tensor) The CNN output layer
"""
img = state[:, 0:-1 , : , :]
vel = state[:, -1, 0:kwargs['multi_input_size'], -1]
vel = tf.math.multiply(vel, kwargs['max_image_value'])
kwargs_conv = {}
activ = tf.nn.relu
layer_1 = activ(conv(img, 'c1', n_filters=64, filter_size=8, stride=4, init_scale=np.sqrt(2), **kwargs_conv))
layer_2 = activ(conv(layer_1, 'c2', n_filters=64, filter_size=4, stride=2, init_scale=np.sqrt(2), **kwargs_conv))
layer_3 = activ(conv(layer_2, 'c3', n_filters=32, filter_size=3, stride=1, init_scale=np.sqrt(2), **kwargs_conv))
layer_4 = activ(conv(layer_3, 'c4', n_filters=32, filter_size=2, stride=1, init_scale=np.sqrt(2), **kwargs_conv))
layer_4 = conv_to_fc(layer_4)
concat_img_goal = tf.concat([layer_4, vel], 1)
# For printing uncomment: velocity
# print_vel = tf.print(vel)
# with tf.control_dependencies([print_vel]):
layer_4 = activ(linear(concat_img_goal, 'fc1', n_hidden=512, init_scale=np.sqrt(2)))
layer_5 = activ(linear(layer_4, 'fc2', n_hidden=216, init_scale=np.sqrt(2)))
return layer_5
class CnnPolicy_multi_input_vel2(common.FeedForwardPolicy):
"""
This class provides a 2D convolutional network for the X-Image Speed Representation
"""
def __init__(self, *args, **kwargs):
kwargs["multi_input_size"] = 2
kwargs["max_image_value"] = 100
super(CnnPolicy_multi_input_vel2, self).__init__(*args, **kwargs, cnn_extractor=cnn_multi_input, feature_extraction="cnn")
# Deprecated
class CnnPolicy_multi_input_vel3(common.FeedForwardPolicy):
"""
CnnPolicy_multi_input_vel3 == CnnPolicy_multi_input_vel2
Is needed because some agents are trained on this policy-name and is still needed for execution mode
"""
def __init__(self, *args, **kwargs):
kwargs["multi_input_size"] = 2
kwargs["max_image_value"] = 100
super(CnnPolicy_multi_input_vel3, self).__init__(*args, **kwargs, cnn_extractor=cnn_multi_input, feature_extraction="cnn")
================================================
FILE: rl_agent/src/rl_agent/env_utils/__init__.py
================================================
================================================
FILE: rl_agent/src/rl_agent/env_utils/debug_ros_env.py
================================================
'''
@name: debug_ros_env.py
@brief: This class provides debugging methods RL-relevant data.
@author: Ronja Gueldenring
@version: 3.5
@date: 2019/04/05
'''
#ros-relevant
import rospy
from sensor_msgs.msg import Image
from std_msgs.msg import Float64
from nav_msgs.msg import OccupancyGrid
from visualization_msgs.msg import Marker
from geometry_msgs.msg import PointStamped
from sensor_msgs.msg import LaserScan
from collections import deque
#python-relevant
import numpy as np
class DebugRosEnv():
"""
This class serves as debugger for RL-relevant data like:
- input state
- rewards
"""
def __init__(self, ns, stack_offset):
self.__ns = ns
self.__stack_offset = stack_offset
print("stack_offset: %d"%self.__stack_offset)
self.__input_images = deque(maxlen=4 * self.__stack_offset)
#Input state
self.__input_img_pub1 = rospy.Publisher('%s/state_image1' % (self.__ns), Image, queue_size=1)
self.__input_img_pub2 = rospy.Publisher('%s/state_image2' % (self.__ns), Image, queue_size=1)
self.__input_img_pub3 = rospy.Publisher('%s/state_image3' % (self.__ns), Image, queue_size=1)
self.__input_img_pub4 = rospy.Publisher('%s/state_image4' % (self.__ns), Image, queue_size=1)
self.__occ_grid_pub = rospy.Publisher('%s/rl_map' % (self.__ns), OccupancyGrid, queue_size=1)
self.__input_scan_pub = rospy.Publisher('%s/state_scan' % (self.__ns), LaserScan, queue_size=1)
# reward info
self.__rew_pub = rospy.Publisher('%s/reward' % (self.__ns), Marker, queue_size=1)
self.__rew_num_pub = rospy.Publisher('%s/reward_num' % (self.__ns), Float64, queue_size=1)
# Waypoint info
self.__wp_pub1 = rospy.Publisher('%s/wp_vis1' % (self.__ns), PointStamped, queue_size=1)
self.__wp_pub2 = rospy.Publisher('%s/wp_vis2' % (self.__ns), PointStamped, queue_size=1)
self.__wp_pub3 = rospy.Publisher('%s/wp_vis3' % (self.__ns), PointStamped, queue_size=1)
self.__wp_pub4 = rospy.Publisher('%s/wp_vis4' % (self.__ns), PointStamped, queue_size=1)
def show_wp(self, data):
"""
Publishing waypoints on the path (maximum 4).
:param data: waypoint message
"""
msg = PointStamped()
msg.header = data.header
msg.point = data.points[0]
self.__wp_pub1.publish(msg)
if(len(data.points) > 1):
msg.point = data.points[1]
self.__wp_pub2.publish(msg)
if (len(data.points) > 2):
msg.point = data.points[2]
self.__wp_pub3.publish(msg)
if (len(data.points) > 3):
msg.point = data.points[3]
self.__wp_pub4.publish(msg)
def show_image_stack(self, data):
"""
Publishing input image stack. Maximal 4 images are displayed in rviz.
:param data: input matrix
"""
self.__input_images.appendleft(data)
self.__input_img_pub1.publish(self.__data_to_image(self.__input_images[0]))
if(len(self.__input_images) > self.__stack_offset):
self.__input_img_pub2.publish(self.__data_to_image(self.__input_images[self.__stack_offset]))
if (len(self.__input_images) > 2*self.__stack_offset):
self.__input_img_pub3.publish(self.__data_to_image(self.__input_images[self.__stack_offset * 2]))
if (len(self.__input_images) > 3*self.__stack_offset):
self.__input_img_pub4.publish(self.__data_to_image(self.__input_images[self.__stack_offset * 3]))
def __data_to_image(self, data):
"""
Transforms input state format to Image msg, displayable in rviz.
:param data: input state
"""
msg = Image()
msg.header.frame_id = "/base_footprint"
msg.height = data.shape[1]
msg.width = data.shape[0]
msg.encoding = "mono8"
msg.data = np.uint8(np.ndarray.flatten(data, order='F'))[::-1].tolist()
return msg
def show_input_image(self, data):
"""
Publishing single input matrix as image.
Can be displayed in rviz.
:param data: input matrix
"""
msg = self.__data_to_image(data)
self.__input_img_pub1.publish(msg)
def show_input_scan(self, scan):
"""
Publishing input scan for displaying in rviz
:param data: input matrix
"""
self.__input_scan_pub.publish(scan)
def show_input_occ_grid(self, state_image):
"""
Publishing occupancy grid.
:param state_image: occupancy grid
"""
self.__occ_grid_pub.publish(state_image)
def show_reward(self, reward):
"""
Publishing reward value as marker.
:param reward
"""
# Publish reward as Marker
msg = Marker()
msg.header.frame_id = "/base_footprint"
msg.ns = ""
msg.id = 0
msg.type = msg.TEXT_VIEW_FACING
msg.action = msg.ADD
msg.pose.position.x = 0.0
msg.pose.position.y = 1.0
msg.pose.position.z = 0.0
msg.pose.orientation.x = 0.0
msg.pose.orientation.y = 0.0
msg.pose.orientation.z = 0.0
msg.pose.orientation.w = 1.0
msg.text = "%f"%reward
msg.scale.x = 10.0
msg.scale.y = 10.0
msg.scale.z = 1.0
msg.color.r = 0.3
msg.color.g = 0.4
msg.color.b = 1.0
msg.color.a = 1.0
self.__rew_pub.publish(msg)
# Publish reward as number
msg = Float64()
msg.data = reward
self. __rew_num_pub.publish(msg)
================================================
FILE: rl_agent/src/rl_agent/env_utils/reward_container.py
================================================
'''
@name: ros_env.py
@brief: This class provides different reward functions and parametrizes them.
@author: Ronja Gueldenring
@version: 3.5
@date: 2019/04/05
'''
import numpy as np
import math
import rospy
class RewardContainer():
"""
This class provides different reward functions and parametrizes them.
"""
def __init__(self, ns, robot_radius, goal_radius, max_trans_vel):
self.old_wps = []
self.old_closest_wp = 0
self.__update_rate = 1/rospy.get_param("%s/rl_agent/update_frequency"%ns)
self.__robot_radius = robot_radius
self.__goal_radius = goal_radius
self.__still_time = 0.0
self.__max_trans_vel = max_trans_vel
def reset(self, wps):
"""
If episode is reset, old waypoints need to be resetted here.
"""
num_wp = len(wps)
self.old_wps = np.zeros(num_wp)
for i in range(num_wp):
self.old_wps[i] = self.mean_sqare_dist_(wps[i].x, wps[i].y)
self.old_closest_wp = 0
def rew_func_1(self, scan, wps, transformed_goal):
'''
Reward function designed for static training setup
:param scan: laser scan of environment
:param wps: next waypoints on path
:param transformed_goal: final goal in robot frame
:return: reward value
'''
min_scan_dist = np.amin(scan.ranges)
if (min_scan_dist < (self.__robot_radius + 0.5) ):
wp_approached_rew = self.__get_wp_approached(wps, 3.5, 2.5, 1.0)
wp_approached_rew = 0
else:
wp_approached_rew = self.__get_wp_approached(wps, 3.5, 2.5, 1.0)
obstacle_punish = self.__get_obstacle_punish(scan.ranges , 15, self.__robot_radius)
goal_reached_rew = self.__get_goal_reached_rew(transformed_goal, 10)
rew = (wp_approached_rew + obstacle_punish + goal_reached_rew)
rew = self.__check_reward(rew, obstacle_punish, goal_reached_rew, 2.5)
return rew
def rew_func_19(self, static_scan, ped_scan_msg, wps, twist, transformed_goal):
'''
Reward function designed for dynamic training setup
:param static_scan: laser scan containing information about static obstacles.
:param ped_scan_msg: laser scan containing information about dynamic obstacles.
:param wps: next waypoints on path
:param twist: velocity of robot
:param transformed_goal: final goal in robot frame
:return: reward value
'''
standing_still_punish = 0
if (abs(twist.twist.linear.x) < 0.001 and abs(twist.twist.angular.z) < 0.001):
standing_still_punish = -0.001
self.__still_time += 0.1
else:
self.__still_time = 0.0
if (abs(twist.twist.linear.x) < 0.001 and abs(twist.twist.angular.z) > 0.001):
standing_still_punish = -0.01
wp_approached_rew = self.__get_wp_approached(wps, 5.5, 4.5, 0.0)
# Did the agent bump into an obstacle?
obstacle_punish_static = self.__get_obstacle_punish(static_scan.ranges, 7, self.__robot_radius)
obstacle_punish_ped = 0
if (self.__still_time < 0.8):
obstacle_punish_ped = self.__get_obstacle_punish(ped_scan_msg.ranges, 7, 0.85)
obstacle_punish = min(obstacle_punish_ped, obstacle_punish_static)
# Did the agent reached the goal?
goal_reached_rew = self.__get_goal_reached_rew(transformed_goal, 10)
rew = (wp_approached_rew + obstacle_punish + goal_reached_rew + standing_still_punish)
if (rew < -2.5):
test = "debug"
rew = self.__check_reward(rew, obstacle_punish, goal_reached_rew, 2.5)
return rew
def rew_func_2_1(self, static_scan, ped_scan_msg, wps, twist, transformed_goal):
'''
Reward function designed for dynamic training setup
:param static_scan: laser scan containing information about static obstacles.
:param ped_scan_msg: laser scan containing information about dynamic obstacles.
:param wps: next waypoints on path
:param twist: velocity of robot
:param transformed_goal: final goal in robot frame
:return: reward value
'''
standing_still_punish = 0
if (abs(twist.twist.linear.x) < 0.001 and abs(twist.twist.angular.z) < 0.001):
standing_still_punish = -0.001
self.__still_time += 0.1
else:
self.__still_time = 0.0
if (abs(twist.twist.linear.x) < 0.001 and abs(twist.twist.angular.z) > 0.001):
standing_still_punish = -0.01
min_scan_dist = np.amin(static_scan.ranges)
if (min_scan_dist < (self.__robot_radius + 0.2)):
wp_approached_rew = self.__get_wp_approached(wps, 5.5, 4.5, 0.8)
wp_approached_rew = 0
else:
wp_approached_rew = self.__get_wp_approached(wps, 5.5, 4.5, 0.8)
# Did the agent bump into an obstacle?
obstacle_punish_static = self.__get_obstacle_punish(static_scan.ranges, 15, self.__robot_radius)
obstacle_punish_ped = 0
if (self.__still_time < 0.8):
obstacle_punish_ped = self.__get_obstacle_punish(ped_scan_msg.ranges, 7, 0.85)
obstacle_punish = min(obstacle_punish_ped, obstacle_punish_static)
# Did the agent reached the goal?
goal_reached_rew = self.__get_goal_reached_rew(transformed_goal, 10)
rew = (wp_approached_rew + obstacle_punish + goal_reached_rew + standing_still_punish)
if (rew < -2.5):
test = "debug"
rew = self.__check_reward(rew, obstacle_punish, goal_reached_rew, 2.5)
return rew
def rew_func_2_2(self, static_scan, ped_scan_msg, wps, twist, transformed_goal):
'''
Reward function designed for dynamic training setup
:param static_scan: laser scan containing information about static obstacles.
:param ped_scan_msg: laser scan containing information about dynamic obstacles.
:param wps: next waypoints on path
:param twist: velocity of robot
:param transformed_goal: final goal in robot frame
:return: reward value
'''
standing_still_punish = 0
if (abs(twist.twist.linear.x) < 0.1):
self.__still_time += 0.1
else:
self.__still_time = 0.0
min_scan_dist = np.amin(static_scan.ranges)
if (min_scan_dist < (self.__robot_radius + 0.2)):
wp_approached_rew = self.__get_wp_approached(wps, 5.5, 4.5, 0.8)
wp_approached_rew = 0
else:
wp_approached_rew = self.__get_wp_approached(wps, 5.5, 4.5, 0.8)
# Did the agent bump into an obstacle?
obstacle_punish_static = self.__get_obstacle_punish(static_scan.ranges, 15, self.__robot_radius)
obstacle_punish_ped = 0
if (self.__still_time < 0.8):
obstacle_punish_ped = self.__get_obstacle_punish(ped_scan_msg.ranges, 7, 0.85)
obstacle_punish = min(obstacle_punish_ped, obstacle_punish_static)
# Did the agent reached the goal?
goal_reached_rew = self.__get_goal_reached_rew(transformed_goal, 10)
rew = (wp_approached_rew + obstacle_punish + goal_reached_rew + standing_still_punish)
if (rew < -2.5):
test = "debug"
rew = self.__check_reward(rew, obstacle_punish, goal_reached_rew, 2.5)
return rew
def __ped_in_box_punish(self, ped_scan_msg, box_width, box_height_pos, box_height_neg, k):
"""
Returns a negative reward k if pedestrians are in the defined box
[box_width x (box_height_pos + box_height_neg)}
:param ped_scan_msg laserscan with pedestrian information only
:param box_width width if box
:param box_height_pos height of box in forward direction of the robot
:param box_height_neg height of box in backward direction of the robot
:param k reward constant
:return: obs, reward, done, info
"""
if self.is_ped_in_box(ped_scan_msg, box_width, box_height_pos, box_height_neg):
ped_punish = -k
else:
ped_punish = 0
return ped_punish
def is_ped_in_box(self, ped_scan_msg, box_width, box_height_pos, box_height_neg):
'''
Checks if pedestrian is in defined box [box_width x (box_height_pos + box_height_neg)]
:param ped_scan_msg laserscan with pedestrian information only
:param box_width width if box
:param box_height_pos height of box in forward direction of the robot
:param box_height_neg height of box in backward direction of the robot
:return: True if pedestrian inside the defined box, else False.
'''
is_ped_in_box = False
box_width = box_width * 2
box_height_pos = box_height_pos
box_height_neg = box_height_neg
angle_min = ped_scan_msg.angle_min
angle_increment = ped_scan_msg.angle_increment
ped_punish = 0.0
for i in range(len(ped_scan_msg.ranges)):
angle = angle_min + i * angle_increment
length = ped_scan_msg.ranges[i]
x = math.cos(angle) * length
y = math.sin(angle) * length
if(x > -box_height_neg and x < box_height_pos and y > -box_width/2 and y < box_width/2):
is_ped_in_box = True
return is_ped_in_box
def __check_reward(self, rew, obstacle_punish, goal_reached_rew, thresh):
"""
Checks if reward makes sense.
:param rew final reward
:param obstacle_punish reward for obstacle collision
:param goal_reached_rew reward for reaching goal
:param thresh [-thresh, +thresh[ the final reward should be inbetween, if no obstacle_punish==0 and goal_reached_rew==0
:return: returns reward value if it makes sense, else 0
"""
if ((rew > thresh and goal_reached_rew == 0.0) or (rew < -thresh and obstacle_punish == 0.0)):
rospy.loginfo("Wrong reward: %f" % rew)
return 0
else:
return rew
def __get_turn_punish(self, w, fac, thresh):
"""
Returns negative reward if the robot turns.
:param w roatational speed of the robot
:param fac weight of reward punish for turning
:param thresh rotational speed > thresh will be punished
:return: returns reward for turning
"""
if abs(w) > thresh:
return abs(w) * -fac
else:
return 0.0
def __get_obstacle_punish(self, scan, k, radius):
"""
Returns negative reward if the robot collides with obstacles.
:param scan containing obstacles that should be considered
:param k reward constant
:return: returns reward colliding with obstacles
"""
min_scan_dist = np.amin(scan)
if min_scan_dist < radius:
return -k
else:
return 0.0
def __get_obstacle_punish_section(self, scan, k, perc):
"""
Returns negative reward if the robot collides with obstacles in section around the robot.
:param scan containing obstacles that should be considered
:param k reward constant
:param perc percentage of the area around the robot, that should be considered.
Example: 0.4 will consider the front view of 144 degree.
:return: returns reward colliding with obstacles in defined section
"""
min_scan_dist_index = np.argmin(scan)
scan_length = len(scan)
lower_bound = int(scan_length/2 - perc*scan_length/2)
upper_bound = int(scan_length/2 + perc*scan_length/2)
if min_scan_dist_index > lower_bound and min_scan_dist_index < upper_bound and scan[min_scan_dist_index] < self.__robot_radius:
return -k
else:
return 0.0
def __get_goal_reached_rew(self, transformed_goal, k):
"""
Returns positive reward if the robot reaches the goal.
:param transformed_goal goal position in robot frame
:param k reward constant
:return: returns reward colliding with obstacles
"""
dist_to_goal = self.mean_sqare_dist_(transformed_goal.position.x, transformed_goal.position.y)
if dist_to_goal < self.__goal_radius:
return k
else:
return 0.0
def __get_wp_approached(self, wps, punish_fac, rew_fac, k):
"""
Returns positive reward if the robot approaches the next waypoint, else negative reward.
:param wps next waypoints
:param punish_fac weight for punishing disapproaching the closest waypoint
:param rew_fac weight for rewarding approaching the closest waypoint
:param k reward constant for reaching a waypoint
:return: returns reward for approaching waypoints.
"""
num_wp = len(wps.points)
dist_to_waypoint = np.zeros(num_wp)
for i in range(num_wp):
dist_to_waypoint[i] = self.mean_sqare_dist_(wps.points[i].x, wps.points[i].y)
if wps.is_new.data:
wp_approached = k
else:
diff = (self.old_wps[0] - dist_to_waypoint[0])
if (abs(diff) > self.__max_trans_vel*self.__update_rate*4):
diff = 0
if (diff < 0):
wp_approached = punish_fac * diff
else:
wp_approached = rew_fac * diff
self.old_wps = dist_to_waypoint
return wp_approached
def mean_sqare_dist_(self, x, y):
"""
Computing mean square distance of x and y
:param x, y
:return: sqrt(x^2 + y^2)
"""
return math.sqrt(math.pow(x, 2) + math.pow(y, 2))
================================================
FILE: rl_agent/src/rl_agent/env_utils/state_collector.py
================================================
'''
@name: state_collector.py
@brief: This class collects most recent relevant state data of the environment of the RL-agent.
@author: Ronja Gueldenring
@version: 3.5
@date: 2019/04/05
'''
# python relevant
import numpy as np
import copy
# ros-relevant
import rospy
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import OccupancyGrid
from rl_msgs.msg import Waypoint
from rl_msgs.srv import StateImageGenerationSrv
from geometry_msgs.msg import TwistStamped, PoseStamped, Pose
from std_srvs.srv import SetBool
from rl_msgs.srv import MergeScans
import time
class StateCollector():
"""
This class collects most recent relevant state data of the environment of the RL-agent.
"""
def __init__(self, ns, train_mode):
# Class variables
self.__mode = train_mode # Mode of RL-agent (Training or Executrion ?)
self.__ns = ns # namespace of simulation environment
self.__is__new = False # True, if waypoint reached
self.__static_scan = LaserScan() # Laserscan only contains static objects
self.__ped_scan = LaserScan() # Laserscan only contains pedestrians
self.__f_scan = LaserScan()
self.__f_scan.header.frame_id = "base_footprint"
self.__b_scan = LaserScan()
self.__b_scan.header.frame_id = "base_footprint"
self.__img = OccupancyGrid() # input image
self.__wps= Waypoint() # most recent Waypoints
self.__twist = TwistStamped() # most recent velocity
self.__goal = Pose() # most recent goal position in robot frame
self.__state_mode = 2 # 0, if image as input state representation
# 1, if stacked image representation in same frame
# 2, if scan as input state representation
# Subscriber
self.wp_sub_ = rospy.Subscriber("%s/wp" % (self.__ns), Waypoint, self.__wp_callback, queue_size=1)
if ["train", "eval"].__contains__(self.__mode):
# Info only avaible during evaluation and training
self.wp_sub_reached_ = rospy.Subscriber("%s/wp_reached" % (self.__ns), Waypoint, self.__wp_reached_callback, queue_size=1)
self.static_scan_sub_ = rospy.Subscriber("%s/static_laser" % (self.__ns), LaserScan, self.__static_scan_callback,
queue_size=1)
self.ped_scan_sub_ = rospy.Subscriber("%s/ped_laser" % (self.__ns), LaserScan, self.__ped_scan_callback,
queue_size=1)
self.twist_sub_ = rospy.Subscriber("%s/twist" % (self.__ns), TwistStamped, self.__twist_callback, queue_size=1)
self.goal_sub_ = rospy.Subscriber("%s/rl_agent/robot_to_goal" % (self.__ns), PoseStamped, self.__goal_callback, queue_size=1)
else:
self.static_scan_sub_ = rospy.Subscriber("%s/b_scan" % (self.__ns), LaserScan,
self.__b_scan_callback,
queue_size=1)
self.static_scan_sub_ = rospy.Subscriber("%s/f_scan" % (self.__ns), LaserScan,
self.__f_scan_callback,
queue_size=1)
# Service
self.__img_srv = rospy.ServiceProxy('%s/image_generator/get_image' % (self.__ns), StateImageGenerationSrv)
self.__sim_in_step = rospy.ServiceProxy('%s/is_in_step' % (self.__ns), SetBool)
self.__merge_scans = rospy.ServiceProxy('%s/merge_scans' % (self.__ns), MergeScans)
def get_state(self):
"""
Provides the most recent state with laserscan data, input image, waypoints, robots velocity, goal position
:returns laser scan of static objects, ([] in execution mode)
laserscan of pedestrians, ([] in execution mode)
state image, ([] if raw data state mode)
next waypoints,
twist of the robot,
goal position in robot frame
"""
if ["train", "eval"].__contains__(self.__mode):
# Fully synchronized --> slows down simulation speed!
# resp = self.__sim_in_step(True)
# while(resp.success):
# time.sleep(0.0001)
# resp = self.__sim_in_step(True)
# while self.__ped_scan.header.stamp.to_sec() + 0.11 < float(resp.message):
# time.sleep(0.005)
# while self.__static_scan.header.stamp.to_sec() + 0.11 < float(resp.message):
# time.sleep(0.005)
# start = time.time()
static_scan_msg = self.__remove_nans_from_scan(self.__static_scan)
ped_scan_msg = self.__remove_nans_from_scan(self.__ped_scan)
# print("__remove_nans_from_scan: %f" % (time.time() - start))
# start = time.time()
merged_scan = LaserScan()
merged_scan.header.frame_id = "base_footprint"
merged_scan.header.stamp = static_scan_msg.header.stamp
merged_scan.ranges = np.minimum(static_scan_msg.ranges, ped_scan_msg.ranges)
merged_scan.range_max = static_scan_msg.range_max
merged_scan.range_min = static_scan_msg.range_min
merged_scan.angle_increment = static_scan_msg.angle_increment
merged_scan.angle_min = static_scan_msg.angle_min
merged_scan.angle_max = static_scan_msg.angle_max
# print("merge_scan: %f" % (time.time() - start))
# start = time.time()
wp_cp = copy.deepcopy(self.__wps)
self.__wps.is_new.data = False
# print("deep_copy: %f" % (time.time() - start))
start = time.time()
if (self.__state_mode == 0):
# ToDo: Service call takes very long. Find more efficient solution!
resp = self.__img_srv(merged_scan, wp_cp)
self.__img = resp.img
# print("img service call: %f" % (time.time() - start))
else:
self.__img = []
return static_scan_msg, ped_scan_msg, merged_scan, self.__img, wp_cp, self.__twist, self.__goal
else:
scans = []
scans.append(self.__f_scan)
scans.append(self.__b_scan)
resp = self.__merge_scans(scans)
merged_scan = resp.merged_scan
wp_cp = copy.deepcopy(self.__wps)
self.__wps.is_new.data = False
if (self.__state_mode == 0):
resp = self.__img_srv(merged_scan, wp_cp)
self.__img = resp.img
else:
self.__img = []
return [], [], merged_scan, self.__img, wp_cp, self.__twist, self.__goal
def get_static_scan(self):
"""
Provides the most recent laser scan of static objects
:return laser scan
"""
scan_msg = self.__remove_nans_from_scan(self.__static_scan)
return scan_msg
def set_state_mode(self, state_mode):
self.__state_mode = state_mode
def __remove_nans_from_scan(self, scan_msg):
"""
Replaces nan-values with maximum distance of scanner.
:param scan_msg scan message, where nan-values need to be removed.
"""
scan = np.array(scan_msg.ranges)
scan[np.isnan(scan)] = scan_msg.range_max
scan_msg.ranges = scan
return scan_msg
def __static_scan_callback(self, data):
self.__static_scan = data
def __ped_scan_callback(self, data):
self.__ped_scan = data
def __f_scan_callback(self, data):
self.__f_scan = data
def __b_scan_callback(self, data):
self.__b_scan = data
def __wp_callback(self, data):
if not self.__wps.is_new.data:
self.__wps = data
def __twist_callback(self, data):
self.__twist = data
def __goal_callback(self, data):
self.__goal = data.pose
def __is_wp_reached_callback(self, data):
self.__is__new = data.data
def __wp_reached_callback(self, data):
self.__wps = data
================================================
FILE: rl_agent/src/rl_agent/env_utils/task_generator.py
================================================
'''
@name: task_generator.py
@brief: This class generates random tasks for training.
@author: Ronja Güldenring
@version: 3.5
@date: 2019/04/05
'''
# python-relevant
import time
import math
import random
from pyquaternion import Quaternion
import numpy as np
from collections import deque
import pickle
#ros-relevant
import rospy
import rospkg
from nav_msgs.msg import OccupancyGrid, Path
from flatland_msgs.srv import MoveModel, DeleteModel, SpawnModel, Step, RespawnModels
from flatland_msgs.msg import Model
from actionlib_msgs.msg import GoalStatusArray
from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped, Pose2D
from std_msgs.msg import Float64, Bool
from geometry_msgs.msg import Twist, Point
from pedsim_srvs.srv import SpawnPeds
from pedsim_msgs.msg import Ped
from std_srvs.srv import SetBool, Empty
class TaskGenerator():
"""
This class generates task for training. The following task generation is implemented:
- generate random paths with static objects on it.
- generate random paths with pedestrians on it.
- generate random paths with pedestrians and static objects on it.
- load predefined path (--> evaluation).
"""
def __init__(self, ns, state_collector, robot_radius):
# Class variables
self.NS = ns # namespace
self.ROBOT_RADIUS = robot_radius # radius of the robot
self.__update_rate = \
1/rospy.get_param("%s/rl_agent/update_frequency"%ns)
self.__state_collector = state_collector # Collects state information
self.__move_base_status_id = "" # recent id of move_base status
self.__map = OccupancyGrid() # global map
self.__path = Path() # global plan
self.__static_objects = [] # static objects that has been spawned in the most recent task
self.__ped_type = rospy.get_param("%s/rl_agent/ped_type"%ns)
# 0: Pedestrians don't avoid robot
# 10: Pedestrians always avoid robot
# 11: Pedestrians avoid robot if it stands still and after reaction time.
self.__peds = [] # pedestrians that has been spawned
self.__old_path_stamp = 0.0 # timestamp of last global plan
self.init = True
self.__flatland_path = rospkg.RosPack().get_path('flatland_setup')
self.__static_object_types = {"name": ["cylinder.model.yaml", # different object types
"palett.model.yaml",
"wagon.model.yaml"], "index": [0, 0, 0]}
self.__ped_file = "person_two_legged.model.yaml"
# self.__ped_file = "person_single_circle.model.yaml"
# Services
self.__sim_step = rospy.ServiceProxy('%s/step' % self.NS, Step)
self.__sim_pause = rospy.ServiceProxy('%s/pause' % self.NS, Empty)
self.__sim_resume = rospy.ServiceProxy('%s/resume' % self.NS, Empty)
self.__move_robot_to = rospy.ServiceProxy('%s/move_model' % self.NS, MoveModel)
self.__delete_model = rospy.ServiceProxy('%s/delete_model' % self.NS, DeleteModel)
self.__spawn_model = rospy.ServiceProxy('%s/spawn_model' % self.NS, SpawnModel)
self.__respawn_models = rospy.ServiceProxy('%s/respawn_models' % self.NS, RespawnModels)
self.__spawn_ped_srv = rospy.ServiceProxy('%s/pedsim_simulator/spawn_ped' % self.NS, SpawnPeds)
self.__respawn_peds_srv = rospy.ServiceProxy('%s/pedsim_simulator/respawn_peds' % self.NS, SpawnPeds)
self.__remove_all_peds_srv = rospy.ServiceProxy('%s/pedsim_simulator/remove_all_peds' % self.NS, SetBool)
# Subscriber
self.__goal_status_sub = rospy.Subscriber("%s/move_base/status" % self.NS, GoalStatusArray,
self.__goal_status_callback, queue_size=1)
self.__map_sub = rospy.Subscriber("%s/map" % self.NS, OccupancyGrid, self.__map_callback)
self.__path_sub = rospy.Subscriber("%s/move_base/NavfnROS/plan" % self.NS, Path, self.__path_callback)
# self.__path_sub = rospy.Subscriber("%s/move_base/GlobalPlanner/plan" % self.NS, Path, self.__path_callback)
# Publisher
self.__initialpose_pub = rospy.Publisher('%s/initialpose' % self.NS, PoseWithCovarianceStamped, queue_size=1)
self.__goal_pub_ = rospy.Publisher('%s/move_base_simple/goal' % self.NS, PoseStamped, queue_size=1)
self.__cmd_vel_pub = rospy.Publisher('%s/cmd_vel' % self.NS, Twist, queue_size=1)
self.__done_pub = rospy.Publisher('%s/rl_agent/done' % self.NS, Bool, queue_size=1)
self.__new_task_pub = rospy.Publisher('%s/rl_agent/new_task_started' % self.NS, Bool, queue_size=1)
self.__resume = rospy.ServiceProxy('%s/resume' % (self.NS), Empty)
#Clear world
self.__init_object_remove()
self.__time_to_set_ped_task = 0.0
def set_task(self, task):
"""
Loading predefined task (e.g. during evaluation)
:param task predefined task, that will be loaded
:return True, if task is loaded successfully
"""
is_path_available = False
count = 0
while not is_path_available and count < 10:
self.__spread_done()
self.__stop_robot()
if not task:
return False
# Setting path
self.set_robot_pos(task["start"][0], task["start"][1], task["start"][2])
self.take_sim_step()
self.__wait_for_laser()
time.sleep(0.11)
self.__publish_goal(task["goal"][0], task["goal"][1], task["goal"][2])
is_path_available = self.__is_new_path_available(task["goal"], task["start"])
print("path not available...trying again...")
count+=1
# Spawning obstacles
self.remove_all_static_objects()
self.__remove_all_peds()
if 'static_objects' in task.keys():
for i in range(len(task["static_objects"]["x"])):
self.spawn_object(task["static_objects"]["model_name"][i], i, task["static_objects"]["x"][i], task["static_objects"]["y"][i], task["static_objects"]["theta"][i])
if 'peds' in task.keys():
self.__respawn_peds(task["peds"])
self.__spread_new_task()
return True
def set_path(self):
"""
Generating a random path in environment.
:return d task information containing start, goal and path
"""
self.__spread_done()
d = {}
d["start"] = self.__set_random_robot_pos()
d["goal"] = self.__publish_random_goal_()
self.__is_new_path_available(d["goal"], d["start"])
d["path"] = self.__path
return d
def set_random_static_task(self):
"""
Generating a random path with static obstacles on it.
:return d task information containing start, goal, path and static objects
"""
# start = time.time()
self.__spread_done()
d = {}
# Setting path
self.__stop_robot()
d["start"] = self.__set_random_robot_pos()
d["goal"] = self.__publish_random_goal_()
# Spawning new obstacles
if self.__is_new_path_available(d["goal"], d["start"]):
self.__spawn_random_static_objects()
d["static_objects"] = self.__static_objects
d["path"] = self.__path
self.__spread_new_task()
# print("Task generation took %f secs."%(time.time() - start))
return d
def set_random_ped_task(self):
"""
Generating a random path with pedestrians on it.
:return d task information containing start, goal, path and pedestrians
"""
begin = time.time()
self.__spread_done()
d = {}
self.__stop_robot()
d["start"] = self.__set_random_robot_pos()
d["goal"] = self.__publish_random_goal_()
# if not self.__is_new_path_available(d["goal"], d["start"]):
# d["goal"] = self.__publish_random_goal_()
#
while not self.__is_new_path_available(d["goal"], d["start"]):
self.__spread_done()
time.sleep(0.1)
self.__spread_done()
time.sleep(0.1)
self.__stop_robot()
d = {}
d["start"] = self.__set_random_robot_pos()
# Finding valid position on map in small radius
valid = False
count = 0
while not valid:
x = d["start"][0] + random.uniform(3, math.floor(count / 10) + 5) * random.choice([-1, 1])
y = d["start"][1] + random.uniform(3, math.floor(count / 10) + 5) * random.choice([-1, 1])
valid = self.__is_pos_valid(x, y, self.__map)
count += 1
self.__publish_goal(x, y, 0)
d["goal"] = [x, y, 0]
self.__spawn_random_peds_on_path()
d["peds"] = self.__peds
d["path"] = self.__path
self.__spread_new_task()
self.__time_to_set_ped_task += time.time()-begin
# if(self.__time_to_set_ped_task%50 < 0.5):
# print("Time spend on setting random path in %s: %f"%(self.NS, self.__time_to_set_ped_task))
return d
def set_random_short_ped_task(self):
"""
Generating a random short path (> 3m and < 5m) with pedestrians on it.
We expect to increase the probability of a robot meeting a pedestrian.
:return d task information containing start, goal, path and pedestrians
"""
self.__spread_done()
self.__stop_robot()
d = {}
d["start"] = self.__set_random_robot_pos()
#Finding valid position on map in small radius
valid = False
count = 0
while not valid:
x = d["start"][0] + random.uniform(3, math.floor(count/10) + 5)*random.choice([-1, 1])
y = d["start"][1] + random.uniform(3, math.floor(count/10) + 5)*random.choice([-1, 1])
valid = self.__is_pos_valid(x, y, self.__map)
count+=1
self.__publish_goal(x, y, 0)
d["goal"] = [x, y, 0]
while not self.__is_new_path_available(d["goal"], d["start"]):
self.__spread_done()
self.__stop_robot()
d = {}
d["start"] = self.__set_random_robot_pos()
# Finding valid position on map in small radius
valid = False
count = 0
while not valid:
x = d["start"][0] + random.uniform(3, math.floor(count / 10) + 5) * random.choice([-1, 1])
y = d["start"][1] + random.uniform(3, math.floor(count / 10) + 5) * random.choice([-1, 1])
valid = self.__is_pos_valid(x, y, self.__map)
count += 1
self.__publish_goal(x, y, 0)
d["goal"] = [x, y, 0]
self.__spawn_random_peds_on_path()
d["peds"] = self.__peds
d["path"] = self.__path
self.__spread_new_task()
return d
def set_random_static_ped_task(self):
"""
Generating a random path with pedestrians and static obstacles on it.
:return d task information containing start, goal, path, pedestrians and static objects
"""
# Setting path
self.__spread_done()
d = {}
self.__stop_robot()
d["start"] = self.__set_random_robot_pos()
d["goal"] = self.__publish_random_goal_()
# Spawning new obstacles
if self.__is_new_path_available(d["goal"], d["start"]):
self.__spawn_random_static_objects()
self.__spawn_random_peds_on_path()
d["peds"] = self.__peds
d["path"] = self.__path
d["static_objects"] = self.__static_objects
self.__spread_new_task()
return d
def take_sim_step(self):
"""
Executing one simulation step of 0.1 sec
"""
msg = Float64()
msg.data = self.__update_rate
rospy.wait_for_service('%s/step' % self.NS)
self.__sim_step(msg)
return
def __set_random_robot_pos(self):
"""
Moving robot to random position (x, y, theta) in simulation
:return: robot position [x, y, theta]
"""
success = False
while not success:
x, y, theta = self.__get_random_pos_on_map(self.__map)
self.set_robot_pos(x, y, theta)
scan = self.__wait_for_laser()
min_obstacle_dist = np.amin(scan)
if min_obstacle_dist > (self.ROBOT_RADIUS + 0.1):
success = True
return x, y, theta
def set_robot_pos(self, x, y, theta):
"""
Move robot to position (x, y, theta) in simulation
:param x x-position of the robot
:param y y-position of the robot
:param theta theta-position of the robot
"""
pose = Pose2D()
pose.x = x
pose.y = y
pose.theta = theta
rospy.wait_for_service('%s/move_model' % self.NS)
self.__move_robot_to('robot_1', pose)
self.take_sim_step()
self.__pub_initial_position(x, y, theta)
def __wait_for_laser(self):
"""
Waiting for most recent laserscan to get available
:return most recent laser scan data
"""
ts = self.__state_collector.get_static_scan().header.stamp
self.take_sim_step()
scan = self.__state_collector.get_static_scan()
begin = time.time()
while len(scan.ranges) == 0 or scan.header.stamp <= ts:
rospy.logdebug("Waiting for laser scan to get available.")
if(time.time() - begin > 1):
self.take_sim_step()
time.sleep(0.00001)
scan = self.__state_collector.get_static_scan()
return scan.ranges
def __pub_initial_position(self, x, y, theta):
"""
Publishing new initial position (x, y, theta) --> for localization
:param x x-position of the robot
:param y y-position of the robot
:param theta theta-position of the robot
"""
initpose = PoseWithCovarianceStamped()
initpose.header.stamp = rospy.get_rostime()
initpose.header.frame_id = "map"
initpose.pose.pose.position.x = x
initpose.pose.pose.position.y = y
quaternion = self.__yaw_to_quat(theta)
initpose.pose.pose.orientation.w = quaternion[0]
initpose.pose.pose.orientation.x = quaternion[1]
initpose.pose.pose.orientation.y = quaternion[2]
initpose.pose.pose.orientation.z = quaternion[3]
self.__initialpose_pub.publish(initpose)
return
def __publish_random_goal_(self):
"""
Publishing new random goal [x, y, theta] for global planner
:return: goal position [x, y, theta]
"""
x, y, theta = self.__get_random_pos_on_map(self.__map)
self.__publish_goal(x, y, theta)
return x, y, theta
def __publish_goal(self, x, y, theta):
"""
Publishing goal (x, y, theta)
:param x x-position of the goal
:param y y-position of the goal
:param theta theta-position of the goal
"""
self.__old_path_stamp = self.__path.header.stamp
goal = PoseStamped()
goal.header.stamp = rospy.get_rostime()
goal.header.frame_id = "map"
goal.pose.position.x = x
goal.pose.position.y = y
quaternion = self.__yaw_to_quat(theta)
goal.pose.orientation.w = quaternion[0]
goal.pose.orientation.x = quaternion[1]
goal.pose.orientation.y = quaternion[2]
goal.pose.orientation.z = quaternion[3]
self.__goal_pub_.publish(goal)
return
def __get_random_pos_on_map(self, map):
"""
Find a valid (free) random position (x, y, theta) on the map
:param map
:return: x, y, theta
"""
map_width = map.info.width * map.info.resolution + map.info.origin.position.x
map_height = map.info.height * map.info.resolution + map.info.origin.position.y
x = random.uniform(0.0 , map_width)
y = random.uniform(0.0, map_height)
while not self.__is_pos_valid(x, y, map):
x = random.uniform(0.0, map_width)
y = random.uniform(0.0, map_height)
theta = random.uniform(-math.pi, math.pi)
return x, y, theta
def __is_pos_valid(self, x, y, map):
"""
Checks if position (x,y) is a valid position on the map.
:param x x-position
:param y y-position
:param map
:return: True if position is valid
"""
cell_radius = int((self.ROBOT_RADIUS + 0.1)/map.info.resolution)
y_index = int((y-map.info.origin.position.y)/map.info.resolution)
x_index = int((x-map.info.origin.position.x)/map.info.resolution)
for i in range(x_index-cell_radius, x_index+cell_radius, 1):
for j in range(y_index-cell_radius, y_index+cell_radius, 1):
index = j * map.info.width + i
if index >= len(map.data):
return False
try:
val = map.data[index]
except IndexError:
print("IndexError: index: %d, map_length: %d"%(index, len(map.data)))
return False
if val != 0:
return False
return True
def __spread_done(self):
"""
Pulishing True on done-pub
"""
self.__done_pub.publish(Bool(True))
def __spread_new_task(self):
"""
Pulishing True if new task is set. Ready to be solved
"""
self.__new_task_pub.publish(Bool(True))
def __stop_robot(self):
"""
Stops robot.
"""
# self.__agent_action_pub.publish(Twist())
self.__cmd_vel_pub.publish(Twist())
return
def __spawn_random_static_objects(self):
"""
Spawns a random number of static objects randomly on the path.
"""
max_num_obstacles = int(len(self.__path.poses) / 150)
self.__static_object_types["index"] = [0, 0, 0]
models = []
if max_num_obstacles == 0:
num_static_obstacles = 0
else:
num_static_obstacles = random.randint(1, max_num_obstacles)
for i in range(num_static_obstacles):
model_type = random.randint(0, len(self.__static_object_types["name"])-1)
model_name = self.__static_object_types["name"][model_type]
[x, y] = self.__generate_rand_pos_on_path(self.__path.poses, 100, 1.0)
theta = random.uniform(-math.pi, math.pi)
model = Model()
model.yaml_path = "%s/objects/%s" % (self.__flatland_path, model_name)
model.name = "%s_%d"%(model_name.split('.')[0], self.__static_object_types["index"][model_type])
model.ns = "stat_obj_%d" % i
model.pose = Pose2D()
model.pose.x = x
model.pose.y = y
model.pose.theta = theta
models.append(model)
self.__static_object_types["index"][model_type] +=1
# self.spawn_object(model_name, i, x, y, theta)
self.respawn_static_objects(models)
return
def respawn_static_objects(self, models):
"""
Respawning a new scene of static objects. Objects from previous tasks are reused
and simply removed to the appropriate object position.
More efficient, because several models are spawned with one service call.
:param models list of models
"""
old_model_names = []
for old_model in self.__static_objects:
old_model_names.append(old_model.name)
rospy.wait_for_service('%s/respawn_models' % self.NS)
try:
self.__respawn_models.call(old_model_names, models)
except (TypeError):
print('Spawn object: TypeError.')
return
except (rospy.ServiceException):
print('Spawn object: rospy.ServiceException. Closing serivce')
try:
self.__spawn_model.close()
except AttributeError:
print('Spawn object close(): AttributeError.')
return
return
except AttributeError:
print('Spawn object: AttributeError.')
return
self.__static_objects = models
def spawn_object(self, model_name, index, x, y, theta):
"""
Spawns one static object.
:param model_name object type
:param x x-position of the object
:param y y-position of the object
:param theta orientation of the object
"""
srv = SpawnModel()
srv.yaml_path = "%s/objects/%s" % (self.__flatland_path, model_name)
srv.name = "stat_obj_%d" % index
srv.ns = "stat_obj_%d" % index
temp = Pose2D()
temp.x = x
temp.y = y
temp.theta = theta
srv.pose = temp
rospy.wait_for_service('%s/spawn_model' % self.NS)
try:
self.__spawn_model.call(srv.yaml_path, srv.name, srv.ns, srv.pose)
except (TypeError):
print('Spawn object: TypeError.')
return
except (rospy.ServiceException):
print('Spawn object: rospy.ServiceException. Closing serivce')
try:
self.__spawn_model.close()
except AttributeError:
print('Spawn object close(): AttributeError.')
return
return
except AttributeError:
print('Spawn object: AttributeError.')
return
stat_obj = Model()
stat_obj.yaml_path = srv.yaml_path
stat_obj.name = srv.name
stat_obj.ns = srv.ns
stat_obj.pose = srv.pose
self.__static_objects.append(stat_obj)
return
def remove_all_static_objects(self):
"""
Removes all static objects, that has been spawned so far
"""
for i in self.__static_objects:
srv = DeleteModel()
srv.name = i.name
rospy.wait_for_service('%s/delete_model' % self.NS)
ret = self.__delete_model.call(srv.name)
self.__static_objects = []
def __init_object_remove(self):
"""
Removes all objects that might be left overs from previous training sessions.
Is supposed to be called in the constructor.
"""
# This is necessary in case old objects are still present in flatland
if len(self.__static_objects) == 0:
for type in self.__static_object_types["name"]:
for i in range(5):
srv = DeleteModel()
srv.name = "%s_%d"%(type.split('.')[0], i)
rospy.wait_for_service('%s/delete_model' % self.NS)
ret = self.__delete_model.call(srv.name)
if not ret.success:
break
ret.success = True
person_num = 1
while ret.success:
srv = DeleteModel()
srv.name = "person_%d" % (person_num)
rospy.wait_for_service('%s/delete_model' % self.NS)
ret = self.__delete_model.call(srv.name)
person_num += 1
self.__remove_all_peds()
self.__static_objects = []
def __remove_all_peds(self):
"""
Removes all pedestrians, that has been spawned so far
"""
srv = SetBool()
srv.data = True
rospy.wait_for_service('%s/pedsim_simulator/remove_all_peds' % self.NS)
self.__remove_all_peds_srv.call(srv.data)
self.__peds = []
return
def spawn_random_peds_in_world(self, n):
"""
Spawning n random pedestrians in the whole world.
:param n number of pedestrians that will be spawned.
"""
ped_array = []
for i in range(1, n):
waypoints = np.array([], dtype=np.int64).reshape(0, 3)
[x, y, theta] = self.__get_random_pos_on_map(self.__map)
waypoints = np.vstack([waypoints, [x, y, 0.3]])
if random.uniform(0.0, 1.0) < 0.8:
for j in range(4):
dist = 0
while dist < 4:
[x2, y2, theta2] = self.__get_random_pos_on_map(self.__map)
dist = self.__mean_sqare_dist_((waypoints[-1,0] - x2), (waypoints[-1,1] - y2))
waypoints = np.vstack([waypoints, [x2, y2, 0.3]])
ped_array.append(i, [x, y, 0.0], waypoints)
self.__respawn_peds(ped_array)
def __spawn_random_peds_on_path(self):
"""
Spawns a random number of pedestrians randomly near the path.
Pedestrians either cross the path, walk along the path or stand around.
"""
if(len(self.__path.poses) == 0):
return
max_num_peds = max(1, int(len(self.__path.poses)/300))
if(max_num_peds == 0):
num_peds = 0
else:
num_peds = random.randint(1,max_num_peds)
id = 1
ped_array = []
# Pedestrians along the path
for i in range(math.ceil(num_peds*0.8)):
ped_array.append(self.__get_random_ped_along_path(id))
id += 1
# Pedestrians crossing path
for i in range(math.ceil(num_peds)):
ped_array.append(self.__get_random_crossing_ped(id))
id += 1
self.__respawn_peds(ped_array)
return ped_array
def __get_random_ped_along_path(self, id):
"""
Spawning a pedestrian walking along the path. The pedestrian's waypoints are generated randomply
"""
start_pos = self.__generate_rand_pos_on_path(self.__path.poses, 100, 2)
start_pos.append(0.0)
waypoints = np.array([], dtype=np.int64).reshape(0, 3)
# if random.uniform(0.0, 1.0) < 0.85:
waypoints = np.vstack([waypoints, [start_pos[0], start_pos[1], 0.3]])
wp = self.__generate_rand_pos_on_path(self.__path.poses, 100, 2)
dist = self.__mean_sqare_dist_((wp[0] - start_pos[0]), (wp[1] - start_pos[1]))
count = 0
#Trying to find a waypoint with a minimum distance of 5.
count_thresh = 50
dist_thresh = 5
while (dist < dist_thresh and count < count_thresh):
wp = self.__generate_rand_pos_on_path(self.__path.poses, 100, 2)
dist = self.__mean_sqare_dist_((wp[0] - start_pos[0]), (wp[1] - start_pos[1]))
count += 1
if (count < count_thresh):
# Found waypoint with a minimum distance of 5m
wp.append(0.3)
waypoints = np.vstack([waypoints, wp])
else:
# Didn't find waypoint with a minimum distance of 5m --> pedestrian will stand around.
waypoints = np.vstack([waypoints, [start_pos[0], start_pos[1], 0.3]])
return [id, start_pos, waypoints]
# self.__spawn_ped(start_pos, waypoints, id)
def __get_random_crossing_ped(self, id):
"""
Spawning a pedestrian crossing the path. The pedestrian's waypoints are generated randomply
"""
try:
pos_index = random.randint(0, len(self.__path.poses) - 2)
except ValueError:
print("Path length < 2. No crossing pedestrians are spawned.")
return []
try:
path_pose = self.__path.poses[pos_index]
path_pose_temp = self.__path.poses[pos_index + 1]
except IndexError:
print("IndexError from retrieving path pose.")
return []
angle = math.atan2((path_pose_temp.pose.position.y - path_pose.pose.position.y),
(path_pose_temp.pose.position.x - path_pose.pose.position.x)) + math.pi / 2
start_pos = self.__generate_rand_pos_near_pos(path_pose, 4, angle + math.pi)
start_pos.append(0.0)
waypoints = np.array([], dtype=np.int64).reshape(0, 3)
wp1 = self.__generate_rand_pos_near_pos(path_pose, 4, angle + math.pi)
waypoints = np.vstack([waypoints, [wp1[0], wp1[1], 0.3]])
wp2 = self.__generate_rand_pos_near_pos(path_pose, 4, angle)
dist = self.__mean_sqare_dist_((wp2[0] - wp1[0]), (wp2[1] - wp1[1]))
#Trying to find a waypoint with a minimum distance of 5.
count = 0
count_thresh = 50
dist_thresh = 10
# while (dist < dist_thresh and count < count_thresh):
wp2 = self.__generate_rand_pos_near_pos(path_pose, 7, angle)
dist = self.__mean_sqare_dist_((wp2[0] - wp1[0]), (wp2[1] - wp1[1]))
count += 1
# if (count < count_thresh):
# Found waypoint with a minimum distance of 5m
wp2.append(0.3)
waypoints = np.vstack([waypoints, wp2])
rand = random.uniform(0.0, 1.0)
start_pos = [wp1[0] + rand * (wp2[0] - wp1[0]), wp1[1] + rand * (wp2[1] - wp1[1]), 0.0]
return [id, start_pos, waypoints]
# self.__spawn_ped(start_pos, waypoints, id)
def __respawn_peds(self, peds):
"""
Spawning one pedestrian in the simulation.
:param start_pos start position of the pedestrian.
:param wps waypoints the pedestrian is supposed to walk to.
:param id id of the pedestrian.
"""
srv = SpawnPeds()
srv.peds = []
for ped in peds:
msg = Ped()
msg.id = ped[0]
msg.pos = Point()
msg.pos.x = ped[1][0]
msg.pos.y = ped[1][1]
msg.pos.z = ped[1][2]
msg.type = self.__ped_type
msg.number_of_peds = 1
msg.yaml_file = self.__ped_file
msg.waypoints = []
for pos in ped[2]:
p = Point()
p.x = pos[0]
p.y = pos[1]
p.z = pos[2]
msg.waypoints.append(p)
srv.peds.append(msg)
rospy.wait_for_service('%s/pedsim_simulator/respawn_peds' % self.NS)
try:
# self.__spawn_ped_srv.call(srv.peds)
self.__respawn_peds_srv.call(srv.peds)
except rospy.ServiceException:
print('Spawn object: rospy.ServiceException. Closing serivce')
try:
self.__spawn_model.close()
except AttributeError:
print('Spawn object close(): AttributeError.')
return
self.__peds = peds
return
def __generate_rand_pos_on_path(self, path, range, max_r):
"""
Generating a random position on/near a path.
:param path the path on that the position is generated
:param range the range from the beginning and end of the path,
that is not considered during generation
:param max_r the radius around the path, where the position is generated
"""
try:
path_pose = random.choice(path[range:-range])
except IndexError:
try:
path_pose = path[range]
except IndexError:
return [0, 0]
pos_on_map = False
while not pos_on_map:
alpha = 2 * math.pi * random.random()
r = max_r * math.sqrt(random.random())
x = r * math.cos(alpha) + path_pose.pose.position.x
y = r * math.sin(alpha) + path_pose.pose.position.y
pos_on_map = self.__is_pos_valid(x, y, self.__map)
pos_on_map = True
return [x,y]
def __is_new_path_available(self, goal, start):
"""
Waiting for path to be published.
:return True if path is available
"""
is_available = False
begin = time.time()
while (time.time() - begin) < 0.1:
if self.__path.header.stamp <= self.__old_path_stamp or len(self.__path.poses) == 0:
time.sleep(0.0001)
else:
is_available = True
self.__old_path_stamp = self.__path.header.stamp
break
dist_goal = 10
dist_start = 10
if len(self.__path.poses) > 2:
#Checking of correct path has been published
dist_goal = self.__mean_sqare_dist_((goal[0] - self.__path.poses[-1].pose.position.x),
(goal[1] - self.__path.poses[-1].pose.position.y))
dist_start = self.__mean_sqare_dist_((start[0] - self.__path.poses[0].pose.position.x),
(start[1] - self.__path.poses[0].pose.position.y))
if not is_available or dist_goal > 0.5 or dist_start > 0.5:
is_available = False
print("path not valid!")
return is_available
def __generate_rand_pos_near_pos(self, path_pose, max_r, alpha):
"""
Generating a random position2 close to another position1 within a radius of max_r.
:param path_pose position1
:param max_r radius in that position2 is generated
:param alpha approx angle between position1 and position2
:return [x, y] position2
"""
pos_on_map = False
r = max_r
while not pos_on_map:
if (random.random() > 0.5):
alpha = alpha + random.random() * 45 * math.pi/180
else:
alpha = alpha - random.random() * 45 * math.pi/180
x = r * math.cos(alpha) + path_pose.pose.position.x
y = r * math.sin(alpha) + path_pose.pose.position.y
pos_on_map = self.__is_pos_valid(x, y, self.__map)
r -= 0.3
return [x, y]
def __map_callback(self, data):
"""
Receiving map from map topic
:param map data
:return:
"""
self.__map = data
return
def __path_callback(self, data):
self.__path = data
def __goal_status_callback(self, data):
"""
Recovery method for stable learning:
Checking goal status callback from global planner.
If goal is not valid, new goal will be published.
:param status_callback
"""
if len(data.status_list) > 0:
last_element = data.status_list[-1]
if last_element.status == 4:
if(self.__move_base_status_id != last_element.goal_id.id):
# self.set_random_static_task()
self.__move_base_status_id = last_element.goal_id.id
return
def __mean_sqare_dist_(self, x, y):
"""
Computing mean square distance of x and y
:param x, y
:return: sqrt(x^2 + y^2)
"""
return math.sqrt(math.pow(x, 2) + math.pow(y, 2))
def __yaw_to_quat(self, yaw):
"""
Computing corresponding quaternion q to angle yaw [rad]
:param yaw
:return: q
"""
q = Quaternion(axis=[0, 0, 1], angle=yaw)
return q.elements
================================================
FILE: rl_agent/src/rl_agent/env_wrapper/__init__.py
================================================
================================================
FILE: rl_agent/src/rl_agent/env_wrapper/ros_env.py
================================================
'''
@name: ros_env.py
@brief: This (abstract) class is a simulation environment wrapper.
It communicates with the BaseLocalPlanner in ROS and
provides all relevant methods for the RL-library stable baselines.
@author: Ronja Gueldenring
@version: 3.5
@date: 2019/04/05
'''
# python relevant
import math
import random
import numpy as np
import gym
import time
# ros-relevant
import rospy
# messages
from std_msgs.msg import Bool
from sensor_msgs.msg import LaserScan
from rl_msgs.msg import Waypoint
from geometry_msgs.msg import TwistStamped, Twist, Pose
from nav_msgs.msg import OccupancyGrid
# Helper classes
from rl_agent.env_utils.debug_ros_env import DebugRosEnv
from rl_agent.env_utils.reward_container import RewardContainer
from rl_agent.env_utils.task_generator import TaskGenerator
class RosEnvAbs(gym.Env):
'''
This (abstract) class is a simulation environment wrapper.
It communicates with the BaseLocalPlanner in ROS and
provides all relevant methods for the RL-library stable baselines.
'''
def __init__(self, ns, state_collector, execution_mode, task_mode, state_size, observation_space, stack_offset, action_size, action_space, debug, goal_radius, wp_radius, robot_radius, reward_fnc):
super(RosEnvAbs, self).__init__()
rospy.init_node("ros_env_%s"%ns, anonymous=True)
#Setting random seed.
seed = random.randint(0,1000)
random.seed(seed)
np.random.seed(seed)
# Class variables
self.STATE_SIZE = state_size
self.observation_space = observation_space
self.ACTION_SIZE = action_size
self.action_space = action_space
self.NS = ns # namespace
self.MODE = execution_mode # mode: TRAIN, EXEC, EXEC_RW, EVAL
self.input_img_ = OccupancyGrid() # occupancy grid containing state information
self.twist_ = TwistStamped() # speed of robot
self.__trigger = False # triggers agent to get state and compute action
self.debug_ = debug # enable debugging
self.done_ = True # Episode done?
self.merged_scan_ = LaserScan()
if self.MODE == "train" or self.MODE == "eval":
self.__task_mode = task_mode # "ped", "static", "toggle_ped_static", "ped_static"
self.__toggle = True
self.GOAL_RADIUS = goal_radius # radius, when goal is reached
self.WP_RADIUS = wp_radius # radius, when waypoint is reached
self.ROBOT_RADIUS = robot_radius # radius of the robot
self.REWARD_FUNC = reward_fnc # which reward function should be used.
self.__is_env_closed = False
self.__num_iterations = 0 # counting number of iterations for each episode
self.__transformed_goal = Pose() # Goal in robot frame
self.wp_ = Waypoint() # next waypoints on global plan
self.static_scan_ = LaserScan() # most recent static Laserscan
self.ped_scan_ = LaserScan() # most recent pedestrian Laserscan
# Helper Classes
self.__state_collector = state_collector
if self.debug_:
self.debugger_ = DebugRosEnv(self.NS, stack_offset)
if self.MODE == "train" or self.MODE == "eval":
if len(self.action_space.shape) == 0:
self.__reward_cont = RewardContainer(self.NS, robot_radius, goal_radius, self.v_max_)
else:
self.__reward_cont = RewardContainer(self.NS, robot_radius, goal_radius, self.action_space.high[0])
self.__task_generator = TaskGenerator(self.NS, self.__state_collector, self.ROBOT_RADIUS)
# Subscriber
self.__trigger_sub = rospy.Subscriber("%s/trigger_agent" % (self.NS), Bool, self.__trigger_callback)
# Publisher
self.__agent_action_pub = rospy.Publisher('%s/rl_agent/action' % (self.NS), Twist, queue_size=1)
# Sleeping so that py-Publisher has time to setup!
time.sleep(2)
def step(self, action):
"""
Action is forwarded to simulation. As reaction a new state is received.
Depending on state observation, reward and done is computed.
:return: obs, reward, done, info
"""
# Publishing action
# start = time.time()
action = self.get_cmd_vel_(action)
self.__agent_action_pub.publish(action)
# print("publish cmd_vel: %f"%(time.time() - start))
# waiting for robot-cycle to end
begin = time.time()
while not self.__trigger:
if self.MODE == "train" or self.MODE == "eval":
# Detecting if pipeline is broke --> resetting
if (time.time() - begin) > 20.0:
rospy.logerr("%s, step(): Timeout while waiting for local planner." % (self.NS))
self.reset()
self.__agent_action_pub.publish(action)
begin = time.time()
time.sleep(0.00001)
self.__trigger = False
# print("waiting for BaseLocalPlanner: %f"%(time.time() - begin))
# start = time.time()
self.__collect_state()
# print("__collect_state: %f"%(time.time() - start))
# start = time.time()
obs = self.get_observation_()
# print("get_observation_: %f"%(time.time() - start))
info = {}
if self.MODE == "train" or self.MODE == "train_demo" or self.MODE == "eval":
# start = time.time()
#info = [self.__num_iterations]
action = np.array([action.linear.x, action.angular.z])
reward = self.__compute_reward(action)
self.done_, done_reason = self.__is_done(self.__num_iterations, self.static_scan_.ranges, self.ped_scan_.ranges, reward)
info["done_reason"] = done_reason
# print("reward, done, ...: %f" % (time.time() - start))
else:
reward = 0
self.done_ = False
return obs, reward, self.done_, info
def close(self):
"""
Function executed when closing the environment.
Use it for closing GUIS and other systems that need closing.
:return:
"""
self.__stop_robot()
__is_env_closed = True
def __set_task(self):
"""
The task is set according to self.__task_mode and self.MODE.
The different task_modes are the following:
- ped: Spawning only pedestrians on path
- static: Spawning only static obstacles on path
- toggle_ped_static: Spawning alternating static obstacles and pedestrians
- ped_static: Spawning static obstacles and pedestrians at the same time
"""
if (self.MODE == "train" or self.MODE == "eval"):
if self.__task_mode == "ped":
self.__task_generator.set_random_ped_task()
elif self.__task_mode == "ped_short":
self.__task_generator.set_random_short_ped_task()
elif self.__task_mode == "static":
self.__task_generator.set_random_static_task()
elif self.__task_mode == "toggle_ped_static":
if self.__toggle:
self.__task_generator.set_random_static_task()
else:
self.__task_generator.set_random_ped_task()
self.__toggle = (not self.__toggle)
elif self.__task_mode == "ped_static":
self.__task_generator.set_random_static_ped_task()
def reset(self):
"""
Resetting simulation environment. That means, the robots position and goal
are set randomly. Furthermore obstacles are spawned on that path.
:return: initial observation of episode
"""
if self.MODE != "train" and self.MODE != "train_demo" and self.MODE != "eval":
return self.get_observation_()
# resetting task
self.__set_task()
self.__agent_action_pub.publish(Twist())
# waiting for planner to be ready for its first action
begin = time.time()
while not self.__trigger and not rospy.is_shutdown():
if (time.time() - begin) > 20.0:
rospy.logerr("%s, reset(): Timeout while waiting for local planner." % (self.NS))
self.__set_task()
begin = time.time()
time.sleep(0.001)
self.__trigger = False
# reseting internal state values
self.__num_iterations = 0
# Initializing state variables
self.__task_generator.take_sim_step()
self.__collect_state()
self.__reward_cont.reset(self.wp_.points)
return self.get_observation_()
def get_observation_(self):
"""
Function returns state that will be fed to the rl-agent
It includes laserscan data, waypoint information, ...
:param
:return: state
"""
raise NotImplementedError("Not implemented!")
def __collect_state(self):
"""
State is collected.
It can include the following information
image containing laser data and waypoint
goal in robot frame
velocity of robot
raw laser scans
waypoints in robot frame.
"""
[self.static_scan_, self.ped_scan_, self.merged_scan_, self.input_img_, self.wp_, self.twist_, self.__transformed_goal] = self.__state_collector.get_state()
if(self.debug_):
self.debugger_.show_wp(self.wp_)
def __compute_reward(self, action):
"""
Reward function gives feedback on taken action of the agent.
:param
:return: reward
"""
if self.REWARD_FUNC == 1:
reward = self.__reward_cont.rew_func_1(self.merged_scan_, self.wp_, self.__transformed_goal)
elif self.REWARD_FUNC == 2.1:
reward = self.__reward_cont.rew_func_2_1(self.static_scan_, self.ped_scan_, self.wp_, self.twist_,
self.__transformed_goal)
elif self.REWARD_FUNC == 2.2:
reward = self.__reward_cont.rew_func_2_2(self.static_scan_, self.ped_scan_, self.wp_, self.twist_, self.__transformed_goal)
elif self.REWARD_FUNC == 19:
reward = self.__reward_cont.rew_func_19(self.static_scan_, self.ped_scan_, self.wp_, self.twist_,
self.__transformed_goal)
else:
raise NotImplementedError
if self.debug_:
self.debugger_.show_reward(reward)
return reward
def __is_done(self, num_iterations, static_scan, ped_scan, reward):
"""
Checks if end of episode is reached. It is reached,
if maximum number of episodes is reached,
if the goal is reached,
if the robot collided with obstacle
if the reward function returns a high negative value.
:param current state
:return: reward
"""
# Is goal reached?
dist_to_goal = self.mean_sqare_dist_(self.__transformed_goal.position.x,
self.__transformed_goal.position.y)
# Obstacle collision?
min_obstacle_dist = np.amin(self.merged_scan_.ranges)
# Ped in box?
# is_ped_in_box = self.__reward_cont.is_ped_in_box(self.ped_scan_, 0.66, 0.86, 0.46)
if self.MODE == "eval":
max_iteration = 2000
else:
max_iteration = 650
if dist_to_goal < self.GOAL_RADIUS:
return [True, 1]
if min_obstacle_dist < self.ROBOT_RADIUS:
return [True, 2]
elif reward < -5:
return [True, 5]
elif num_iterations > max_iteration:
return [True, 3]
# elif self.MODE == "train" and self.mean_sqare_dist_(self.wp_.points[0].x, self.wp_.points[0].y) > 4:
# return [True, 4]
self.__num_iterations += 1
return [False, 0]
def __trigger_callback(self, data):
"""
If trigger is True, robot executed action successfully and provides a new state.
The agent can now determine the next action.
"""
self.__trigger = data.data
return
def mean_sqare_dist_(self, x, y):
"""
Computing mean square distance of x and y
:param x, y
:return: sqrt(x^2 + y^2)
"""
return math.sqrt(math.pow(x, 2) + math.pow(y, 2))
def get_cmd_vel_(self, action):
"""
Decoding action (from rl_agent) to cmd_vel message
:param action
:return: cmd_vel message
"""
raise NotImplementedError("Not implemented!")
def get_action_list(self):
"""
Getter for the action list. Is empty, when continuous action space
:return: action list
"""
raise NotImplementedError("Not implemented!")
def get_goal_radius(self):
"""
Getter for goal radius.
:return: goal radius
"""
return self.GOAL_RADIUS
def get_wp_radius(self):
"""
Getter for waypoint radius.
:return: waypoint radius
"""
return self.WP_RADIUS
def get_state_size(self):
"""
Getter for state size.
:return: state size
"""
return self.STATE_SIZE
def get_action_size(self):
"""
Getter for action size. Is inf if continuous
:return: action size
"""
return self.ACTION_SIZE
================================================
FILE: rl_agent/src/rl_agent/env_wrapper/ros_env_cont_img.py
================================================
'''
@name: ros_env_cont_img.py
@brief: This class is a simulation environment wrapper for
the X-Image Representation
with continuous action space.
@author: Ronja Gueldenring
@version: 3.5
@date: 2019/04/05
'''
# ros-relevant
import rospy
# python relevant
import numpy as np
from gym import spaces
#custom classes
from rl_agent.env_wrapper.ros_env_img import RosEnvImg
# Messages
from geometry_msgs.msg import Twist
# Parameters
GOAL_RADIUS = 0.4
WAYPOINT_RADIUS = 0.2
class RosEnvContImg(RosEnvImg):
'''
This class is a simulation environment wrapper for
the X-Image Representation
with continuous action space.
'''
def __init__(self, ns, state_collector, stack_offset, stack_size, robot_radius = 0.46, reward_fnc=6, debug=False, execution_mode="train", task_mode="static"):
img_width = rospy.get_param("%s/rl_agent/img_width_pos"%ns) + rospy.get_param("%s/rl_agent/img_width_neg"%ns)
img_height = rospy.get_param("%s/rl_agent/img_height"%ns)
state_size = (img_height, img_width, 1)
observation_space = spaces.Box(low=0, high=100, shape=state_size, dtype=np.float)
action_space = spaces.Box(low=np.array([0.0, -1.2]), high=np.array([0.8, 1.2]), dtype=np.float)
# action_space = spaces.Box(low=np.array([0.0, 0.0]), high=np.array([0.0, 0.0]), dtype=np.float)
super(RosEnvContImg, self).__init__(ns, state_collector, execution_mode, task_mode, state_size, observation_space, stack_offset, [], action_space, debug, GOAL_RADIUS, WAYPOINT_RADIUS, robot_radius, reward_fnc)
def get_cmd_vel_(self, action):
vel_msg = Twist()
vel_msg.linear.x = action[0]
vel_msg.angular.z = action[1]
return vel_msg
def get_action_list(self):
action_list = []
return action_list
================================================
FILE: rl_agent/src/rl_agent/env_wrapper/ros_env_cont_img_vel.py
================================================
'''
@name: ros_env_cont_img_vel.py
@brief: This class is a simulation environment wrapper for
the X-Image Speed Representation
with continuous action space.
@author: Ronja Gueldenring
@version: 3.5
@date: 2019/04/05
'''
# ros-relevant
import rospy
# python relevant
import numpy as np
from gym import spaces
#custom classes
from rl_agent.env_wrapper.ros_env_img_vel import RosEnvImgVel
# Messages
from geometry_msgs.msg import Twist
# Parameters
GOAL_RADIUS = 0.4
WAYPOINT_RADIUS = 0.2
class RosEnvContImgVel(RosEnvImgVel):
'''
This class is a simulation environment wrapper for
the X-Image Speed Representation
with continuous action space.
'''
def __init__(self, ns, state_collector, stack_offset, stack_size, robot_radius = 0.46, reward_fnc=6,debug=False, execution_mode="train", task_mode="static"):
img_width = rospy.get_param("%s/rl_agent/img_width_pos" % ns) + rospy.get_param(
"%s/rl_agent/img_width_neg" % ns)
img_height = rospy.get_param("%s/rl_agent/img_height" % ns)
state_size = (img_height + 1, img_width , 1)
observation_space = spaces.Box(low=0, high=100, shape=state_size, dtype=np.float)
action_space = spaces.Box(low=np.array([0.0, -0.7]), high=np.array([0.5, 0.7]), dtype=np.float)
super(RosEnvContImgVel, self).__init__(ns, state_collector, execution_mode, task_mode, state_size, observation_space, stack_offset, [], action_space, debug, GOAL_RADIUS, WAYPOINT_RADIUS, robot_radius, reward_fnc)
def get_cmd_vel_(self, action):
vel_msg = Twist()
vel_msg.linear.x = action[0]
vel_msg.angular.z = action[1]
return vel_msg
def get_action_list(self):
action_list = []
return action_list
================================================
FILE: rl_agent/src/rl_agent/env_wrapper/ros_env_cont_raw_data.py
================================================
'''
@name: ros_env_cont_raw_data.py
@brief: This class is a simulation environment wrapper for
the Raw Representation
with continuous action space.
@author: Ronja Gueldenring
@version: 3.5
@date: 2019/04/05
'''
import rospy
# python relevant
import numpy as np
from gym import spaces
# custom classes
from rl_agent.env_wrapper.ros_env_raw_data import RosEnvRaw
# messages
from geometry_msgs.msg import Twist
# Parameters
GOAL_RADIUS = 0.4
WAYPOINT_RADIUS = 0.2
class RosEnvContRaw(RosEnvRaw):
'''
This class is a simulation environment wrapper for
the X-Image Representation
with continuous action space.
'''
def __init__(self, ns, state_collector, stack_offset, stack_size, robot_radius = 0.46, reward_fnc=6, debug=False, execution_mode="train", task_mode="static"):
state_size_t = rospy.get_param("%s/rl_agent/scan_size"% ns) + rospy.get_param("%s/rl_agent/num_of_wps"%ns)*2
state_size = (1, state_size_t, 1)
observation_space = spaces.Box(low=0, high=6, shape=state_size, dtype=np.float)
action_space = spaces.Box(low=np.array([0.0, -0.5]), high=np.array([0.5, 0.5]), dtype=np.float)
super(RosEnvContRaw, self).__init__(ns, state_collector, execution_mode, task_mode, state_size, observation_space, stack_offset, [], action_space, debug, GOAL_RADIUS, WAYPOINT_RADIUS, robot_radius, reward_fnc)
def get_cmd_vel_(self, action):
vel_msg = Twist()
vel_msg.linear.x = action[0]
vel_msg.angular.z = action[1]
return vel_msg
def get_action_list(self):
action_list = []
return action_list
================================================
FILE: rl_agent/src/rl_agent/env_wrapper/ros_env_cont_raw_scan_prep_wp.py
================================================
'''
@name: ros_env_cont_raw_scan_prep_wo.py
@brief: This class is a simulation environment wrapper for
the Polar Representation
with continuous action space.
@author: Ronja Gueldenring
@version: 3.5
@date: 2019/04/05
'''
import rospy
# python relevant
import numpy as np
from gym import spaces
# custom classes
from rl_agent.env_wrapper.ros_env_raw_scan_prep_wp import RosEnvRawScanPrepWp
# messages
from geometry_msgs.msg import Twist
# Parameters
GOAL_RADIUS = 0.4
WAYPOINT_RADIUS = 0.2
class RosEnvContRawScanPrepWp(RosEnvRawScanPrepWp):
'''
This class is a simulation environment wrapper for
the Polar Representation
with continuous action space.
'''
def __init__(self, ns, state_collector, stack_offset, stack_size, robot_radius = 0.46, reward_fnc=6, debug=False, execution_mode="train", task_mode="static"):
state_size_t = rospy.get_param("%s/rl_agent/scan_size"% ns)
state_size = (state_size_t,2, 1)
observation_space = spaces.Box(low=0, high=6, shape=state_size, dtype=np.float)
action_space = spaces.Box(low=np.array([0.0, -0.5]), high=np.array([0.5, 0.5]), dtype=np.float)
super(RosEnvContRawScanPrepWp, self).__init__(ns, state_collector, execution_mode, task_mode, state_size, observation_space, stack_offset, [], action_space, debug, GOAL_RADIUS, WAYPOINT_RADIUS, robot_radius, reward_fnc)
def get_cmd_vel_(self, action):
vel_msg = Twist()
vel_msg.linear.x = action[0]
vel_msg.angular.z = action[1]
return vel_msg
def get_action_list(self):
action_list = []
return action_list
==================================
gitextract_wiq5yc1c/
├── .gitignore
├── .rosinstall
├── LICENSE
├── README.md
├── docker/
│ └── Dockerfile
├── example_agents/
│ ├── ppo2_1_img_disc_0/
│ │ └── ppo2_1_img_disc_0.pkl
│ ├── ppo2_1_raw_data_cont_0/
│ │ └── ppo2_1_raw_data_cont_0.pkl
│ ├── ppo2_1_raw_data_disc_0/
│ │ └── ppo2_1_raw_data_disc_0.pkl
│ └── ppo2_3_raw_data_disc_0/
│ └── ppo2_3_raw_data_disc_0.pkl
├── flatland_setup/
│ ├── CMakeLists.txt
│ ├── maps/
│ │ ├── complex_map_1/
│ │ │ ├── map.yaml
│ │ │ ├── pedsim_scenario.xml
│ │ │ └── world.yaml
│ │ ├── complex_map_2/
│ │ │ ├── map.yaml
│ │ │ ├── pedsim_scenario.xml
│ │ │ └── world.yaml
│ │ ├── complex_map_3/
│ │ │ ├── map.yaml
│ │ │ ├── pedsim_scenario.xml
│ │ │ └── world.yaml
│ │ ├── corridor_1/
│ │ │ ├── map.yaml
│ │ │ ├── pedsim_scenario.xml
│ │ │ └── world.yaml
│ │ ├── corridor_2/
│ │ │ ├── map.yaml
│ │ │ ├── pedsim_scenario.xml
│ │ │ └── world.yaml
│ │ ├── corridor_3/
│ │ │ ├── map.yaml
│ │ │ ├── pedsim_scenario.xml
│ │ │ └── world.yaml
│ │ ├── map_middle_complexity/
│ │ │ ├── map.yaml
│ │ │ ├── pedsim_scenario.xml
│ │ │ └── world.yaml
│ │ ├── no_map/
│ │ │ └── map.yaml
│ │ ├── open_field_1/
│ │ │ ├── map.yaml
│ │ │ ├── pedsim_scenario.xml
│ │ │ └── world.yaml
│ │ ├── open_field_2/
│ │ │ ├── map.yaml
│ │ │ ├── pedsim_scenario.xml
│ │ │ └── world.yaml
│ │ └── open_field_3/
│ │ ├── agent_scenario.xml
│ │ ├── map.yaml
│ │ ├── pedsim_scenario.xml
│ │ └── world.yaml
│ ├── objects/
│ │ ├── cylinder.model.yaml
│ │ ├── palett.model.yaml
│ │ ├── person_single_circle.model.yaml
│ │ ├── person_two_legged.model.yaml
│ │ └── wagon.model.yaml
│ ├── package.xml
│ └── robot/
│ └── robot1.model.yaml
├── requirements.txt
├── rl_agent/
│ ├── CMakeLists.txt
│ ├── include/
│ │ └── rl_agent/
│ │ ├── laser_scan_merger.h
│ │ └── tf_python.h
│ ├── launch/
│ │ ├── run_1_img_disc.launch
│ │ ├── run_1_raw_cont.launch
│ │ ├── run_1_raw_disc.launch
│ │ ├── run_3_raw_disc.launch
│ │ └── run_ppo_agent.launch
│ ├── package.xml
│ ├── scripts/
│ │ ├── evaluation/
│ │ │ ├── analysis.py
│ │ │ ├── evaluate_agent.py
│ │ │ ├── qualitative_plot.py
│ │ │ └── save_test_episodes.py
│ │ ├── run_scripts/
│ │ │ └── run_ppo.py
│ │ └── train_scripts/
│ │ └── train_ppo.py
│ ├── setup.py
│ └── src/
│ ├── __init__.py
│ ├── laser_scan_merger.cpp
│ ├── rl_agent/
│ │ ├── __init__.py
│ │ ├── common_custom_policies.py
│ │ ├── env_utils/
│ │ │ ├── __init__.py
│ │ │ ├── debug_ros_env.py
│ │ │ ├── reward_container.py
│ │ │ ├── state_collector.py
│ │ │ └── task_generator.py
│ │ ├── env_wrapper/
│ │ │ ├── __init__.py
│ │ │ ├── ros_env.py
│ │ │ ├── ros_env_cont_img.py
│ │ │ ├── ros_env_cont_img_vel.py
│ │ │ ├── ros_env_cont_raw_data.py
│ │ │ ├── ros_env_cont_raw_scan_prep_wp.py
│ │ │ ├── ros_env_disc_img.py
│ │ │ ├── ros_env_disc_img_vel.py
│ │ │ ├── ros_env_disc_raw_data.py
│ │ │ ├── ros_env_disc_raw_scan_prep_wp.py
│ │ │ ├── ros_env_img.py
│ │ │ ├── ros_env_img_vel.py
│ │ │ ├── ros_env_raw_data.py
│ │ │ └── ros_env_raw_scan_prep_wp.py
│ │ └── evaluation/
│ │ ├── Analysis_eval.py
│ │ ├── Evaluation.py
│ │ └── __init__.py
│ └── tf_python.cpp
├── rl_bringup/
│ ├── CMakeLists.txt
│ ├── config/
│ │ ├── costmap_common_params.yaml
│ │ ├── dwa_params.yaml
│ │ ├── global_costmap_params.yaml
│ │ ├── local_costmap_params.yaml
│ │ ├── movebase_params.yaml
│ │ ├── path_config.ini
│ │ ├── path_config_docker.ini
│ │ ├── rl_common.yaml
│ │ ├── rl_params_img.yaml
│ │ └── rl_params_scan.yaml
│ ├── launch/
│ │ ├── rviz.launch
│ │ ├── setup.launch
│ │ └── sub_launch/
│ │ ├── dummy_localization.launch
│ │ ├── flatland.launch
│ │ ├── move_base.launch
│ │ ├── navigation.launch
│ │ ├── pedestrians_only.launch
│ │ ├── pedsim_visualizer.launch
│ │ └── simulation.launch
│ ├── package.xml
│ ├── rviz/
│ │ └── robot_navigation.rviz
│ └── src/
│ └── toggle_setup_init.cpp
├── rl_local_planner/
│ ├── CMakeLists.txt
│ ├── blp_plugin.xml
│ ├── include/
│ │ └── rl_local_planner/
│ │ ├── image_generator.h
│ │ ├── rl_local_planner.h
│ │ └── wp_generator.h
│ ├── package.xml
│ └── src/
│ ├── image_generator.cpp
│ ├── rl_local_planner.cpp
│ └── wp_generator.cpp
├── rl_msgs/
│ ├── CMakeLists.txt
│ ├── msg/
│ │ └── Waypoint.msg
│ ├── package.xml
│ └── srv/
│ ├── MergeScans.srv
│ ├── SetPath.srv
│ └── StateImageGenerationSrv.srv
└── start_scripts/
├── entrypoint_ppo2.sh
├── ppo2_training.sh
├── ros.sh
├── start_roscore.sh
└── training_params/
├── ppo2_params.csv
└── training_maps.csv
SYMBOL INDEX (214 symbols across 35 files)
FILE: rl_agent/include/rl_agent/laser_scan_merger.h
function namespace (line 26) | namespace rl_agent {
FILE: rl_agent/include/rl_agent/tf_python.h
function namespace (line 17) | namespace rl_agent {
FILE: rl_agent/scripts/evaluation/analysis.py
function analyse (line 16) | def analyse(complexity, evaluation_file_path, reward_file_path, save_path):
FILE: rl_agent/scripts/evaluation/evaluate_agent.py
function evaluate (line 21) | def evaluate(ns, sc, evaluation_set_path, save_path):
FILE: rl_agent/scripts/run_scripts/run_ppo.py
function load_train_env (line 33) | def load_train_env(ns, state_collector, robot_radius, rew_fnc, num_stacks,
function run_ppo (line 73) | def run_ppo(config, state_collector, agent_name ="ppo_99_8507750", polic...
FILE: rl_agent/scripts/train_scripts/train_ppo.py
function train_callback (line 39) | def train_callback(_locals, _globals):
function load_train_env (line 65) | def load_train_env(num_envs, robot_radius, rew_fnc, num_stacks, stack_of...
function train_agent_ppo2 (line 103) | def train_agent_ppo2(config, agent_name, total_timesteps, policy,
function evaluate_during_training (line 186) | def evaluate_during_training(ns, save_path, robot_radius):
FILE: rl_agent/src/laser_scan_merger.cpp
type rl_agent (line 10) | namespace rl_agent {
function main (line 118) | int main(int argc, char** argv){
FILE: rl_agent/src/rl_agent/common_custom_policies.py
function laser_cnn_multi_input (line 21) | def laser_cnn_multi_input(state, **kwargs):
class CNN1DPolicy_multi_input (line 46) | class CNN1DPolicy_multi_input(common.FeedForwardPolicy):
method __init__ (line 50) | def __init__(self, *args, **kwargs):
function laser_cnn_stack (line 59) | def laser_cnn_stack(state, **kwargs):
class CNN1DPolicy (line 85) | class CNN1DPolicy(common.FeedForwardPolicy):
method __init__ (line 89) | def __init__(self, *args, **kwargs):
function nature_cnn_multi_input (line 96) | def nature_cnn_multi_input(state, **kwargs):
class CnnPolicy_multi_input_vel (line 124) | class CnnPolicy_multi_input_vel(common.FeedForwardPolicy):
method __init__ (line 128) | def __init__(self, *args, **kwargs):
function cnn_multi_input (line 139) | def cnn_multi_input(state, **kwargs):
class CnnPolicy_multi_input_vel2 (line 169) | class CnnPolicy_multi_input_vel2(common.FeedForwardPolicy):
method __init__ (line 173) | def __init__(self, *args, **kwargs):
class CnnPolicy_multi_input_vel3 (line 180) | class CnnPolicy_multi_input_vel3(common.FeedForwardPolicy):
method __init__ (line 185) | def __init__(self, *args, **kwargs):
FILE: rl_agent/src/rl_agent/env_utils/debug_ros_env.py
class DebugRosEnv (line 23) | class DebugRosEnv():
method __init__ (line 29) | def __init__(self, ns, stack_offset):
method show_wp (line 55) | def show_wp(self, data):
method show_image_stack (line 74) | def show_image_stack(self, data):
method __data_to_image (line 88) | def __data_to_image(self, data):
method show_input_image (line 101) | def show_input_image(self, data):
method show_input_scan (line 110) | def show_input_scan(self, scan):
method show_input_occ_grid (line 117) | def show_input_occ_grid(self, state_image):
method show_reward (line 124) | def show_reward(self, reward):
FILE: rl_agent/src/rl_agent/env_utils/reward_container.py
class RewardContainer (line 16) | class RewardContainer():
method __init__ (line 20) | def __init__(self, ns, robot_radius, goal_radius, max_trans_vel):
method reset (line 31) | def reset(self, wps):
method rew_func_1 (line 41) | def rew_func_1(self, scan, wps, transformed_goal):
method rew_func_19 (line 63) | def rew_func_19(self, static_scan, ped_scan_msg, wps, twist, transform...
method rew_func_2_1 (line 101) | def rew_func_2_1(self, static_scan, ped_scan_msg, wps, twist, transfor...
method rew_func_2_2 (line 144) | def rew_func_2_2(self, static_scan, ped_scan_msg, wps, twist, transfor...
method __ped_in_box_punish (line 184) | def __ped_in_box_punish(self, ped_scan_msg, box_width, box_height_pos,...
method is_ped_in_box (line 202) | def is_ped_in_box(self, ped_scan_msg, box_width, box_height_pos, box_h...
method __check_reward (line 227) | def __check_reward(self, rew, obstacle_punish, goal_reached_rew, thresh):
method __get_turn_punish (line 242) | def __get_turn_punish(self, w, fac, thresh):
method __get_obstacle_punish (line 255) | def __get_obstacle_punish(self, scan, k, radius):
method __get_obstacle_punish_section (line 269) | def __get_obstacle_punish_section(self, scan, k, perc):
method __get_goal_reached_rew (line 287) | def __get_goal_reached_rew(self, transformed_goal, k):
method __get_wp_approached (line 301) | def __get_wp_approached(self, wps, punish_fac, rew_fac, k):
method mean_sqare_dist_ (line 331) | def mean_sqare_dist_(self, x, y):
FILE: rl_agent/src/rl_agent/env_utils/state_collector.py
class StateCollector (line 23) | class StateCollector():
method __init__ (line 27) | def __init__(self, ns, train_mode):
method get_state (line 76) | def get_state(self):
method get_static_scan (line 143) | def get_static_scan(self):
method set_state_mode (line 151) | def set_state_mode(self, state_mode):
method __remove_nans_from_scan (line 154) | def __remove_nans_from_scan(self, scan_msg):
method __static_scan_callback (line 164) | def __static_scan_callback(self, data):
method __ped_scan_callback (line 167) | def __ped_scan_callback(self, data):
method __f_scan_callback (line 170) | def __f_scan_callback(self, data):
method __b_scan_callback (line 173) | def __b_scan_callback(self, data):
method __wp_callback (line 176) | def __wp_callback(self, data):
method __twist_callback (line 180) | def __twist_callback(self, data):
method __goal_callback (line 183) | def __goal_callback(self, data):
method __is_wp_reached_callback (line 186) | def __is_wp_reached_callback(self, data):
method __wp_reached_callback (line 189) | def __wp_reached_callback(self, data):
FILE: rl_agent/src/rl_agent/env_utils/task_generator.py
class TaskGenerator (line 33) | class TaskGenerator():
method __init__ (line 41) | def __init__(self, ns, state_collector, robot_radius):
method set_task (line 106) | def set_task(self, task):
method set_path (line 143) | def set_path(self):
method set_random_static_task (line 156) | def set_random_static_task(self):
method set_random_ped_task (line 178) | def set_random_ped_task(self):
method set_random_short_ped_task (line 225) | def set_random_short_ped_task(self):
method set_random_static_ped_task (line 273) | def set_random_static_ped_task(self):
method take_sim_step (line 295) | def take_sim_step(self):
method __set_random_robot_pos (line 305) | def __set_random_robot_pos(self):
method set_robot_pos (line 320) | def set_robot_pos(self, x, y, theta):
method __wait_for_laser (line 336) | def __wait_for_laser(self):
method __pub_initial_position (line 353) | def __pub_initial_position(self, x, y, theta):
method __publish_random_goal_ (line 374) | def __publish_random_goal_(self):
method __publish_goal (line 383) | def __publish_goal(self, x, y, theta):
method __get_random_pos_on_map (line 404) | def __get_random_pos_on_map(self, map):
method __is_pos_valid (line 421) | def __is_pos_valid(self, x, y, map):
method __spread_done (line 447) | def __spread_done(self):
method __spread_new_task (line 453) | def __spread_new_task(self):
method __stop_robot (line 459) | def __stop_robot(self):
method __spawn_random_static_objects (line 467) | def __spawn_random_static_objects(self):
method respawn_static_objects (line 498) | def respawn_static_objects(self, models):
method spawn_object (line 528) | def spawn_object(self, model_name, index, x, y, theta):
method remove_all_static_objects (line 572) | def remove_all_static_objects(self):
method __init_object_remove (line 584) | def __init_object_remove(self):
method __remove_all_peds (line 611) | def __remove_all_peds(self):
method spawn_random_peds_in_world (line 622) | def spawn_random_peds_in_world(self, n):
method __spawn_random_peds_on_path (line 642) | def __spawn_random_peds_on_path(self):
method __get_random_ped_along_path (line 670) | def __get_random_ped_along_path(self, id):
method __get_random_crossing_ped (line 703) | def __get_random_crossing_ped(self, id):
method __respawn_peds (line 752) | def __respawn_peds(self, peds):
method __generate_rand_pos_on_path (line 794) | def __generate_rand_pos_on_path(self, path, range, max_r):
method __is_new_path_available (line 819) | def __is_new_path_available(self, goal, start):
method __generate_rand_pos_near_pos (line 848) | def __generate_rand_pos_near_pos(self, path_pose, max_r, alpha):
method __map_callback (line 873) | def __map_callback(self, data):
method __path_callback (line 882) | def __path_callback(self, data):
method __goal_status_callback (line 885) | def __goal_status_callback(self, data):
method __mean_sqare_dist_ (line 901) | def __mean_sqare_dist_(self, x, y):
method __yaw_to_quat (line 909) | def __yaw_to_quat(self, yaw):
FILE: rl_agent/src/rl_agent/env_wrapper/ros_env.py
class RosEnvAbs (line 33) | class RosEnvAbs(gym.Env):
method __init__ (line 39) | def __init__(self, ns, state_collector, execution_mode, task_mode, sta...
method step (line 102) | def step(self, action):
method close (line 150) | def close(self):
method __set_task (line 159) | def __set_task(self):
method reset (line 185) | def reset(self):
method get_observation_ (line 219) | def get_observation_(self):
method __collect_state (line 228) | def __collect_state(self):
method __compute_reward (line 242) | def __compute_reward(self, action):
method __is_done (line 266) | def __is_done(self, num_iterations, static_scan, ped_scan, reward):
method __trigger_callback (line 304) | def __trigger_callback(self, data):
method mean_sqare_dist_ (line 313) | def mean_sqare_dist_(self, x, y):
method get_cmd_vel_ (line 321) | def get_cmd_vel_(self, action):
method get_action_list (line 329) | def get_action_list(self):
method get_goal_radius (line 336) | def get_goal_radius(self):
method get_wp_radius (line 343) | def get_wp_radius(self):
method get_state_size (line 350) | def get_state_size(self):
method get_action_size (line 357) | def get_action_size(self):
FILE: rl_agent/src/rl_agent/env_wrapper/ros_env_cont_img.py
class RosEnvContImg (line 29) | class RosEnvContImg(RosEnvImg):
method __init__ (line 36) | def __init__(self, ns, state_collector, stack_offset, stack_size, robo...
method get_cmd_vel_ (line 45) | def get_cmd_vel_(self, action):
method get_action_list (line 51) | def get_action_list(self):
FILE: rl_agent/src/rl_agent/env_wrapper/ros_env_cont_img_vel.py
class RosEnvContImgVel (line 30) | class RosEnvContImgVel(RosEnvImgVel):
method __init__ (line 36) | def __init__(self, ns, state_collector, stack_offset, stack_size, robo...
method get_cmd_vel_ (line 45) | def get_cmd_vel_(self, action):
method get_action_list (line 51) | def get_action_list(self):
FILE: rl_agent/src/rl_agent/env_wrapper/ros_env_cont_raw_data.py
class RosEnvContRaw (line 28) | class RosEnvContRaw(RosEnvRaw):
method __init__ (line 34) | def __init__(self, ns, state_collector, stack_offset, stack_size, robo...
method get_cmd_vel_ (line 42) | def get_cmd_vel_(self, action):
method get_action_list (line 48) | def get_action_list(self):
FILE: rl_agent/src/rl_agent/env_wrapper/ros_env_cont_raw_scan_prep_wp.py
class RosEnvContRawScanPrepWp (line 27) | class RosEnvContRawScanPrepWp(RosEnvRawScanPrepWp):
method __init__ (line 33) | def __init__(self, ns, state_collector, stack_offset, stack_size, robo...
method get_cmd_vel_ (line 41) | def get_cmd_vel_(self, action):
method get_action_list (line 47) | def get_action_list(self):
FILE: rl_agent/src/rl_agent/env_wrapper/ros_env_disc_img.py
class RosEnvDiscImg (line 30) | class RosEnvDiscImg(RosEnvImg):
method __init__ (line 36) | def __init__(self, ns, state_collector, stack_offset, stack_size, robo...
method get_cmd_vel_ (line 59) | def get_cmd_vel_(self, action):
method __encode_action (line 66) | def __encode_action(self, action):
method get_action_list (line 69) | def get_action_list(self):
FILE: rl_agent/src/rl_agent/env_wrapper/ros_env_disc_img_vel.py
class RosEnvDiscImgVel (line 29) | class RosEnvDiscImgVel(RosEnvImgVel):
method __init__ (line 35) | def __init__(self, ns, state_collector, stack_offset, stack_size, robo...
method get_cmd_vel_ (line 57) | def get_cmd_vel_(self, action):
method __encode_action (line 64) | def __encode_action(self, action):
method get_action_list (line 67) | def get_action_list(self):
FILE: rl_agent/src/rl_agent/env_wrapper/ros_env_disc_raw_data.py
class RosEnvDiscRaw (line 28) | class RosEnvDiscRaw(RosEnvRaw):
method __init__ (line 34) | def __init__(self, ns, state_collector, stack_offset, stack_size, robo...
method get_cmd_vel_ (line 57) | def get_cmd_vel_(self, action):
method __encode_action (line 64) | def __encode_action(self, action):
method get_action_list (line 67) | def get_action_list(self):
FILE: rl_agent/src/rl_agent/env_wrapper/ros_env_disc_raw_scan_prep_wp.py
class RosEnvDiscRawScanPrepWp (line 26) | class RosEnvDiscRawScanPrepWp(RosEnvRawScanPrepWp):
method __init__ (line 32) | def __init__(self, ns, state_collector, stack_offset, stack_size, robo...
method get_cmd_vel_ (line 52) | def get_cmd_vel_(self, action):
method __encode_action (line 60) | def __encode_action(self, action):
method get_action_list (line 63) | def get_action_list(self):
FILE: rl_agent/src/rl_agent/env_wrapper/ros_env_img.py
class RosEnvImg (line 20) | class RosEnvImg(RosEnvAbs):
method __init__ (line 25) | def __init__(self, ns, state_collector, execution_mode, task_mode, sta...
method get_observation_ (line 30) | def get_observation_(self):
FILE: rl_agent/src/rl_agent/env_wrapper/ros_env_img_vel.py
class RosEnvImgVel (line 19) | class RosEnvImgVel(RosEnvAbs):
method __init__ (line 24) | def __init__(self, ns, state_collector, execution_mode, task_mode, sta...
method get_observation_ (line 29) | def get_observation_(self):
FILE: rl_agent/src/rl_agent/env_wrapper/ros_env_raw_data.py
class RosEnvRaw (line 18) | class RosEnvRaw(RosEnvAbs):
method __init__ (line 23) | def __init__(self, ns, state_collector, execution_mode, task_mode, sta...
method get_observation_ (line 29) | def get_observation_(self):
FILE: rl_agent/src/rl_agent/env_wrapper/ros_env_raw_scan_prep_wp.py
class RosEnvRawScanPrepWp (line 19) | class RosEnvRawScanPrepWp(RosEnvAbs):
method __init__ (line 24) | def __init__(self, ns, state_collector, execution_mode, task_mode, sta...
method get_observation_ (line 29) | def get_observation_(self):
FILE: rl_agent/src/rl_agent/evaluation/Analysis_eval.py
class Analysis (line 14) | class Analysis():
method __init__ (line 15) | def __init__(self):
method load_results (line 19) | def load_results(self, filename):
method get_timestep_list (line 30) | def get_timestep_list(self, results):
method reconstruct_timestep_array (line 37) | def reconstruct_timestep_array(self, timesteps):
method get_scores (line 58) | def get_scores(self, results):
method get_percentual_success_drive (line 87) | def get_percentual_success_drive(self, results):
method get_path_length_ratio (line 106) | def get_path_length_ratio(self, results):
method get_speed (line 129) | def get_speed(self, results):
method get_reward (line 152) | def get_reward(self, filename):
method __closest_path_pos (line 174) | def __closest_path_pos(self, path, pose):
method __mean_square_dist (line 196) | def __mean_square_dist(self, x, y):
method __get_path_length (line 206) | def __get_path_length(self, poses):
FILE: rl_agent/src/rl_agent/evaluation/Evaluation.py
class Evaluation (line 26) | class Evaluation():
method __init__ (line 31) | def __init__(self, state_collector, ns, robot_radius = 0,robot_width =...
method load_evaluation_set (line 67) | def load_evaluation_set(self, eval_set_path):
method evaluate_set (line 78) | def evaluate_set(self, evaluation_set_path, save_path):
method evaluate_episode (line 99) | def evaluate_episode(self, train):
method __sleep (line 170) | def __sleep(self, secs):
method evaluate_training (line 185) | def evaluate_training(self, save_path):
method show_paths (line 204) | def show_paths(self, result):
method generate_qualitative_static_image_rviz (line 212) | def generate_qualitative_static_image_rviz(self, results, results_comp...
method generate_qualitative_ped_image_rviz (line 246) | def generate_qualitative_ped_image_rviz(self, results, i_task, i_pos):
method __rect_robot_collision (line 315) | def __rect_robot_collision(self, static_scan_msg, ped_scan_msg, robot_...
method __odom_callback (line 336) | def __odom_callback(self, data):
method __path_callback (line 339) | def __path_callback(self, data):
method __done_callback (line 342) | def __done_callback(self, data):
method __new_task_callback (line 345) | def __new_task_callback(self, data):
method __flatland_topic_callback (line 348) | def __flatland_topic_callback(self, data):
method __trigger_callback (line 351) | def __trigger_callback(self, data):
method __clock_callback (line 354) | def __clock_callback(self, data):
method __ped_callback (line 357) | def __ped_callback(self, data):
FILE: rl_agent/src/tf_python.cpp
type rl_agent (line 12) | namespace rl_agent {
function main (line 60) | int main(int argc, char** argv){
FILE: rl_bringup/src/toggle_setup_init.cpp
function main (line 10) | int main(int argc, char** argv){
FILE: rl_local_planner/include/rl_local_planner/image_generator.h
function namespace (line 33) | namespace rl_image_generator {
FILE: rl_local_planner/include/rl_local_planner/rl_local_planner.h
function namespace (line 20) | namespace rl_local_planner {
FILE: rl_local_planner/include/rl_local_planner/wp_generator.h
function namespace (line 21) | namespace rl_local_planner {
FILE: rl_local_planner/src/image_generator.cpp
type rl_image_generator (line 10) | namespace rl_image_generator {
function main (line 223) | int main(int argc, char** argv){
FILE: rl_local_planner/src/rl_local_planner.cpp
type rl_local_planner (line 13) | namespace rl_local_planner {
FILE: rl_local_planner/src/wp_generator.cpp
type rl_local_planner (line 11) | namespace rl_local_planner {
function main (line 156) | int main(int argc, char** argv){
Condensed preview — 137 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (337K chars).
[
{
"path": ".gitignore",
"chars": 46,
"preview": "*.png\n*__pycache__*\n*.idea*\n*.pyc\n*screenlog*\n"
},
{
"path": ".rosinstall",
"chars": 559,
"preview": "- git:\n local-name: drl_local_planner_forks/flatland\n uri: https://github.com/RGring/flatland.git\n version: ped"
},
{
"path": "LICENSE",
"chars": 1506,
"preview": "BSD 3-Clause License\n\nCopyright (c) 2019, RGring\nAll rights reserved.\n\nRedistribution and use in source and binary forms"
},
{
"path": "README.md",
"chars": 10199,
"preview": "# What is this repository for?\n* Setup to train a local planner with reinforcement learning approaches from [stable base"
},
{
"path": "docker/Dockerfile",
"chars": 1275,
"preview": "FROM ros:kinetic-robot-xenial\n\nRUN apt-get update && apt-get install -y \\\n libqt4-dev \\\n libopencv-dev \\\n liblua5.2-d"
},
{
"path": "flatland_setup/CMakeLists.txt",
"chars": 6795,
"preview": "cmake_minimum_required(VERSION 2.8.3)\nproject(flatland_setup)\n\n## Compile as C++11, supported in ROS Kinetic and newer\n#"
},
{
"path": "flatland_setup/maps/complex_map_1/map.yaml",
"chars": 188,
"preview": "image: complex_map_1.png\nresolution: 0.05\n# origin: [-1.1, -9.05, 0.0]\n# origin: [-0.5, -1.45, 0.0]\norigin: [0.0, 0.0, 0"
},
{
"path": "flatland_setup/maps/complex_map_1/pedsim_scenario.xml",
"chars": 349,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<scenario>\n\n <waypoint id=\"robot_goal\" x=\"22\" y=\"27\" r=\"2\"/> -->\n <waypoint"
},
{
"path": "flatland_setup/maps/complex_map_1/world.yaml",
"chars": 1107,
"preview": "properties: \n # optional, defaults to 10, number of velocity iterations for the Box2D\n # physics solver\n velocity_ite"
},
{
"path": "flatland_setup/maps/complex_map_2/map.yaml",
"chars": 188,
"preview": "image: complex_map_2.png\nresolution: 0.05\n# origin: [-1.1, -9.05, 0.0]\n# origin: [-0.5, -1.45, 0.0]\norigin: [0.0, 0.0, 0"
},
{
"path": "flatland_setup/maps/complex_map_2/pedsim_scenario.xml",
"chars": 349,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<scenario>\n\n <waypoint id=\"robot_goal\" x=\"22\" y=\"27\" r=\"2\"/> -->\n <waypoint"
},
{
"path": "flatland_setup/maps/complex_map_2/world.yaml",
"chars": 1107,
"preview": "properties: \n # optional, defaults to 10, number of velocity iterations for the Box2D\n # physics solver\n velocity_ite"
},
{
"path": "flatland_setup/maps/complex_map_3/map.yaml",
"chars": 188,
"preview": "image: complex_map_3.png\nresolution: 0.05\n# origin: [-1.1, -9.05, 0.0]\n# origin: [-0.5, -1.45, 0.0]\norigin: [0.0, 0.0, 0"
},
{
"path": "flatland_setup/maps/complex_map_3/pedsim_scenario.xml",
"chars": 349,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<scenario>\n\n <waypoint id=\"robot_goal\" x=\"22\" y=\"27\" r=\"2\"/> -->\n <waypoint"
},
{
"path": "flatland_setup/maps/complex_map_3/world.yaml",
"chars": 1107,
"preview": "properties: \n # optional, defaults to 10, number of velocity iterations for the Box2D\n # physics solver\n velocity_ite"
},
{
"path": "flatland_setup/maps/corridor_1/map.yaml",
"chars": 185,
"preview": "image: corridor_1.png\nresolution: 0.05\n# origin: [-1.1, -9.05, 0.0]\n# origin: [-0.5, -1.45, 0.0]\norigin: [0.0, 0.0, 0.0]"
},
{
"path": "flatland_setup/maps/corridor_1/pedsim_scenario.xml",
"chars": 349,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<scenario>\n\n <waypoint id=\"robot_goal\" x=\"22\" y=\"27\" r=\"2\"/> -->\n <waypoint"
},
{
"path": "flatland_setup/maps/corridor_1/world.yaml",
"chars": 1107,
"preview": "properties: \n # optional, defaults to 10, number of velocity iterations for the Box2D\n # physics solver\n velocity_ite"
},
{
"path": "flatland_setup/maps/corridor_2/map.yaml",
"chars": 185,
"preview": "image: corridor_2.png\nresolution: 0.05\n# origin: [-1.1, -9.05, 0.0]\n# origin: [-0.5, -1.45, 0.0]\norigin: [0.0, 0.0, 0.0]"
},
{
"path": "flatland_setup/maps/corridor_2/pedsim_scenario.xml",
"chars": 349,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<scenario>\n\n <waypoint id=\"robot_goal\" x=\"22\" y=\"27\" r=\"2\"/> -->\n <waypoint"
},
{
"path": "flatland_setup/maps/corridor_2/world.yaml",
"chars": 1107,
"preview": "properties: \n # optional, defaults to 10, number of velocity iterations for the Box2D\n # physics solver\n velocity_ite"
},
{
"path": "flatland_setup/maps/corridor_3/map.yaml",
"chars": 185,
"preview": "image: corridor_3.png\nresolution: 0.05\n# origin: [-1.1, -9.05, 0.0]\n# origin: [-0.5, -1.45, 0.0]\norigin: [0.0, 0.0, 0.0]"
},
{
"path": "flatland_setup/maps/corridor_3/pedsim_scenario.xml",
"chars": 349,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<scenario>\n\n <waypoint id=\"robot_goal\" x=\"22\" y=\"27\" r=\"2\"/> -->\n <waypoint"
},
{
"path": "flatland_setup/maps/corridor_3/world.yaml",
"chars": 1107,
"preview": "properties: \n # optional, defaults to 10, number of velocity iterations for the Box2D\n # physics solver\n velocity_ite"
},
{
"path": "flatland_setup/maps/map_middle_complexity/map.yaml",
"chars": 126,
"preview": "image: map_middle_complexity.png\nresolution: 0.05\norigin: [0.0, 0.0, 0.0]\noccupied_thresh: 0.65\nfree_thresh: 0.196\nnegat"
},
{
"path": "flatland_setup/maps/map_middle_complexity/pedsim_scenario.xml",
"chars": 2814,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<scenario>\n\n <obstacle x1=\"1.4\" y1=\"24.6\" x2=\"16.9\" y2=\"24.6\"/>\n <obstacle "
},
{
"path": "flatland_setup/maps/map_middle_complexity/world.yaml",
"chars": 571,
"preview": "properties: \n # optional, defaults to 10, number of velocity iterations for the Box2D\n # physics solver\n velocity_ite"
},
{
"path": "flatland_setup/maps/no_map/map.yaml",
"chars": 146,
"preview": "# image: no_map.png\nimage: no_map.png\nresolution: 0.05\norigin: [-0.5, -1.45, 0.0]\noccupied_thresh: 0.65\nfree_thresh: 0.1"
},
{
"path": "flatland_setup/maps/open_field_1/map.yaml",
"chars": 187,
"preview": "image: open_field_1.png\nresolution: 0.05\n# origin: [-1.1, -9.05, 0.0]\n# origin: [-0.5, -1.45, 0.0]\norigin: [0.0, 0.0, 0."
},
{
"path": "flatland_setup/maps/open_field_1/pedsim_scenario.xml",
"chars": 349,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<scenario>\n\n <waypoint id=\"robot_goal\" x=\"22\" y=\"27\" r=\"2\"/> -->\n <waypoint"
},
{
"path": "flatland_setup/maps/open_field_1/world.yaml",
"chars": 1107,
"preview": "properties: \n # optional, defaults to 10, number of velocity iterations for the Box2D\n # physics solver\n velocity_ite"
},
{
"path": "flatland_setup/maps/open_field_2/map.yaml",
"chars": 187,
"preview": "image: open_field_2.png\nresolution: 0.05\n# origin: [-1.1, -9.05, 0.0]\n# origin: [-0.5, -1.45, 0.0]\norigin: [0.0, 0.0, 0."
},
{
"path": "flatland_setup/maps/open_field_2/pedsim_scenario.xml",
"chars": 349,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<scenario>\n\n <waypoint id=\"robot_goal\" x=\"22\" y=\"27\" r=\"2\"/> -->\n <waypoint"
},
{
"path": "flatland_setup/maps/open_field_2/world.yaml",
"chars": 1107,
"preview": "properties: \n # optional, defaults to 10, number of velocity iterations for the Box2D\n # physics solver\n velocity_ite"
},
{
"path": "flatland_setup/maps/open_field_3/agent_scenario.xml",
"chars": 1276,
"preview": "<waypoint id=\"Entry1\" x=\"6\" y=\"26\" r=\"2\"/>\n<waypoint id=\"Entry2\" x=\"2\" y=\"5\" r=\"2\"/>\n<waypoint id=\"Entry3\" x=\"2\" y=\"15\" "
},
{
"path": "flatland_setup/maps/open_field_3/map.yaml",
"chars": 187,
"preview": "image: open_field_3.png\nresolution: 0.05\n# origin: [-1.1, -9.05, 0.0]\n# origin: [-0.5, -1.45, 0.0]\norigin: [0.0, 0.0, 0."
},
{
"path": "flatland_setup/maps/open_field_3/pedsim_scenario.xml",
"chars": 349,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<scenario>\n\n <waypoint id=\"robot_goal\" x=\"22\" y=\"27\" r=\"2\"/> -->\n <waypoint"
},
{
"path": "flatland_setup/maps/open_field_3/world.yaml",
"chars": 1107,
"preview": "properties: \n # optional, defaults to 10, number of velocity iterations for the Box2D\n # physics solver\n velocity_ite"
},
{
"path": "flatland_setup/objects/cylinder.model.yaml",
"chars": 859,
"preview": "# Round obstacle\n\nbodies: # List of named bodies\n - name: cylinder\n pose: [0, 0, 0] \n type: dynamic \n color:"
},
{
"path": "flatland_setup/objects/palett.model.yaml",
"chars": 1000,
"preview": "bodies: # List of named bodies\n - name: pallet\n pose: [0, 0, 0] \n type: dynamic \n color: [0.2, 0.8, 0.2, 0.7"
},
{
"path": "flatland_setup/objects/person_single_circle.model.yaml",
"chars": 1361,
"preview": "# Person\nbodies: # List of named bodies\n - name: base\n pose: [0, 0, 0] \n type: dynamic \n color: [1.0, 1.0, 1"
},
{
"path": "flatland_setup/objects/person_two_legged.model.yaml",
"chars": 1670,
"preview": "# Person\nbodies: # List of named bodies\n - name: base\n pose: [0, 0, 0] \n type: dynamic \n color: [1.0, 1.0, 1"
},
{
"path": "flatland_setup/objects/wagon.model.yaml",
"chars": 1043,
"preview": "# Round obstacle\n\nbodies: # List of named bodies\n - name: foot1\n pose: [0, 0, 0] \n type: dynamic \n color: [0"
},
{
"path": "flatland_setup/package.xml",
"chars": 2883,
"preview": "<?xml version=\"1.0\"?>\n<package format=\"2\">\n <name>flatland_setup</name>\n <version>0.0.0</version>\n <description>The f"
},
{
"path": "flatland_setup/robot/robot1.model.yaml",
"chars": 1438,
"preview": "bodies: # List of named bodies\n - name: base_footprint\n pose: [0, 0, 0] \n type: dynamic \n color: [0.5, 0.5, "
},
{
"path": "requirements.txt",
"chars": 1630,
"preview": "absl-py==0.7.1\nalabaster==0.7.12\nargh==0.26.2\nastor==0.7.1\natari-py==0.1.7\nattrs==19.1.0\nBabel==2.6.0\ncatkin-pkg==0.4.11"
},
{
"path": "rl_agent/CMakeLists.txt",
"chars": 1818,
"preview": "cmake_minimum_required(VERSION 2.8.3)\nproject(rl_agent)\n\n## Compile as C++11, supported in ROS Kinetic and newer\n# add_c"
},
{
"path": "rl_agent/include/rl_agent/laser_scan_merger.h",
"chars": 1780,
"preview": "/*\n * @name\t \t laser_scan_merger.h\n * @brief\t \t Merges several scans to a single one with coordinate system in robot_f"
},
{
"path": "rl_agent/include/rl_agent/tf_python.h",
"chars": 1007,
"preview": "/**\n * \n * Ronja Gueldenring\n * his class is needed, because rospy and python3.5 is not compatible regarding \n * the tf-"
},
{
"path": "rl_agent/launch/run_1_img_disc.launch",
"chars": 177,
"preview": "<launch>\n <arg name=\"mode\" />\n <node pkg=\"rl_agent\" type=\"run_ppo.py\" name=\"run_ppo.py\" output=\"screen\" args=\"ppo2_1_i"
},
{
"path": "rl_agent/launch/run_1_raw_cont.launch",
"chars": 196,
"preview": "<launch>\n <arg name=\"mode\" />\n <node pkg=\"rl_agent\" type=\"run_ppo.py\" name=\"run_ppo.py\" output=\"screen\" args=\"ppo2_1_r"
},
{
"path": "rl_agent/launch/run_1_raw_disc.launch",
"chars": 196,
"preview": "<launch>\n <arg name=\"mode\" />\n <node pkg=\"rl_agent\" type=\"run_ppo.py\" name=\"run_ppo.py\" output=\"screen\" args=\"ppo2_1_r"
},
{
"path": "rl_agent/launch/run_3_raw_disc.launch",
"chars": 196,
"preview": "<launch>\n <arg name=\"mode\" />\n <node pkg=\"rl_agent\" type=\"run_ppo.py\" name=\"run_ppo.py\" output=\"screen\" args=\"ppo2_3_r"
},
{
"path": "rl_agent/launch/run_ppo_agent.launch",
"chars": 182,
"preview": "<launch>\n <arg name=\"mode\" />\n <node pkg=\"rl_agent\" type=\"run_ppo.py\" name=\"run_ppo.py\" output=\"screen\" args=\"ppo2_foo"
},
{
"path": "rl_agent/package.xml",
"chars": 3418,
"preview": "<?xml version=\"1.0\"?>\n<package format=\"2\">\n <name>rl_agent</name>\n <version>0.0.0</version>\n <description>The rl_agen"
},
{
"path": "rl_agent/scripts/evaluation/analysis.py",
"chars": 3396,
"preview": "'''\n @name: analysis.py\n @brief: Analysis of evaluation data\n @author: Ronja Gueldenring\n @versi"
},
{
"path": "rl_agent/scripts/evaluation/evaluate_agent.py",
"chars": 2468,
"preview": "'''\n @name: evaluate_agent.py\n @brief: Evaluates an agent according to a test set.\n @author: Ronja "
},
{
"path": "rl_agent/scripts/evaluation/qualitative_plot.py",
"chars": 2662,
"preview": "'''\n @name: qualitative_plot.py\n @brief: Generates a situation in rviz, that shows the agents qualitative"
},
{
"path": "rl_agent/scripts/evaluation/save_test_episodes.py",
"chars": 4421,
"preview": "'''\n @name: save_test_episodes.py\n @brief: For saving n different episodes. Each episode needs to be rewi"
},
{
"path": "rl_agent/scripts/run_scripts/run_ppo.py",
"chars": 6076,
"preview": "#! /usr/bin/env python\n'''\n @name: run_ppo.py\n @brief: Trained agent is loaded and executed.\n @author:"
},
{
"path": "rl_agent/scripts/train_scripts/train_ppo.py",
"chars": 11287,
"preview": "'''\n @name: train_ppo.py\n @brief: Starts a ppo2-training process. It is expected that move_base, simulati"
},
{
"path": "rl_agent/setup.py",
"chars": 388,
"preview": "## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD\n\nfrom distutils.core import setup\nfrom catkin_pkg.python_s"
},
{
"path": "rl_agent/src/__init__.py",
"chars": 0,
"preview": ""
},
{
"path": "rl_agent/src/laser_scan_merger.cpp",
"chars": 4788,
"preview": "/*\n * @name\t \tlaser_scan_merger.cpp\n * @brief\t \tMerges several scans to a single one with coordinate system in robot_fra"
},
{
"path": "rl_agent/src/rl_agent/__init__.py",
"chars": 0,
"preview": ""
},
{
"path": "rl_agent/src/rl_agent/common_custom_policies.py",
"chars": 7923,
"preview": "'''\n @name: state_collector.py\n @brief: This class provides custom policies used in stable-baselines libr"
},
{
"path": "rl_agent/src/rl_agent/env_utils/__init__.py",
"chars": 0,
"preview": ""
},
{
"path": "rl_agent/src/rl_agent/env_utils/debug_ros_env.py",
"chars": 5661,
"preview": "'''\n @name: debug_ros_env.py\n @brief: This class provides debugging methods RL-relevant data.\n @author"
},
{
"path": "rl_agent/src/rl_agent/env_utils/reward_container.py",
"chars": 13861,
"preview": "'''\n @name: ros_env.py\n @brief: This class provides different reward functions and parametrizes them.\n "
},
{
"path": "rl_agent/src/rl_agent/env_utils/state_collector.py",
"chars": 8407,
"preview": "'''\n @name: state_collector.py\n @brief: This class collects most recent relevant state data of the enviro"
},
{
"path": "rl_agent/src/rl_agent/env_utils/task_generator.py",
"chars": 35362,
"preview": "'''\n @name: task_generator.py\n @brief: This class generates random tasks for training.\n @author: Ro"
},
{
"path": "rl_agent/src/rl_agent/env_wrapper/__init__.py",
"chars": 0,
"preview": ""
},
{
"path": "rl_agent/src/rl_agent/env_wrapper/ros_env.py",
"chars": 13862,
"preview": "'''\n @name: ros_env.py\n @brief: This (abstract) class is a simulation environment wrapper.\n "
},
{
"path": "rl_agent/src/rl_agent/env_wrapper/ros_env_cont_img.py",
"chars": 1873,
"preview": "'''\n @name: ros_env_cont_img.py\n @brief: This class is a simulation environment wrapper for\n "
},
{
"path": "rl_agent/src/rl_agent/env_wrapper/ros_env_cont_img_vel.py",
"chars": 1823,
"preview": "'''\n @name: ros_env_cont_img_vel.py\n @brief: This class is a simulation environment wrapper for\n "
},
{
"path": "rl_agent/src/rl_agent/env_wrapper/ros_env_cont_raw_data.py",
"chars": 1685,
"preview": "'''\n @name: ros_env_cont_raw_data.py\n @brief: This class is a simulation environment wrapper for\n "
},
{
"path": "rl_agent/src/rl_agent/env_wrapper/ros_env_cont_raw_scan_prep_wp.py",
"chars": 1690,
"preview": "'''\n @name: ros_env_cont_raw_scan_prep_wo.py\n @brief: This class is a simulation environment wrapper for\n"
},
{
"path": "rl_agent/src/rl_agent/env_wrapper/ros_env_disc_img.py",
"chars": 2501,
"preview": "'''\n @name: ros_env_cont_img.py\n @brief: This class is a simulation environment wrapper for\n "
},
{
"path": "rl_agent/src/rl_agent/env_wrapper/ros_env_disc_img_vel.py",
"chars": 2447,
"preview": "'''\n @name: ros_env_cont_img_vel.py\n @brief: This class is a simulation environment wrapper for\n "
},
{
"path": "rl_agent/src/rl_agent/env_wrapper/ros_env_disc_raw_data.py",
"chars": 2398,
"preview": "'''\n @name: ros_env_cont_raw_data.py\n @brief: This class is a simulation environment wrapper for\n "
},
{
"path": "rl_agent/src/rl_agent/env_wrapper/ros_env_disc_raw_scan_prep_wp.py",
"chars": 2332,
"preview": "'''\n @name: ros_env_disc_raw_scan_prep_wo.py\n @brief: This class is a simulation environment wrapper for\n"
},
{
"path": "rl_agent/src/rl_agent/env_wrapper/ros_env_img.py",
"chars": 1523,
"preview": "'''\n @name: ros_env_img.py\n @brief: This (abstract) class is a simulation environment wrapper for\n "
},
{
"path": "rl_agent/src/rl_agent/env_wrapper/ros_env_img_vel.py",
"chars": 1775,
"preview": "'''\n @name: ros_env_img_vel.py\n @brief: This (abstract) class is a simulation environment wrapper for\n "
},
{
"path": "rl_agent/src/rl_agent/env_wrapper/ros_env_raw_data.py",
"chars": 2346,
"preview": "'''\n @name: ros_env_raw_data.py\n @brief: This (abstract) class is a simulation environment wrapper for\n "
},
{
"path": "rl_agent/src/rl_agent/env_wrapper/ros_env_raw_scan_prep_wp.py",
"chars": 2765,
"preview": "'''\n @name: ros_env_raw_scan_prep_wo.py\n @brief: This class is a simulation environment wrapper for\n "
},
{
"path": "rl_agent/src/rl_agent/evaluation/Analysis_eval.py",
"chars": 7306,
"preview": "'''\n @name: Analysis_eval.py\n @brief: The class can be used to analyse the produced evaluation_data gener"
},
{
"path": "rl_agent/src/rl_agent/evaluation/Evaluation.py",
"chars": 16850,
"preview": "'''\n @name: Analysis_eval.py\n @brief: The class records relevant data of the agent during driving. The da"
},
{
"path": "rl_agent/src/rl_agent/evaluation/__init__.py",
"chars": 0,
"preview": ""
},
{
"path": "rl_agent/src/tf_python.cpp",
"chars": 1838,
"preview": "/**\n * \n * Ronja Gueldenring\n * This class is needed, because rospy and python3.5 is not compatible regarding \n * the tf"
},
{
"path": "rl_bringup/CMakeLists.txt",
"chars": 693,
"preview": "cmake_minimum_required(VERSION 2.8.3)\nproject(rl_bringup)\n\nset(PACKAGE_DEPS\n roscpp\n rospy\n std_msgs\n nav_msgs\n tf\n"
},
{
"path": "rl_bringup/config/costmap_common_params.yaml",
"chars": 727,
"preview": "obstacle_range: 3.0\nraytrace_range: 6.0\nrobot_radius: 0.46\ninflation_radius: 0.6\nstatic_laser: {clearing: true, data_typ"
},
{
"path": "rl_bringup/config/dwa_params.yaml",
"chars": 575,
"preview": "DWAPlannerROS:\n max_trans_vel: 0.8 \n min_trans_vel: 0.05 \n\n max_vel_x: 0.8 \n min_vel_x: 0.05 \n\n max"
},
{
"path": "rl_bringup/config/global_costmap_params.yaml",
"chars": 138,
"preview": "global_costmap:\n global_frame: /map\n robot_base_frame: base_footprint\n update_frequency: 5\n static_map: true\n publi"
},
{
"path": "rl_bringup/config/local_costmap_params.yaml",
"chars": 208,
"preview": "local_costmap:\n global_frame: odom\n robot_base_frame: base_footprint\n update_frequency: 5.0\n publish_frequency: 2.0\n"
},
{
"path": "rl_bringup/config/movebase_params.yaml",
"chars": 146,
"preview": "\nbase_local_planner: rl_local_planner/RLLocalPlanner #dwa_local_planner/DWAPlannerROS \nbase_global_planner: navfn/NavfnR"
},
{
"path": "rl_bringup/config/path_config.ini",
"chars": 455,
"preview": "[PATHES]\npath_to_venv=/home/ronja/venv_p3\npath_to_train_data=/home/ronja/database\npath_to_eval_data_train=/home/ronja/da"
},
{
"path": "rl_bringup/config/path_config_docker.ini",
"chars": 347,
"preview": "[PATHES]\npath_to_venv=/venv_p3\npath_to_train_data=/data\npath_to_eval_data_train=/data/evaluation_data/train\npath_to_eval"
},
{
"path": "rl_bringup/config/rl_common.yaml",
"chars": 517,
"preview": "rl_agent:\n f_scan_topic: f_scan\n b_scan_topic: b_scan\n robot_frame: base_footprint\n \n train_mode: 1 # 0: Execution "
},
{
"path": "rl_bringup/config/rl_params_img.yaml",
"chars": 135,
"preview": "rl_agent:\n state_mode: 0 # 0: image, 1: laserscan\n \n img_width_pos: 70\n img_width_neg: 20 \n img_height: 70 \n reso"
},
{
"path": "rl_bringup/config/rl_params_scan.yaml",
"chars": 95,
"preview": "rl_agent:\n state_mode: 1 # 0: image, 1: laserscan\n \n resolution: 0.05\n scan_size: 90 \n\n\n \n"
},
{
"path": "rl_bringup/launch/rviz.launch",
"chars": 4906,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n <arg name=\"error_log\" default=\"\"/>\n <arg name=\"rviz_config\" default=\"$(find rl"
},
{
"path": "rl_bringup/launch/setup.launch",
"chars": 2293,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n <param name=\"use_sim_time\" value=\"true\"/>\n <arg name=\"ns\" default=\"\"/>\n <arg na"
},
{
"path": "rl_bringup/launch/sub_launch/dummy_localization.launch",
"chars": 275,
"preview": "<launch>\n\t<arg name=\"ns\"/> \n\t<!-- odom frame == map frame-->\n\t<node pkg=\"tf\" type=\"static_transform_publisher\" name=\"lin"
},
{
"path": "rl_bringup/launch/sub_launch/flatland.launch",
"chars": 1985,
"preview": "<launch>\n\t<arg name=\"ns\"/> \n <arg name=\"map_path\"/> \n <arg name=\"update_rate\"/>\n\n\n <!-- ******************** flatlan"
},
{
"path": "rl_bringup/launch/sub_launch/move_base.launch",
"chars": 807,
"preview": "<launch>\n\t<arg name=\"ns\"/>\n\n <node pkg=\"move_base\" type=\"move_base\" respawn=\"false\" name=\"move_base\" output=\"screen\">\n"
},
{
"path": "rl_bringup/launch/sub_launch/navigation.launch",
"chars": 697,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n\t<arg name=\"ns\"/> \n\t<arg name=\"map_path\"/> \n\n\t<!-- Load map -->\n <node pkg=\"map_server"
},
{
"path": "rl_bringup/launch/sub_launch/pedestrians_only.launch",
"chars": 1321,
"preview": "<launch>\n <arg name=\"map_path\"/> \n\t<arg name=\"ns\" default=\"\"/> \n <arg name=\"kbd_teleop\" default=\"false\"/>\n <arg name="
},
{
"path": "rl_bringup/launch/sub_launch/pedsim_visualizer.launch",
"chars": 245,
"preview": "<launch>\n\t<arg name=\"ns\"/> \n\n <node name=\"pedsim_visualizer\" type=\"pedsim_visualizer_node\" pkg=\"pedsim_visualizer\" outp"
},
{
"path": "rl_bringup/launch/sub_launch/simulation.launch",
"chars": 653,
"preview": "<launch>\n\t<arg name=\"ns\"/>\n <arg name=\"map_path\"/> \n <arg name=\"update_rate\"/> \n\n <param name=\"use_sim_time\" va"
},
{
"path": "rl_bringup/package.xml",
"chars": 3265,
"preview": "<?xml version=\"1.0\"?>\n<package format=\"2\">\n <name>rl_bringup</name>\n <version>0.0.0</version>\n <description>The rl_br"
},
{
"path": "rl_bringup/rviz/robot_navigation.rviz",
"chars": 17594,
"preview": "Panels:\n - Class: rviz/Displays\n Help Height: 0\n Name: Displays\n Property Tree Widget:\n Expanded:\n "
},
{
"path": "rl_bringup/src/toggle_setup_init.cpp",
"chars": 1039,
"preview": " /*\n * @name\t \t toggle_setup_init.cpp\n * @brief\t \t Simulation will be triggered for n_sec for initialize setup.\n * @au"
},
{
"path": "rl_local_planner/CMakeLists.txt",
"chars": 2162,
"preview": "cmake_minimum_required(VERSION 2.8.3)\nproject(rl_local_planner)\n\nfind_package(catkin REQUIRED COMPONENTS\n roscpp\n rosp"
},
{
"path": "rl_local_planner/blp_plugin.xml",
"chars": 308,
"preview": "<library path=\"lib/librlLocalPlanner\">\n <class name=\"rl_local_planner/RLLocalPlanner\" type=\"rl_local_planner::RLLocalPl"
},
{
"path": "rl_local_planner/include/rl_local_planner/image_generator.h",
"chars": 3912,
"preview": "/*\n * @name\t \t image_generator.h\n * @brief\t \t An image is generated from laserscan data and waypoints on the path\n * @"
},
{
"path": "rl_local_planner/include/rl_local_planner/rl_local_planner.h",
"chars": 3922,
"preview": "/*\n * @name\t \t rl_local_planner.h\n * @brief\t \t Connector of move_base and rl_agent using RL library. Forwards action o"
},
{
"path": "rl_local_planner/include/rl_local_planner/wp_generator.h",
"chars": 2893,
"preview": "/*\n * @name\t \twp_generator.h\n * @brief\t \tGeneration of waypoints on the global path and \n * \t\t\t\t \tdetermination of the c"
},
{
"path": "rl_local_planner/package.xml",
"chars": 2517,
"preview": "<?xml version=\"1.0\"?>\n<package format=\"2\">\n <name>rl_local_planner</name>\n <version>0.0.0</version>\n <description>The"
},
{
"path": "rl_local_planner/src/image_generator.cpp",
"chars": 9142,
"preview": "/*\n * @name\t \timage_generator.cpp\n * @brief\t \tAn image is generated from laserscan data and waypoints on the path\n * @au"
},
{
"path": "rl_local_planner/src/rl_local_planner.cpp",
"chars": 3817,
"preview": "/*\n * @name\t \trl_local_planner.cpp\n * @brief\t \tConnector of move_base and rl_agent using RL library. Forwards action of "
},
{
"path": "rl_local_planner/src/wp_generator.cpp",
"chars": 5183,
"preview": "/*\n * @name\t \twp_generator.cpp\n * @brief\t \tGeneration of waypoints on the global path and \n * \t\t\t\tdetermination of the c"
},
{
"path": "rl_msgs/CMakeLists.txt",
"chars": 1097,
"preview": "cmake_minimum_required(VERSION 2.8.3)\nproject(rl_msgs)\n\n# Ensure we're using c++11\nset(CMAKE_CXX_FLAGS \"${CMAKE_CXX_FLAG"
},
{
"path": "rl_msgs/msg/Waypoint.msg",
"chars": 72,
"preview": "std_msgs/Header header\ngeometry_msgs/Point[] points\nstd_msgs/Bool is_new"
},
{
"path": "rl_msgs/package.xml",
"chars": 682,
"preview": "<?xml version=\"1.0\"?>\n<package format=\"2\">\n <name>rl_msgs</name>\n <version>1.1.1</version>\n <description>The rl_msgs "
},
{
"path": "rl_msgs/srv/MergeScans.srv",
"chars": 68,
"preview": "sensor_msgs/LaserScan[] scans\n---\nsensor_msgs/LaserScan merged_scan\n"
},
{
"path": "rl_msgs/srv/SetPath.srv",
"chars": 19,
"preview": "nav_msgs/Path path\n"
},
{
"path": "rl_msgs/srv/StateImageGenerationSrv.srv",
"chars": 80,
"preview": "sensor_msgs/LaserScan scan\nrl_msgs/Waypoint wps\n\n---\nnav_msgs/OccupancyGrid img\n"
},
{
"path": "start_scripts/entrypoint_ppo2.sh",
"chars": 2863,
"preview": "#! /bin/bash\n#get config-params\npath_to_venv=$(awk -F \"=\" '/path_to_venv/ {print $2}' ../rl_bringup/config/path_config.i"
},
{
"path": "start_scripts/ppo2_training.sh",
"chars": 1191,
"preview": "#! /bin/sh\n\n#get config-params\npath_to_venv=$(awk -F \"=\" '/path_to_venv/ {print $2}' ../rl_bringup/config/path_config.in"
},
{
"path": "start_scripts/ros.sh",
"chars": 964,
"preview": "#! /bin/sh\nsim_name=$1\npolicy=$2\nmap=$3\n\n#get config-params\npath_to_catkin_ws=$(awk -F \"=\" '/path_to_catkin_ws/ {print $"
},
{
"path": "start_scripts/start_roscore.sh",
"chars": 326,
"preview": "#! /bin/sh\n\n#get config-params\npath_to_catkin_ws=$(awk -F \"=\" '/path_to_catkin_ws/ {print $2}' ../rl_bringup/config/path"
},
{
"path": "start_scripts/training_params/ppo2_params.csv",
"chars": 1429,
"preview": "agent_name, total_timesteps, policy, gamma, n_steps, ent_coef, learning_rate, vf_coef, max_grad_norm,lam,nminibatches,no"
},
{
"path": "start_scripts/training_params/training_maps.csv",
"chars": 50,
"preview": "open_field_1\ncorridor_1\ncorridor_3\ncomplex_map_1\n\n"
}
]
// ... and 4 more files (download for full content)
About this extraction
This page contains the full source code of the RGring/drl_local_planner_ros_stable_baselines GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 137 files (307.1 KB), approximately 88.5k tokens, and a symbol index with 214 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.