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 /src/drl_local_planner_ros_stable_baselines cp .rosinstall ../ cd .. rosws update cd 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 /venv_p3 --python=python3 source /venv_p3/bin/activate /venv_p3/bin/pip install \ pyyaml \ rospkg \ catkin_pkg \ exception \ numpy \ tensorflow=="1.13.1" \ gym \ pyquaternion \ mpi4py \ matplotlib cd /src/drl_local_planner_forks/stable_baselines/ /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 /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 /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. ``` /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 /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 /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 /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 /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 :/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 :/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 ================================================ --> ================================================ 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 ================================================ --> ================================================ 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 ================================================ --> ================================================ 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 ================================================ --> ================================================ 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 ================================================ --> ================================================ 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 ================================================ --> ================================================ 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 ================================================ --> ================================================ 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 ================================================ --> ================================================ 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 ================================================ --> ================================================ 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 ================================================ ================================================ 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 ================================================ --> ================================================ 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 ================================================ flatland_setup 0.0.0 The flatland_setup package ronja TODO catkin roscpp rospy std_msgs roscpp rospy std_msgs roscpp rospy std_msgs ================================================ 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 #include #include #include #include #include #include #include #include #include #include #include #include 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 #include #include 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 ================================================ ================================================ FILE: rl_agent/launch/run_1_raw_cont.launch ================================================ ================================================ FILE: rl_agent/launch/run_1_raw_disc.launch ================================================ ================================================ FILE: rl_agent/launch/run_3_raw_disc.launch ================================================ ================================================ FILE: rl_agent/launch/run_ppo_agent.launch ================================================ ================================================ FILE: rl_agent/package.xml ================================================ rl_agent 0.0.0 The rl_agent package ronja TODO catkin rospy roscpp std_msgs rl_msgs sensor_msgs geometry_msgs message_generation stable_baselines rospy roscpp std_msgs geometry_msgs sensor_msgs rospy roscpp std_msgs rl_msgs message_runtime geometry_msgs sensor_msgs stable_baselines ================================================ 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 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; iheader = 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; irange_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 ================================================ FILE: rl_agent/src/rl_agent/env_wrapper/ros_env_disc_img.py ================================================ ''' @name: ros_env_cont_img.py @brief: This class is a simulation environment wrapper for the X-Image Representation with discrente 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 ACTION_SIZE = 6 GOAL_RADIUS = 0.4 WAYPOINT_RADIUS = 0.2 class RosEnvDiscImg(RosEnvImg): ''' This class is a simulation environment wrapper for the X-Image Representation with discrente action space. ''' def __init__(self, ns, state_collector, stack_offset, stack_size, robot_radius, reward_fnc, debug, 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) self.action = np.array([0.0, 0.0]) self.v_max_ = 0.8 # ?1.5? self.w_max_ = 1.2 self.__possible_actions = { 0: [0.0, -self.w_max_], 1: [self.v_max_, 0.0], 2: [0.0, self.w_max_], 3: [self.v_max_, self.w_max_/2], 4: [self.v_max_, -self.w_max_/2], 5: [0.0, 0.0], } action_size = len(self.__possible_actions) action_space = spaces.Discrete(action_size) super(RosEnvDiscImg, self).__init__(ns, state_collector, execution_mode, task_mode, state_size, observation_space, stack_offset, action_size, action_space, debug, GOAL_RADIUS, WAYPOINT_RADIUS, robot_radius, reward_fnc) def get_cmd_vel_(self, action): encoded_action = self.__encode_action(action) vel_msg = Twist() vel_msg.linear.x = encoded_action[0] vel_msg.angular.z = encoded_action[1] return vel_msg def __encode_action(self, action): return self.__possible_actions.get(action, 5) def get_action_list(self): action_list = [] for i in range(self.ACTION_SIZE): action_list.append(self.__encode_action(i)) return action_list ================================================ FILE: rl_agent/src/rl_agent/env_wrapper/ros_env_disc_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 discrete 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 RosEnvDiscImgVel(RosEnvImgVel): ''' This class is a simulation environment wrapper for the X-Image Speed Representation with discrete 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) self.v_max_ = 0.8 # ?1.5? self.w_max_ = 1.2 self.__possible_actions = { 0: [0.0, -self.w_max_], 1: [self.v_max_, 0.0], 2: [0.0, self.w_max_], 3: [self.v_max_, self.w_max_/2], 4: [self.v_max_, -self.w_max_/2], 5: [0.0, 0.0] # 6: [0.09, 0.0] } action_size = len(self.__possible_actions) action_space = spaces.Discrete(action_size) super(RosEnvDiscImgVel, self).__init__(ns, state_collector, execution_mode, task_mode, state_size, observation_space, stack_offset, action_size, action_space, debug, GOAL_RADIUS, WAYPOINT_RADIUS, robot_radius, reward_fnc) def get_cmd_vel_(self, action): encoded_action = self.__encode_action(action) vel_msg = Twist() vel_msg.linear.x = encoded_action[0] vel_msg.angular.z = encoded_action[1] return vel_msg def __encode_action(self, action): return self.__possible_actions.get(action, 5) def get_action_list(self): action_list = [] for i in range(self.ACTION_SIZE): action_list.append(self.__encode_action(i)) return action_list ================================================ FILE: rl_agent/src/rl_agent/env_wrapper/ros_env_disc_raw_data.py ================================================ ''' @name: ros_env_cont_raw_data.py @brief: This class is a simulation environment wrapper for the Raw Representation with discrete action space. @author: Ronja Gueldenring @version: 3.5 @date: 2019/04/05 ''' # python relevant import numpy as np from gym import spaces import rospy # 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 RosEnvDiscRaw(RosEnvRaw): ''' This class is a simulation environment wrapper for the Raw Representation with discrete 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=10, shape=state_size, dtype=np.float) self.action = np.array([0.0, 0.0]) self.v_max_ = 0.8 # ?1.5? self.w_max_ = 1.2 self.__possible_actions = { 0: [0.0, -self.w_max_], 1: [self.v_max_, 0.0], 2: [0.0, self.w_max_], 3: [self.v_max_, self.w_max_ / 2], 4: [self.v_max_, -self.w_max_ / 2], 5: [0.0, 0.0] # 6: [0.09, 0.0], } action_size = len(self.__possible_actions) action_space = spaces.Discrete(action_size) super(RosEnvDiscRaw, self).__init__(ns, state_collector, execution_mode, task_mode, state_size, observation_space, stack_offset, action_size, action_space, debug, GOAL_RADIUS, WAYPOINT_RADIUS, robot_radius, reward_fnc) self.action = np.array([0.0, 0.0]) def get_cmd_vel_(self, action): encoded_action = self.__encode_action(action) vel_msg = Twist() vel_msg.linear.x = encoded_action[0] vel_msg.angular.z = encoded_action[1] return vel_msg def __encode_action(self, action): return self.__possible_actions.get(action, 5) def get_action_list(self): action_list = [] for i in range(self.ACTION_SIZE): action_list.append(self.__encode_action(i)) return action_list ================================================ FILE: rl_agent/src/rl_agent/env_wrapper/ros_env_disc_raw_scan_prep_wp.py ================================================ ''' @name: ros_env_disc_raw_scan_prep_wo.py @brief: This class is a simulation environment wrapper for the Polar Representation with discrete action space @author: Ronja Gueldenring @version: 3.5 @date: 2019/04/05 ''' # python relevant import numpy as np from gym import spaces import rospy # 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 RosEnvDiscRawScanPrepWp(RosEnvRawScanPrepWp): ''' This class is a simulation environment wrapper for the Polar Representation with discrete 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=10, shape=state_size, dtype=np.float) self.v_max_ = 0.8 # ?1.5? self.w_max_ = 1.2 self.__possible_actions = { 0: [0.0, -self.w_max_], 1: [self.v_max_, 0.0], 2: [0.0, self.w_max_], 3: [self.v_max_, self.w_max_ / 2], 4: [self.v_max_, -self.w_max_ / 2], 5: [0.0, 0.0], } action_size = len(self.__possible_actions) action_space = spaces.Discrete(action_size) super(RosEnvDiscRawScanPrepWp, self).__init__(ns, state_collector, execution_mode, task_mode, state_size, observation_space, stack_offset, action_size, action_space, debug, GOAL_RADIUS, WAYPOINT_RADIUS, robot_radius, reward_fnc) self.action = np.array([0.0, 0.0]) def get_cmd_vel_(self, action): encoded_action = self.__encode_action(action) vel_msg = Twist() vel_msg.linear.x = encoded_action[0] vel_msg.angular.z = encoded_action[1] return vel_msg def __encode_action(self, action): return self.__possible_actions.get(action, 5) def get_action_list(self): action_list = [] for i in range(self.ACTION_SIZE): action_list.append(self.__encode_action(i)) return action_list ================================================ FILE: rl_agent/src/rl_agent/env_wrapper/ros_env_img.py ================================================ ''' @name: ros_env_img.py @brief: This (abstract) class is a simulation environment wrapper for the X-Image Representation. @author: Ronja Gueldenring @version: 3.5 @date: 2019/04/05 ''' # python relevant import numpy as np # custom classes from rl_agent.env_wrapper.ros_env import RosEnvAbs # ros-relevant import rospy class RosEnvImg(RosEnvAbs): ''' This (abstract) class is a simulation environment wrapper for the X-Image Representation. ''' 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): state_collector.set_state_mode(0) super(RosEnvImg, self).__init__(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) def get_observation_(self): """ Function returns state that will be fed to the rl-agent It includes the laserscan and the waypoint information stored in an image. :return: state """ obs = np.zeros(self.STATE_SIZE, dtype=np.float) obs[:,:,0] = np.array(self.input_img_.data).reshape((self.STATE_SIZE[0:2])) if self.debug_: self.debugger_.show_input_occ_grid(self.input_img_) self.debugger_.show_input_image(obs[:,:,0]) return obs ================================================ FILE: rl_agent/src/rl_agent/env_wrapper/ros_env_img_vel.py ================================================ ''' @name: ros_env_img_vel.py @brief: This (abstract) class is a simulation environment wrapper for the X-Image Speed Representation. @author: Ronja Gueldenring @version: 3.5 @date: 2019/04/05 ''' # python relevant import numpy as np # custom classes from rl_agent.env_wrapper.ros_env import RosEnvAbs # ros-relevant import rospy class RosEnvImgVel(RosEnvAbs): ''' This (abstract) class is a simulation environment wrapper for the X-Image Speed Representation. ''' 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): state_collector.set_state_mode(0) super(RosEnvImgVel, self).__init__(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) def get_observation_(self): """ Function returns state that will be fed to the rl-agent It includes the laserscan and waypoint data in form of an image, the last speed of the robot, that is raw and stored in the last row in the state matrix. :return: state """ obs = np.zeros(self.STATE_SIZE, dtype=np.float) recent_img = np.array(self.input_img_.data).reshape((self.STATE_SIZE[0] - 1, self.STATE_SIZE[1])) obs[0:-1,:,0] = recent_img obs[-1, 0:2, 0] = [self.twist_.twist.linear.x, self.twist_.twist.angular.z] if self.debug_: self.debugger_.show_input_occ_grid(self.input_img_) self.debugger_.show_image_stack(obs[:-1, :, :]) return obs ================================================ FILE: rl_agent/src/rl_agent/env_wrapper/ros_env_raw_data.py ================================================ ''' @name: ros_env_raw_data.py @brief: This (abstract) class is a simulation environment wrapper for the Raw Representation. @author: Ronja Gueldenring @version: 3.5 @date: 2019/04/05 ''' # python relevant import numpy as np import rospy # custom classes from rl_agent.env_wrapper.ros_env import RosEnvAbs from sensor_msgs.msg import LaserScan class RosEnvRaw(RosEnvAbs): ''' This (abstract) class is a simulation environment wrapper for the Raw Representation. ''' 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): state_collector.set_state_mode(2) super(RosEnvRaw, self).__init__(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) self.__scan_size = rospy.get_param("%s/rl_agent/scan_size"%ns) def get_observation_(self): """ Function returns state that will be fed to the rl-agent It includes the raw laser scan data, the raw waypoint data. :return: state """ waypoint = self.wp_ num_of_wps = len(waypoint.points) state = np.ones(self.STATE_SIZE, dtype=np.float) # add laserscan state[0, 0:self.__scan_size, 0] = self.merged_scan_.ranges # add goal position wp_index = self.STATE_SIZE[1] - num_of_wps * 2 for i in range(num_of_wps): state[0, (wp_index + i*2):(wp_index + i*2 + 2),0] = [waypoint.points[i].x, waypoint.points[i].y] # Discretize to a resolution of 5cm. state = np.round(np.divide(state, 0.05))*0.05 if self.debug_: debug_scan = LaserScan() debug_scan.header = self.merged_scan_.header debug_scan.angle_min = self.merged_scan_.angle_min debug_scan.angle_max = self.merged_scan_.angle_max debug_scan.angle_increment = self.merged_scan_.angle_increment debug_scan.range_max = 7.0 debug_scan.ranges = state[0, 0:self.__scan_size, 0] self.debugger_.show_input_scan(debug_scan) return state ================================================ FILE: rl_agent/src/rl_agent/env_wrapper/ros_env_raw_scan_prep_wp.py ================================================ ''' @name: ros_env_raw_scan_prep_wo.py @brief: This class is a simulation environment wrapper for the Polar Representation. @author: Ronja Gueldenring @version: 3.5 @date: 2019/04/05 ''' # python relevant import numpy as np import math # ros-relevant import rospy # custom classes from rl_agent.env_wrapper.ros_env import RosEnvAbs from sensor_msgs.msg import LaserScan class RosEnvRawScanPrepWp(RosEnvAbs): ''' This class is a simulation environment wrapper for the Polar Representation. ''' 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): state_collector.set_state_mode(2) super(RosEnvRawScanPrepWp, self).__init__(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) self.__res = rospy.get_param("%s/rl_agent/resolution"%ns) def get_observation_(self): """ Function returns state that will be fed to the rl-agent It includes the raw laser scan data, the waypoint data in with the same format as the laser scan data. The distance of the waypoint is saved at the appropriate angle position in the vector. :return: state """ waypoint = self.wp_ num_of_wps = len(waypoint.points) state = np.ones(self.STATE_SIZE, dtype=np.float) # add laserscan state[ :, 0, 0] = self.merged_scan_.ranges # generate wp-vector wp_vector = np.zeros(self.STATE_SIZE[0]) for i in range(num_of_wps): dist = math.sqrt(math.pow(waypoint.points[i].x, 2) + math.pow(waypoint.points[i].y, 2)) angle = math.atan2(waypoint.points[i].y, waypoint.points[i].x) + math.pi wp_vector[math.floor(angle/self.merged_scan_.angle_increment)] = dist state[:,1,0] = wp_vector # Discretize to a resolution of 5cm. state = np.round(np.divide(state, self.__res))*self.__res if self.debug_: debug_scan = LaserScan() # debug_scan.header.frame_id = self.merged_scan_.header.frame_id debug_scan.header = self.merged_scan_.header debug_scan.angle_min = self.merged_scan_.angle_min debug_scan.angle_max = self.merged_scan_.angle_max debug_scan.angle_increment = self.merged_scan_.angle_increment debug_scan.range_max = 7.0 debug_scan.ranges = state[:, 0, 0] self.debugger_.show_scan_stack(debug_scan) return state ================================================ FILE: rl_agent/src/rl_agent/evaluation/Analysis_eval.py ================================================ ''' @name: Analysis_eval.py @brief: The class can be used to analyse the produced evaluation_data generated by Evaluation.py @author: Ronja Gueldenring @version: 3.5 @date: 2019/04/05 ''' import rospkg import pickle import math import csv class Analysis(): def __init__(self): rospack = rospkg.RosPack() self.__rl_agent_path = rospack.get_path('rl_agent') # absolute path rl_agent-package def load_results(self, filename): results = [] with open('%s.pickle'%(filename), 'rb') as fr: try: while True: d = pickle.load(fr) results.append(d) except EOFError: pass return results def get_timestep_list(self, results): timesteps = [] for result in results: timesteps.append(result["timestep"]) return timesteps def reconstruct_timestep_array(self, timesteps): previous_ts = timesteps[0] break_indices = [] for i, ts in enumerate(timesteps): if (previous_ts - ts) >1000: break_indices.append(i) previous_ts = ts break_indices.append((len(timesteps))) for k in range(len(break_indices)-1): base_timestep = timesteps[break_indices[k]-1] for j in range(break_indices[k], break_indices[k+1]): timesteps[j] = base_timestep + timesteps[j] return timesteps def get_scores(self, results): """ Analyses the success rate for all n episodes in results. :param results: list of n results :returns: success-rate, time_exceeded-rate, collision-rate (all lists) """ success_vec = [] time_exceeded_vec = [] collision_vec = [] for i in range(0, len(results)): exit = results[i]["success"] collision = 0 time_exceeded = 0 success = 0 if exit == -1: collision = 1 elif exit == 0: time_exceeded = 1 elif exit == 1: success = 1 success_vec.append(success) time_exceeded_vec.append(time_exceeded) collision_vec.append(collision) return success_vec, time_exceeded_vec, collision_vec def get_percentual_success_drive(self, results): """ Computes the percentage of successful path following for each episode :param results: list of n results :return list of percentages of successful path following """ percentage_vec = [] for result in results: if len(result["global_path"].poses) == 0: percentage_vec.append(1) else: if result["success"] == 1: percentage_vec.append(1) else: i = self.__closest_path_pos(result["global_path"], result["driven_path"].poses[-1].pose) percentage_vec.append(i / len(result["global_path"].poses)) return percentage_vec def get_path_length_ratio(self, results): """ Computes the path length ratio (driven path/global path) :param results: list of n results :return list of the path length ratios """ path_length_ratio_vec = [] for result in results: if len(result["global_path"].poses) == 0: path_length_ratio_vec.append(1) else: i = len(result["global_path"].poses) if result["success"] != 1: i = self.__closest_path_pos(result["global_path"], result["driven_path"].poses[-1].pose) global_path_length = self.__get_path_length(result["global_path"].poses[:i]) driven_path_length = self.__get_path_length(result["driven_path"].poses) if global_path_length <= 0: path_length_ratio_vec.append(1) else: path_length_ratio_vec.append(driven_path_length/global_path_length) return path_length_ratio_vec def get_speed(self, results): """ Computes the average speed for each episode :param results: list of n results :return list of the average speed """ speed_vec = [] for result in results: if len(result["global_path"].poses) == 0: speed_vec.append(1.0) else: i = len(result["global_path"].poses) if (result["success"] != 1): i = self.__closest_path_pos(result["global_path"], result["driven_path"].poses[-1].pose) path_length = self.__get_path_length(result["global_path"].poses[:i]) speed = path_length/result["time"].to_sec() # Handling outliers if (speed > 1.0): speed_vec.append(1.0) else: speed_vec.append(path_length/result["time"].to_sec()) return speed_vec def get_reward(self, filename): """ Getter for the reward for each episode :param results: list of n results :param filename: filename of the .csv-file, where the reward is saved. :return list of the rewards """ timesteps = [] reward = [] line_count = 0 with open('%s.csv'%(filename), 'r') as csvfile: reader = csv.reader(csvfile, delimiter=',') for row in reader: if (line_count == 0): line_count += 1 continue timesteps.append(float(row[1])) reward.append(float(row[2])) return [timesteps, reward] def __closest_path_pos(self, path, pose): """ Computes the path_pose on the path that is closest to pose :param path :param pose :return path_pose """ index = 0 count = 0 dist = 10 for path_pose in path.poses: temp_dist = self.__mean_square_dist((pose.position.x - path_pose.pose.position.x), (pose.position.y - path_pose.pose.position.y)) if (temp_dist < dist): dist = temp_dist index = count count += 1 if (dist > 3): index = 0 return index def __mean_square_dist(self, x, y): """ Computes the mean-square distance :param x :param y :return sqrt(x^2 + y^2) """ return math.sqrt(math.pow(x, 2) + math.pow(y, 2)) def __get_path_length(self, poses): """ Computes the length of a path :param poses: poses on the path :return length of the path """ if len(poses) <= 1: return 0 path_length = 0 old_pose =poses[0].pose.position for pose in poses[1:]: new_pose = pose.pose.position path_length += self.__mean_square_dist((new_pose.x - old_pose.x), (new_pose.y - old_pose.y)) old_pose = new_pose return path_length ================================================ FILE: rl_agent/src/rl_agent/evaluation/Evaluation.py ================================================ ''' @name: Analysis_eval.py @brief: The class records relevant data of the agent during driving. The data can be later used for analysing the training process. @author: Ronja Gueldenring @version: 3.5 @date: 2019/04/05 ''' import rospkg import rospy import numpy as np from rl_agent.env_utils.task_generator import TaskGenerator from nav_msgs.msg import Odometry, Path from sensor_msgs.msg import LaserScan from std_msgs.msg import Bool import pickle import time import math from geometry_msgs.msg import PoseStamped, Twist, Point from flatland_msgs.msg import DebugTopicList from rosgraph_msgs.msg import Clock from pedsim_msgs.msg import AgentStates from visualization_msgs.msg import MarkerArray, Marker class Evaluation(): ''' The class records relevant data of the agent during driving. The data can be later used for analyising the training process. ''' def __init__(self, state_collector, ns, robot_radius = 0,robot_width = 0.58, robot_height = 0.89): self.__robot_radius = robot_radius # robot radius self.__robot_height = robot_height # robot width self.__robot_width = robot_width # robot heigth self.__odom = Odometry() # most recently published odometry of the robot self.__path = Path() # most recently published path self.__done = False # is episode done? self.__new_task_started = False # has a new task started? self.__state_collector = state_collector # for getting relevant state values of the robot self.__rl_agent_path = rospkg.RosPack().get_path('rl_agent') # absolute path rl_agent-package self.__flatland_topics = [] # list of flatland topics self.__timestep = 0 # actual timestemp of training self.__NS = ns self.MODE = rospy.get_param("%s/rl_agent/train_mode", 1) self.__clock = Clock().clock self.__task_generator = TaskGenerator(self.__NS, self.__state_collector, 0.46) self.__recent_agent_states = [] # Subscriber for getting data #self.__odom_sub = rospy.Subscriber("%s/odom"%self.__NS, Odometry, self.__odom_callback, queue_size=1) self.__global_plan_sub = rospy.Subscriber("%s/move_base/NavfnROS/plan"%self.__NS, Path, self.__path_callback, queue_size=1) # self.__path_sub = rospy.Subscriber("%s/move_base/GlobalPlanner/plan" % self.__NS, Path, self.__path_callback, queue_size=1) self.__done_sub = rospy.Subscriber("%s/rl_agent/done"%self.__NS, Bool, self.__done_callback, queue_size=1) self.__new_task_sub = rospy.Subscriber('%s/rl_agent/new_task_started'%self.__NS, Bool, self.__new_task_callback, queue_size=1) self.__flatland_topics_sub = rospy.Subscriber("%s/flatland_server/debug/topics"%self.__NS, DebugTopicList, self.__flatland_topic_callback, queue_size=1) self.__agent_action_sub = rospy.Subscriber('%s/rl_agent/action'%self.__NS, Twist, self.__trigger_callback) self.__ped_sub = rospy.Subscriber('%s/pedsim_simulator/simulated_agents' % self.__NS, AgentStates, self.__ped_callback) if self.MODE == 1 or self.MODE == 0: self.clock_sub = rospy.Subscriber('%s/clock' % self.__NS, Clock, self.__clock_callback) # Publisher for generating qualitative image self.__driven_path_pub = rospy.Publisher('%s/rl_eval/driven_path'%self.__NS, Path, queue_size=1) self.__driven_path_pub2 = rospy.Publisher('%s/rl_eval/driven_path2'%self.__NS, Path, queue_size=1) self.__global_path_pub = rospy.Publisher('%s/rl_eval/global_path'%self.__NS, Path, queue_size=1) self.__agent_pub = rospy.Publisher('%s/rl_eval/viz_agents'%self.__NS, MarkerArray, queue_size=1) def load_evaluation_set(self, eval_set_path): """ Loading evaluation set with name eval_set_name. :param eval_data_path: path to evaluation data :param eval_set_name: name of evaluation set (.pickle) :return: evaluation_set as list """ with open('%s.pickle'%(eval_set_path), 'rb') as handle: self.__evaluation_set = pickle.load(handle) return len(self.__evaluation_set) def evaluate_set(self, evaluation_set_path, save_path): """ Evaluates an agent with a evaluation_set and saves results in evaluation_data/test :param eval_data_path: path to evaluation data :param evaluation_set_name: name of evaluation set (.pickle) :param agent_name: name of the agent (.pkl) """ task_generator = TaskGenerator(self.__NS, self.__state_collector, 0.46) self.load_evaluation_set(evaluation_set_path) results = [] i = 0 for task in self.__evaluation_set: print("Evaluating task %d..."%i) task_generator.set_task(task) self.__done = False result = self.evaluate_episode(False) results.append(result) i += 1 with open('%s.pickle' % (save_path), 'ab') as handle: pickle.dump(result, handle, protocol=pickle.HIGHEST_PROTOCOL) def evaluate_episode(self, train): """ Evaluates current episode :return result of the episode. """ result = {} if not train: result["global_path"] = self.__path result["agent_states"] = [] done = False max_time = len(self.__path.poses)/10*2 # in secs if not self.MODE == 2: start_time = self.__clock else: start_time = rospy.get_rostime() driven_route = Path() driven_route.header = self.__path.header poses = [] while not done: [static_scan, ped_scan, merged_scan, img, wp, twist, goal] = self.__state_collector.get_state() min_obstacle_dist = np.amin(merged_scan.ranges) dist_to_goal = math.sqrt(math.pow(goal.position.x, 2) + math.pow(goal.position.y, 2)) # Check if task over pose = PoseStamped() pose.header = self.__odom.header pose.pose = self.__odom.pose.pose poses.append(pose) if not self.MODE == 2: now = self.__clock else: now = rospy.get_rostime() # Check if task over if min_obstacle_dist <= self.__robot_radius or \ self.__rect_robot_collision(static_scan, ped_scan, self.__robot_width, self.__robot_height): rospy.loginfo("Robot collided with obstacle.") done = True result["success"] = -1 elif dist_to_goal < 0.65: rospy.loginfo("Goal reached.") done = True result["success"] = 1 elif self.__done or (now - start_time).to_sec() > max_time: rospy.loginfo("Time exceeded.") done = True result["success"] = 0 if (not train): result["agent_states"].append(self.__recent_agent_states) self.__sleep(0.1) # Info that we don't want to capture during training if not train: result["num_stat_obj"] = 0 result["num_peds"] = 0 #Counting number of static objects and number of dynamic objects (pedestrians) for topic in self.__flatland_topics: if topic.find("stat_obj") != -1: result["num_stat_obj"] +=1 continue if topic.find("person") != -1: result["num_peds"] +=1 driven_route.poses = poses result["time"] = self.__clock - start_time result["driven_path"] = driven_route result["timestep"] = self.__timestep return result def __sleep(self, secs): """ Sleep method, that takes into account the namespace we are in. As we don't run the python3.5-scripts as rosnodes, manual redirecting necessary. :return result of the episode. """ if not self.MODE == 2: now = self.__clock while (self.__clock - now).to_sec() < secs: time.sleep(0.0001) else: # time.sleep(secs) rospy.sleep(secs) def evaluate_training(self, save_path): """ Evaluates an agent during training. Results are saved in /evaluation_data/train :param agent_name: name of the agent (.pkl) """ while True: self.__timestep -= 1 #Waiting for new task while not self.__new_task_started: time.sleep(0.001) self.__done = False self.__new_task_started = False result = self.evaluate_episode(True) with open('%s.pickle' % (save_path), 'ab') as handle: pickle.dump(result, handle, protocol=pickle.HIGHEST_PROTOCOL) def show_paths(self, result): """ Publishs the driven and global path of the result :param result """ self.__driven_path_pub.publish(result["driven_path"]) self.__global_path_pub.publish(result["global_path"]) def generate_qualitative_static_image_rviz(self, results, results_comp, i_task, i_pos): """ Generates a qualitative image of the result of an evaluation set. Furthermore a second agent can be plotted to compare both in the image (generated in rviz) :param evaluation_set_name: name of the used evaluation set :param results: results of first agent :param results_comp: results of second agent :param i_task: index of episode in evaluation set that should be displayed :param i_pos: position where robot should be displayed. """ if len(results) != len(self.__evaluation_set): print("Error: results and evaluations_set need to have the same length. They don't fit.") return self.__global_path_pub.publish(results[i_task]["global_path"]) self.__driven_path_pub.publish(results[i_task]["driven_path"]) if len(results) == len(results_comp): self.__driven_path_pub2.publish(results_comp[i_task]["driven_path"]) x_robot = results[i_task]["driven_path"].poses[i_pos].pose.position.y y_robot = results[i_task]["driven_path"].poses[i_pos].pose.position.x theta = math.atan2(results[i_task]["driven_path"].poses[i_pos + 4].pose.position.y - x_robot, results[i_task]["driven_path"].poses[i_pos + 4].pose.position.x - y_robot) self.__task_generator.set_robot_pos(y_robot, x_robot, theta) self.__task_generator.remove_all_static_objects() task = self.__evaluation_set[i_task] if 'static_objects' in task.keys(): for i in range(len(task["static_objects"]["x"])): self.__task_generator.spawn_object(task["static_objects"]["model_name"][i], i, task["static_objects"]["x"][i], task["static_objects"]["y"][i], task["static_objects"]["theta"][i]) # Making visible by taking a sim_step self.__task_generator.take_sim_step() def generate_qualitative_ped_image_rviz(self, results, i_task, i_pos): """ Generates a dynamic qualitative image of the result of a pedestrian evaluation set. The state of the pedestrians and the robot is plotted at position i_pos. :param evaluation_set_name: name of the used evaluation set :param results: results of first agent :param results_comp: results of second agent :param i_task: index of episode in evaluation set that should be displayed :param i_pos: position where robot should be displayed. """ if (i_pos >= len(results[i_task]["success"])-4): i_pos = len(results[i_task]["success"])-5 self.generate_qualitative_static_image_rviz(results, [], i_task, i_pos) agent_states = results[i_task]["agent_states"][i_pos] marker_array = MarkerArray() for i, agent_state in enumerate(agent_states): marker_array.markers.append(Marker()) marker_array.markers[i*3] = Marker() marker_array.markers[i*3].header.frame_id = "/map" marker_array.markers[i*3].id = i*3 marker_array.markers[i*3].action = 0 marker_array.markers[i*3].type = 0 marker_array.markers[i*3].scale.x = 0.07 marker_array.markers[i*3].scale.y = 0.2 marker_array.markers[i*3].scale.z = 0.3 marker_array.markers[i*3].color.a = 0.5 marker_array.markers[i*3].color.r = 170/255 marker_array.markers[i*3].color.g = 0.0 marker_array.markers[i*3].color.b = 0.0 start_point = agent_state.pose.position marker_array.markers[i*3].points.append(start_point) end_point = Point() fac = 3 end_point.x = agent_state.pose.position.x + agent_state.twist.linear.x*fac end_point.y = agent_state.pose.position.y + agent_state.twist.linear.y*fac marker_array.markers[i*3].points.append(end_point) marker_array.markers.append(Marker()) marker_array.markers[i*3+1].header.frame_id = "/map" marker_array.markers[i*3+1].id = i*3+1 marker_array.markers[i*3+1].action = 0 marker_array.markers[i*3+1].type = 2 marker_array.markers[i*3+1].pose = agent_state.pose marker_array.markers[i*3+1].scale.x = 0.32 marker_array.markers[i*3+1].scale.y = 0.32 marker_array.markers[i*3+1].scale.z = 0.1 marker_array.markers[i*3+1].color.a = 1.0 marker_array.markers[i*3+1].color.r = 170/255 marker_array.markers[i*3+1].color.g = 0.0 marker_array.markers[i*3+1].color.b = 0.0 lookahead = 100 marker_array.markers.append(Marker()) marker_array.markers[i*3+2].header.frame_id = "/map" marker_array.markers[i*3+2].id = i*3+2 marker_array.markers[i*3+2].action = 0 marker_array.markers[i*3+2].type = 4 marker_array.markers[i*3+2].scale.x = 0.015 marker_array.markers[i*3+2].color.a = 1.0 marker_array.markers[i*3+2].color.r = 170/255 marker_array.markers[i*3+2].color.g = 0.0 marker_array.markers[i*3+2].color.b = 0.0 for agent_states_lookahead in results[i_task]["agent_states"][i_pos:min(len(results[i_task]["agent_states"]), i_pos + lookahead)]: marker_array.markers[i*3+2].points.append(agent_states_lookahead[i].pose.position) # Publish Marker self.__agent_pub.publish(marker_array) def __rect_robot_collision(self, static_scan_msg, ped_scan_msg, robot_width, robot_height): """ Checks if the robot footprint robot_width x robot_height collided :param static_scan_msg: scan of the static objects :param ped_scan_msg: scan of the pedestrians :param robot_width: width of the robots footprint :param robot_height: height of the robots footprint """ robot_collision = False angle_min = ped_scan_msg.angle_min angle_increment = ped_scan_msg.angle_increment for i in range(len(ped_scan_msg.ranges)): angle = angle_min + i * angle_increment length = min(ped_scan_msg.ranges[i], static_scan_msg.ranges[i]) x = math.cos(angle) * length y = math.sin(angle) * length if(x > -robot_height/2 and x < robot_height/2 and y > -robot_width/2 and y < robot_width/2): robot_collision = True return robot_collision def __odom_callback(self, data): self.__odom = data def __path_callback(self, data): self.__path = data def __done_callback(self, data): self.__done = data.data def __new_task_callback(self, data): self.__new_task_started = data.data def __flatland_topic_callback(self, data): self.__flatland_topics = data.topics def __trigger_callback(self, data): self.__timestep += 1 def __clock_callback(self, data): self.__clock = data.clock def __ped_callback(self, data): self.__recent_agent_states = data.agent_states ================================================ FILE: rl_agent/src/rl_agent/evaluation/__init__.py ================================================ ================================================ FILE: rl_agent/src/tf_python.cpp ================================================ /** * * Ronja Gueldenring * 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 * **/ #include namespace rl_agent { TFPython::TFPython(const ros::NodeHandle& node_handle): nh_(node_handle){ goal_sub_ = nh_.subscribe("move_base_simple/goal", 1, &TFPython::goal_callback, this); transformed_goal_pub_ = nh_.advertise("rl_agent/robot_to_goal", 1, false); } /** * * @brief Publishes transform from robot to goal for rl-agent * **/ void TFPython::robot_to_goal_transform(){ if ( goal_.header.frame_id == ""){ return; } tf::StampedTransform rob_to_map; try{ listener_.lookupTransform("base_footprint", goal_.header.frame_id, ros::Time(0), rob_to_map); } catch (tf::TransformException ex){ ROS_ERROR("%s",ex.what()); ros::Duration(1.0).sleep(); } tf::Transform map_to_goal; tf::poseMsgToTF(goal_.pose, map_to_goal); tf::Transform rob_to_goal; rob_to_goal = rob_to_map * map_to_goal; geometry_msgs::PoseStamped msg; msg.header.stamp = ros::Time::now(); msg.header.frame_id = "base_footprint"; tf::poseTFToMsg(rob_to_goal, msg.pose); transformed_goal_pub_.publish(msg); return; } void TFPython::goal_callback(const geometry_msgs::PoseStamped& goal){ goal_ = goal; } }; // namespace rl_agent int main(int argc, char** argv){ ros::init(argc, argv, "tf_python"); ros::NodeHandle node; rl_agent::TFPython tf(node); ros::WallRate r(25); while (ros::ok()) { tf.robot_to_goal_transform(); ros::spinOnce(); r.sleep(); } return 0; }; ================================================ FILE: rl_bringup/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 2.8.3) project(rl_bringup) set(PACKAGE_DEPS roscpp rospy std_msgs nav_msgs tf flatland_msgs ) find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPS}) catkin_package( CATKIN_DEPENDS ${PACKAGE_DEPS} ) install(DIRECTORY config/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config/ ) ########### ## Build ## ########### include_directories(${catkin_INCLUDE_DIRS}) link_directories(${catkin_LIBRARY_DIRS}) set(TOGGLE_SETUP_INIT toggle_setup_init) add_executable(${TOGGLE_SETUP_INIT} src/toggle_setup_init.cpp) add_dependencies(${TOGGLE_SETUP_INIT} ${catkin_EXPORTED_TARGETS}) target_link_libraries(${TOGGLE_SETUP_INIT} ${catkin_LIBRARIES}) ================================================ FILE: rl_bringup/config/costmap_common_params.yaml ================================================ obstacle_range: 3.0 raytrace_range: 6.0 robot_radius: 0.46 inflation_radius: 0.6 static_laser: {clearing: true, data_type: LaserScan, expected_update_rate: 0.0, marking: true, max_obstacle_height: 0.25, min_obstacle_height: -0.1, sensor_frame: static_laser_link, topic: static_laser} back_scan: {clearing: true, data_type: LaserScan, expected_update_rate: 0.0, marking: true, max_obstacle_height: 0.25, min_obstacle_height: -0.1, sensor_frame: back_laser_link, topic: b_scan} front_scan: {clearing: true, data_type: LaserScan, expected_update_rate: 0.0, marking: true, max_obstacle_height: 0.25, min_obstacle_height: -0.1, sensor_frame: front_laser_link, topic: f_scan} observation_sources: front_scan back_scan static_laser ================================================ FILE: rl_bringup/config/dwa_params.yaml ================================================ DWAPlannerROS: max_trans_vel: 0.8 min_trans_vel: 0.05 max_vel_x: 0.8 min_vel_x: 0.05 max_vel_y: 0 min_vel_y: 0 max_rot_vel: 1.2 min_rot_vel: 0.0 acc_lim_theta: 1000.0 # 3.2 acc_lim_x: 1000.0 # 2.5 acc_lim_y: 0 # 2.5 acc_lim_trans: 1000.0 # rot_stopped_vel: 0.01 trans_stopped_vel: 0.01 # Goal Tolerance Parameters yaw_goal_tolerance: 0.5 # 0.05 xy_goal_tolerance: 0.5 # 0.10 # Forward Simulation Parameters sim_time: 1.7 vx_samples: 5 vy_samples: 0 vtheta_samples: 20 ================================================ FILE: rl_bringup/config/global_costmap_params.yaml ================================================ global_costmap: global_frame: /map robot_base_frame: base_footprint update_frequency: 5 static_map: true publish_frequency: 0.0 ================================================ FILE: rl_bringup/config/local_costmap_params.yaml ================================================ local_costmap: global_frame: odom robot_base_frame: base_footprint update_frequency: 5.0 publish_frequency: 2.0 static_map: false rolling_window: true width: 6.0 height: 6.0 resolution: 0.05 ================================================ FILE: rl_bringup/config/movebase_params.yaml ================================================ base_local_planner: rl_local_planner/RLLocalPlanner #dwa_local_planner/DWAPlannerROS base_global_planner: navfn/NavfnROS update_frequency: 10.0 ================================================ FILE: rl_bringup/config/path_config.ini ================================================ [PATHES] path_to_venv=/home/ronja/venv_p3 path_to_train_data=/home/ronja/database path_to_eval_data_train=/home/ronja/database/evaluation_data/train path_to_eval_data_test=/home/ronja/database/evaluation_data/test path_to_eval_sets=/home/ronja/database/evaluation_data/evaluation_sets path_to_catkin_ws=/home/ronja/catkin_ws path_to_tensorboard_log=/home/ronja/database/tensorboard_log_ppo_10 path_to_models=/home/ronja/database/agents ros_version=kinetic ================================================ FILE: rl_bringup/config/path_config_docker.ini ================================================ [PATHES] path_to_venv=/venv_p3 path_to_train_data=/data path_to_eval_data_train=/data/evaluation_data/train path_to_eval_data_test=/data/evaluation_data/test path_to_eval_sets=/data/evaluation_data/evaluation_sets path_to_catkin_ws=/usr/catkin_ws path_to_tensorboard_log=/data/tensorboard_log_ppo_12 path_to_models=/data/agents ros_version=kinetic ================================================ FILE: rl_bringup/config/rl_common.yaml ================================================ rl_agent: f_scan_topic: f_scan b_scan_topic: b_scan robot_frame: base_footprint train_mode: 1 # 0: Execution in Simulation, 1: Training, 2: Execution in Real World update_frequency: 10 goal_theshold: 0.6 num_of_wps: 8 look_ahead_distance: 1.5 wp_reached_dist: 0.2 ped_type: 11 # 0: Pedestrians don't avoid robot # 10: Pedestrians always avoid robot # 11: Pedestrians avoid robot if it stands still and after reaction t ================================================ FILE: rl_bringup/config/rl_params_img.yaml ================================================ rl_agent: state_mode: 0 # 0: image, 1: laserscan img_width_pos: 70 img_width_neg: 20 img_height: 70 resolution: 0.15 ================================================ FILE: rl_bringup/config/rl_params_scan.yaml ================================================ rl_agent: state_mode: 1 # 0: image, 1: laserscan resolution: 0.05 scan_size: 90 ================================================ FILE: rl_bringup/launch/rviz.launch ================================================ ================================================ FILE: rl_bringup/launch/setup.launch ================================================ ================================================ FILE: rl_bringup/launch/sub_launch/dummy_localization.launch ================================================ ================================================ FILE: rl_bringup/launch/sub_launch/flatland.launch ================================================ ================================================ FILE: rl_bringup/launch/sub_launch/move_base.launch ================================================ ================================================ FILE: rl_bringup/launch/sub_launch/navigation.launch ================================================ ================================================ FILE: rl_bringup/launch/sub_launch/pedestrians_only.launch ================================================ ================================================ FILE: rl_bringup/launch/sub_launch/pedsim_visualizer.launch ================================================ ================================================ FILE: rl_bringup/launch/sub_launch/simulation.launch ================================================ ================================================ FILE: rl_bringup/package.xml ================================================ rl_bringup 0.0.0 The rl_bringup package ronja TODO catkin roscpp rospy std_msgs nav_msgs tf flatland_msgs roscpp rospy std_msgs nav_msgs tf flatland_msgs roscpp rospy std_msgs nav_msgs tf flatland_msgs ================================================ FILE: rl_bringup/rviz/robot_navigation.rviz ================================================ Panels: - Class: rviz/Displays Help Height: 0 Name: Displays Property Tree Widget: Expanded: - /TF1/Tree1 - /TrackedPersons1/History as line1 - /static obstacles1/sta_obj_21 - /static obstacles1/stat_obj_51 - /static obstacles1/stat_obj_91 - /static obstacles1/stat_obj_81 - /static obstacles1/stat_obj_71 - /static obstacles1/stat_obj_61 - /static obstacles1/stat_obj_41 - /static obstacles1/stat_obj_31 - /static obstacles1/stat_obj_11 Splitter Ratio: 0.690048933 Tree Height: 644 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 Name: Tool Properties Splitter Ratio: 0.588679016 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time Experimental: false Name: Time SyncMode: 0 SyncSource: static_laser - Class: rviz_plugin/Teleop Name: Teleop - Class: rviz_plugin/Teleop Name: Teleop Toolbars: toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 Class: rviz/Grid Color: 160; 160; 164 Enabled: false Line Style: Line Width: 0.0299999993 Value: Lines Name: Grid Normal Cell Count: 0 Offset: X: 0 Y: 0 Z: 0 Plane: XY Plane Cell Count: 10 Reference Frame: Value: false - Class: rviz/TF Enabled: true Frame Timeout: 15 Frames: All Enabled: false base_footprint: Value: true map: Value: true odom: Value: true ped_laser_link: Value: true static_laser_link: Value: true Marker Scale: 1 Name: TF Show Arrows: false Show Axes: true Show Names: false Tree: map: odom: base_footprint: ped_laser_link: {} static_laser_link: {} Update Interval: 0 Value: true - Class: rviz/Group Displays: - Class: rviz/MarkerArray Enabled: false Marker Topic: /flatland_server/debug/layer/2d Name: World Namespaces: {} Queue Size: 100 Value: false - Class: rviz/Marker Enabled: false Marker Topic: /pedsim_visualizer/walls Name: pedsim_walls Namespaces: {} Queue Size: 100 Value: false - Alpha: 0.899999976 Class: rviz/Map Color Scheme: map Draw Behind: true Enabled: true Name: Map Topic: /map Unreliable: false Use Timestamp: false Value: true Enabled: true Name: world - Class: rviz/Group Displays: - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/LaserScan Color: 85; 85; 255 Color Transformer: FlatColor Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 Name: static_laser Position Transformer: XYZ Queue Size: 1 Selectable: true Size (Pixels): 3 Size (m): 0.100000001 Style: Flat Squares Topic: /static_laser Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/LaserScan Color: 255; 170; 255 Color Transformer: FlatColor Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 Name: ped_laser Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.100000001 Style: Flat Squares Topic: /ped_laser Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/LaserScan Color: 85; 85; 255 Color Transformer: FlatColor Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 Name: f_scan Position Transformer: XYZ Queue Size: 1 Selectable: true Size (Pixels): 3 Size (m): 0.100000001 Style: Flat Squares Topic: /f_scan Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/LaserScan Color: 85; 85; 255 Color Transformer: FlatColor Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 Name: b_scan Position Transformer: XYZ Queue Size: 1 Selectable: true Size (Pixels): 3 Size (m): 0.100000001 Style: Flat Squares Topic: /b_scan Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true Enabled: true Name: LaserScanner - Alpha: 1 Class: spencer_tracking_rviz_plugin/TrackedPersons Color: 170; 0; 0 Color map offset: 0 Color transform: Constant color Delete after no. cycles: 20 Enabled: true Excluded person IDs: "" Font color: 85; 85; 127 Font color style: Constant color Font scale: 2 History as line: Line width: 0.00999999978 Value: true History size: 50 Included person IDs: "" Min. history point distance: 0.400000006 Missed alpha: 0.5 Name: TrackedPersons Occlusion alpha: 0.300000012 Render covariances: Line width: 0.100000001 Value: true Render detection IDs: false Render history: true Render person visual: false Render track IDs: false Render track state: false Render velocities: true Show DELETED tracks: false Show MATCHED tracks: true Show MISSED tracks: true Show OCCLUDED tracks: true Style: Line width: 0.0500000007 Scaling factor: 1 Value: Simple Topic: /pedsim_visualizer/tracked_persons Unreliable: false Value: true Z offset: Use Z position from message: false Value: 0 - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 0; 0; 255 Enabled: true Head Diameter: 0.300000012 Head Length: 0.200000003 Length: 0.300000012 Line Style: Billboards Line Width: 0.0599999987 Name: global_path Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.0299999993 Shaft Diameter: 0.100000001 Shaft Length: 0.100000001 Topic: /move_base/NavfnROS/plan Unreliable: false Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /flatland_server/debug/model/robot_1 Name: robot1 Namespaces: "": true Queue Size: 100 Value: true - Class: rviz/Group Displays: - Alpha: 1 Class: rviz/PointStamped Color: 85; 85; 255 Enabled: true History Length: 1 Name: PointStamped Radius: 0.200000003 Topic: /wp_vis1 Unreliable: false Value: true - Alpha: 1 Class: rviz/PointStamped Color: 85; 85; 255 Enabled: true History Length: 1 Name: PointStamped Radius: 0.200000003 Topic: /wp_vis2 Unreliable: false Value: true - Alpha: 1 Class: rviz/PointStamped Color: 85; 85; 255 Enabled: true History Length: 1 Name: PointStamped Radius: 0.200000003 Topic: /wp_vis3 Unreliable: false Value: true - Alpha: 1 Class: rviz/PointStamped Color: 85; 85; 255 Enabled: true History Length: 1 Name: PointStamped Radius: 0.200000003 Topic: /wp_vis4 Unreliable: false Value: true Enabled: true Name: waypoints - Class: rviz/Group Displays: - Class: rviz/MarkerArray Enabled: true Marker Topic: flatland_server/debug/model/stat_obj_0 Name: stat_obj_0 Namespaces: {} Queue Size: 1 Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /flatland_server/debug/model/stat_obj_2 Name: sta_obj_2 Namespaces: {} Queue Size: 1 Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /flatland_server/debug/model/stat_obj_5 Name: stat_obj_5 Namespaces: {} Queue Size: 1 Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /flatland_server/debug/model/stat_obj_9 Name: stat_obj_9 Namespaces: {} Queue Size: 1 Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /flatland_server/debug/model/stat_obj_8 Name: stat_obj_8 Namespaces: {} Queue Size: 1 Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /flatland_server/debug/model/stat_obj_7 Name: stat_obj_7 Namespaces: {} Queue Size: 1 Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /flatland_server/debug/model/stat_obj_6 Name: stat_obj_6 Namespaces: {} Queue Size: 1 Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /flatland_server/debug/model/stat_obj_4 Name: stat_obj_4 Namespaces: {} Queue Size: 1 Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /flatland_server/debug/model/stat_obj_3 Name: stat_obj_3 Namespaces: {} Queue Size: 1 Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /flatland_server/debug/model/stat_obj_1 Name: stat_obj_1 Namespaces: {} Queue Size: 1 Value: true Enabled: true Name: static obstacles - Class: rviz/Group Displays: - Alpha: 0.699999988 Class: rviz/Map Color Scheme: map Draw Behind: false Enabled: true Name: Map Topic: /rl_map Unreliable: false Use Timestamp: false Value: true Enabled: true Name: input_state - Class: rviz/Group Displays: - Class: rviz/MarkerArray Enabled: true Marker Topic: /flatland_server/debug/model/person_8 Name: person8 Namespaces: {} Queue Size: 100 Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /flatland_server/debug/model/person_7 Name: person7 Namespaces: {} Queue Size: 100 Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /flatland_server/debug/model/person_5 Name: person5 Namespaces: {} Queue Size: 100 Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /flatland_server/debug/model/person_4 Name: person4 Namespaces: "": true Queue Size: 100 Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /flatland_server/debug/model/person_3 Name: person3 Namespaces: "": true Queue Size: 100 Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /flatland_server/debug/model/person_6 Name: person6 Namespaces: {} Queue Size: 100 Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /flatland_server/debug/model/person_2 Name: person2 Namespaces: "": true Queue Size: 100 Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /flatland_server/debug/model/person_0 Name: person0 Namespaces: {} Queue Size: 100 Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /flatland_server/debug/model/person_1 Name: person1 Namespaces: "": true Queue Size: 100 Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /flatland_server/debug/model/person_9 Name: person9 Namespaces: {} Queue Size: 100 Value: true Enabled: true Name: persons Enabled: true Global Options: Background Color: 255; 255; 255 Default Light: true Fixed Frame: map Frame Rate: 30 Name: root Tools: - Class: rviz/MoveCamera - Class: rviz/Interact Hide Inactive Objects: true - Class: rviz/Select - Class: rviz/SetInitialPose Topic: /initialpose - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/Measure Value: true Views: Current: Angle: 0.0750005692 Class: rviz/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.0599999987 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Invert Z Axis: false Name: Current View Near Clip Distance: 0.00999999978 Scale: 35.4415321 Target Frame: Value: TopDownOrtho (rviz) X: 15.649415 Y: 13.8903675 Saved: ~ Window Geometry: Displays: collapsed: false Height: 1029 Hide Left Dock: false Hide Right Dock: true QMainWindow State: 000000ff00000000fd0000000400000000000002a2000003aafc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006f00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000039500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000041000002c5000000f300fffffffb0000000c00540065006c0065006f0070020000012c000003ef0000026a00000049fb0000000a0049006d006100670065000000021b000001370000000000000000fb0000000a0049006d00610067006500000002fa0000006a0000000000000000fb0000000c00540065006c0065006f0070010000030c000000df0000008100fffffffb0000000a0049006d0061006700650100000310000000c600000000000000000000000100000170000004d5fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000069000004d5000000bb00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000027600fffffffb0000000800540069006d00650100000000000004500000000000000000000004d4000003aa00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Teleop: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: true Width: 1916 X: 0 Y: 49 ================================================ FILE: rl_bringup/src/toggle_setup_init.cpp ================================================ /* * @name toggle_setup_init.cpp * @brief Simulation will be triggered for n_sec for initialize setup. * @author Ronja Gueldenring * @date 2019/04/05 **/ #include #include int main(int argc, char** argv){ ros::init(argc, argv, "map_echo"); ros::NodeHandle node; std::string train_mode_topic = ros::this_node::getNamespace() + "/rl_agent/train_mode"; int rl_mode; node.getParam(train_mode_topic, rl_mode); bool keep_clock_running = false; if(rl_mode == 2){ keep_clock_running = true; } float n_sec = 10.0; ros::ServiceClient step_simulation_ = node.serviceClient("step"); flatland_msgs::Step msg; msg.request.step_time.data = 0.1; ros::WallTime begin = ros::WallTime::now(); ros::WallRate r(30); while ((ros::WallTime::now() - begin).toSec() < n_sec || keep_clock_running) { if(!step_simulation_.call(msg)){ ROS_ERROR("Failed to call step_simulation_ service"); } r.sleep(); } return 0; }; ================================================ FILE: rl_local_planner/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 2.8.3) project(rl_local_planner) find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs base_local_planner tf sensor_msgs nav_msgs nav_core laser_geometry geometry_msgs message_generation geometry_msgs ) find_package(PCL REQUIRED) find_package(Eigen3 REQUIRED) find_package(Boost REQUIRED COMPONENTS system) ################################### ## catkin specific configuration ## ################################### catkin_package( INCLUDE_DIRS include ${EIGEN3_INCLUDE_DIRS} LIBRARIES rlLocalPlanner CATKIN_DEPENDS base_local_planner costmap_2d nav_msgs nav_core roscpp std_msgs tf laser_geometry geometry_msgs message_runtime nav_msgs DEPENDS EIGEN3 ) ########### ## Build ## ########### include_directories( include include/${PROJECT_NAME} ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ) add_definitions(${EIGEN3_DEFINITIONS}) add_library(rlLocalPlanner src/rl_local_planner.cpp ) add_dependencies(rlLocalPlanner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ) target_link_libraries(rlLocalPlanner ${catkin_LIBRARIES} ${boost_LIBRARIES} ) add_executable(wp_generator src/wp_generator.cpp ) add_dependencies(wp_generator ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ) target_link_libraries(wp_generator ${catkin_LIBRARIES} ) add_executable(image_generator src/image_generator.cpp ) add_dependencies(image_generator ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ) target_link_libraries(image_generator ${catkin_LIBRARIES} ) ############# ## Install ## ############# install( TARGETS rlLocalPlanner ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ) install( FILES blp_plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) install( TARGETS wp_generator image_generator DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) ================================================ FILE: rl_local_planner/blp_plugin.xml ================================================ An implementation of a local planner communicating with an rl-agent. ================================================ FILE: rl_local_planner/include/rl_local_planner/image_generator.h ================================================ /* * @name image_generator.h * @brief An image is generated from laserscan data and waypoints on the path * @author Ronja Gueldenring * @date 2019/04/05 **/ #ifndef IMAGE_GENERATOR_H #define IMAGE_GENERATOR_H #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include namespace rl_image_generator { /** * This class generates an image from laserscan data and waypoints on the path. */ class ImageGenerator { public: /** * @brief Constructor f */ ImageGenerator(const ros::NodeHandle& node_handle); virtual ~ImageGenerator() = default; protected: ros::NodeHandle nh_; private: int img_width_pos_; //number of height cells in positive direction int img_width_neg_; //number of height cells in negative direction int img_height_; //number of height cells in negative direction float resolution_; //resolution in m/cell tf::TransformListener tf_; std::string robot_frame_; /** * set_path_service + path_callback_ * @brief Service, that generates image from most recent scan and waypoint. */ ros::ServiceServer get_image_service_; bool get_img_callback_(rl_msgs::StateImageGenerationSrv::Request& request, rl_msgs::StateImageGenerationSrv::Response& response); /** * @brief Image is generated by adding obstacles retrieved * from the laser scans and adding a line from waypoint * to waypoint. * @param scan laser scan that has to be added to the image. * @wp waypoint vector that has to be added to the image. **/ nav_msgs::OccupancyGrid generate_image(sensor_msgs::LaserScan& scan, rl_msgs::Waypoint& wp); /** * @brief The index of the image for point (x, y) in [m] is computed. * @param x x-position * @param y y-position * @return image index */ int point_to_index_(double x, double y); /** * @brief Adding scan and occlusions to the image * @param image Image, where to add the scan * @param scan Scan, that should be added to the image. */ void add_scan_to_img_(std::vector& image, sensor_msgs::LaserScan& scan); /** * @brief Adding goal point as square * @param image Image, where to add the square * @param x x-position of goal. * @param y y-position of goal. */ void add_goal_point(std::vector& image, float x_goal, float y_goal); /** * @brief Adding Line to the image (Bresenham) * @param image Image, where to add the line * @param x1 x-position of first line point. * @param y1 y-position of first line point. * @param x2 x-position of second line point. * @param y2 y-position of second line point. */ void add_line_to_img_(std::vector& image, float x1, float y1, float x2, float y2, int value); /** * @brief Mean square distance. * @param x x-position * @param y x-position * @retur sqrt(x^2 + y^2) */ double metric_dist(double x, double y); float get_res(); int get_img_height(); int get_img_pos_width(); int get_img_neg_width(); }; }; #endif /* IMAGE_GENERATOR_H */ ================================================ FILE: rl_local_planner/include/rl_local_planner/rl_local_planner.h ================================================ /* * @name rl_local_planner.h * @brief Connector of move_base and rl_agent using RL library. Forwards action of rl_agent * to move_base each time step. * @author Ronja Gueldenring * @date 2019/04/05 **/ #ifndef RL_LOCAL_PLANNER_H #define RL_LOCAL_PLANNER_H #include #include #include #include #include #include namespace rl_local_planner { /** * This class serves as connector between move\_base and the rl_agent, that uses * a RL-library. The rl_agent provides an action each timestep that is forwarded * to move_base over this class. */ class RLLocalPlanner : public nav_core::BaseLocalPlanner { public: /** * @brief Constructor for RLLocalPlanner wrapper */ RLLocalPlanner(); virtual ~RLLocalPlanner() = default; /** * @brief Constructs the ros wrapper * @param name The name to give this instance of the trajectory planner * @param tf A pointer to a transform listener * @param costmap The cost map to use for assigning costs to trajectories */ void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros); /** * @brief Check if the goal pose has been achieved * @return True if achieved, false otherwise */ bool isGoalReached(); /** * @brief Set the plan that the controller is following * @param orig_global_plan The plan to pass to the controller * @return True if the plan was updated successfully, false otherwise */ bool setPlan(const std::vector& orig_plan); /** * @brief Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base * @param cmd_vel Will be filled with the velocity command to be passed to the robot base * @return >0 if a valid velocity command was found, otherwise a result <0, contains the fail_code */ bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel); protected: ros::NodeHandle nh_; private: // Class variables tf::TransformListener* tf_; tf::Vector3 original_goal_; // goal of global plan geometry_msgs::Twist action_; // most recent action std::string path_frame_; // name of path frame std::string robot_frame_; // name of robot frame double goal_threshold_; // threshold, when goal is reached bool is_action_new_; // True, if new action is available int rl_mode_; // mode of rl_agent bool done_; // True, if the rl_agent is done // Services ros::ServiceClient rl_agent_; ros::ServiceClient set_path_service_; // Publisher ros::Publisher trigger_agent_pub; // Subscriber ros::Subscriber agent_action_sub_; /** * @brief cmd_vel of rl-agent is published here. * @param cmd_vel velocity command */ void agent_action_callback_(const geometry_msgs::Twist& cmd_vel); ros::Subscriber done_sub_; /** * @brief If episode is finished done=True is published here. * @param done True, if episode is done. */ void done_callback_(const std_msgs::Bool& done); // functions /** * @brief Mean square distance. * @param x x-position * @param y x-position * @retur sqrt(x^2 + y^2) */ double metric_dist(double x, double y); }; }; #endif /* RL_LOCAL_PLANNER_H */ ================================================ FILE: rl_local_planner/include/rl_local_planner/wp_generator.h ================================================ /* * @name wp_generator.h * @brief Generation of waypoints on the global path and * determination of the closest waypoints to the robot. * @author Ronja Gueldenring * @date 2019/04/05 **/ #ifndef WP_GENERATOR_H #define WP_GENERATOR_H #include #include #include #include #include #include #include #include namespace rl_local_planner { /** * This class downsamples the global path to waypoints and * determines the next closest waypoints to the robot. */ class WpGenerator { public: /** * @brief Constructor for WpGenerator wrapper */ WpGenerator(const ros::NodeHandle& node_handle); virtual ~WpGenerator() = default; /** * @brief Determines the next closest waypoints to the robot */ void get_closest_wps(); protected: ros::NodeHandle nh_; private: std::vector waypoints_; //List of all remaining waypoints on path double look_ahead_distance_; //distance between waypoints double wp_reached_dist_; //distance, when threshold is reaches int num_of_wps_; //number of closest waypoints, that are published std::string path_frame_; //frame name of path std::string robot_frame_; //frame name of the robot tf::Vector3 goal_; //most recent goal ros::Publisher wp_pub_; //Publisher for next closest waypoints ros::Publisher wp_reached_pub_; //Publisher for reaching waypoint within a wp_reached_dist_ tf::TransformListener tf_; /** * @brief global plan is downsampled to a number of waypoints with distance look_ahead_distance_. * @param global_plan The most recent global plan. */ void precalculate_waypoints_(std::vector global_plan); /** * @brief Mean square distance. * @param x x-position * @param y x-position * @retur sqrt(x^2 + y^2) */ double metric_dist(double x, double y); tf::Vector3 get_transformed_wp_(geometry_msgs::PoseStamped wp, tf::StampedTransform transform); /** * set_path_service + path_callback_ * @brief If new path available, it can be set here. * Wp generator only considers most recent path in further computations. */ ros::ServiceServer set_path_service; bool path_callback_(rl_msgs::SetPath::Request& request, rl_msgs::SetPath::Response& response); }; }; #endif /* WP_GENERATOR_H */ ================================================ FILE: rl_local_planner/package.xml ================================================ rl_local_planner 0.0.0 The rl_local_planner package ronja TODO catkin roscpp rospy std_msgs tf sensor_msgs nav_msgs base_local_planner costmap_2d nav_core laser_geometry geometry_msgs message_generation rl_msgs roscpp rospy std_msgs tf sensor_msgs nav_msgs base_local_planner costmap_2d nav_core laser_geometry geometry_msgs roscpp rospy std_msgs tf sensor_msgs nav_msgs base_local_planner costmap_2d nav_core laser_geometry geometry_msgs message_runtime ================================================ FILE: rl_local_planner/src/image_generator.cpp ================================================ /* * @name image_generator.cpp * @brief An image is generated from laserscan data and waypoints on the path * @author Ronja Gueldenring * @date 2019/04/05 **/ #include namespace rl_image_generator { ImageGenerator::ImageGenerator(const ros::NodeHandle& node_handle) : nh_{node_handle}{ // getting params from param server nh_.getParam("rl_agent/img_width_pos", img_width_pos_); nh_.getParam("rl_agent/img_width_neg", img_width_neg_); nh_.getParam("rl_agent/img_height", img_height_); nh_.getParam("rl_agent/resolution", resolution_); nh_.getParam("rl_agent/robot_frame", robot_frame_); //Services std::string img_service_name_ = ros::this_node::getName() + "/get_image"; get_image_service_ = nh_.advertiseService(img_service_name_, &ImageGenerator::get_img_callback_, this); } bool ImageGenerator::get_img_callback_(rl_msgs::StateImageGenerationSrv::Request& request, rl_msgs::StateImageGenerationSrv::Response& response){ nav_msgs::OccupancyGrid img = generate_image(request.scan, request.wps); response.img = img; return true; }//get_img_callback_ nav_msgs::OccupancyGrid ImageGenerator::generate_image(sensor_msgs::LaserScan& scan, rl_msgs::Waypoint& wp){ //Initializing image nav_msgs::OccupancyGrid img; img.header.stamp = ros::Time::now(); img.header.frame_id = robot_frame_; img.info.resolution = resolution_; img.info.width = (img_width_pos_ + img_width_neg_); img.info.height = img_height_; img.info.origin.position.x = -img_width_neg_*resolution_; img.info.origin.position.y = -img_height_*resolution_/2.0; std::vector image; image.resize((img_height_*(img_width_pos_ + img_width_neg_)), 50); img.header.stamp = scan.header.stamp; //Adding laserscan infromation to the image add_scan_to_img_(image, scan); //Adding path infromation to the image if (wp.points.size() > 0){ if(img.header.stamp > wp.header.stamp){ img.header.stamp = wp.header.stamp; } geometry_msgs::Point zero_point; zero_point.x = 0.0; zero_point.y = 0.0; add_line_to_img_(image, zero_point.x, zero_point.y, wp.points[0].x, wp.points[0].y, 100); int num_wps = wp.points.size(); for(int i = 1; i < num_wps; i++){ add_line_to_img_(image, wp.points[i-1].x, wp.points[i-1].y, wp.points[i].x, wp.points[i].y, 100); } } img.data = image; return img; }//generate_image void ImageGenerator::add_goal_point(std::vector& image, float x_goal, float y_goal){ float size = 0.5; for (int iy = 0; iy < (int)(size/resolution_); iy++){ for (int ix = 0; ix < (int)(size/resolution_); ix++){ float y = y_goal - size/2 + iy*resolution_; float x = x_goal - size/2 + ix*resolution_; int index = point_to_index_(x,y); if(index >= 0 && index < (img_height_*(img_width_pos_ + img_width_neg_))){ image[index] = 100; } } } }//add_goal_point void ImageGenerator::add_scan_to_img_(std::vector& image, sensor_msgs::LaserScan& scan){ if(scan.ranges.size() == 0){ return; } tf::StampedTransform scan_transform_; try{ tf_.lookupTransform(robot_frame_, scan.header.frame_id, ros::Time(0), scan_transform_); } catch (tf::TransformException ex){ ROS_ERROR("%s",ex.what()); ros::WallDuration(1.0).sleep(); } double max_dist = sqrt(pow(((img_width_pos_ + img_width_neg_)*resolution_),2) + pow((img_height_*resolution_/2.0),2)); for (int i=0; i < scan.ranges.size(); i+=1){ // std::vector angles = {scan.angle_min + i * scan.angle_increment - 0.333*scan.angle_increment, // scan.angle_min + i * scan.angle_increment - 0.0*scan.angle_increment, // scan.angle_min + i * scan.angle_increment - 0.333*scan.angle_increment // }; std::vector angles; angles.push_back(scan.angle_min + i * scan.angle_increment - 0.4444*scan.angle_increment); angles.push_back(scan.angle_min + i * scan.angle_increment - 0.3333*scan.angle_increment); angles.push_back(scan.angle_min + i * scan.angle_increment - 0.2222*scan.angle_increment); angles.push_back(scan.angle_min + i * scan.angle_increment - 0.1111*scan.angle_increment); angles.push_back(scan.angle_min + i * scan.angle_increment - 0.0*scan.angle_increment); angles.push_back(scan.angle_min + i * scan.angle_increment + 0.1111*scan.angle_increment); angles.push_back(scan.angle_min + i * scan.angle_increment + 0.2222*scan.angle_increment); angles.push_back(scan.angle_min + i * scan.angle_increment + 0.3333*scan.angle_increment); angles.push_back(scan.angle_min + i * scan.angle_increment + 0.4444*scan.angle_increment); for (int k=0; k < angles.size(); k++){ double angle = angles[k]; //scan.angle_min + i * scan.angle_increment; double length; double length2; if (isnan(scan.ranges[i]) || scan.ranges[i] == 0.0){ continue; }else{ length = scan.ranges[i]; length2 = max_dist; } // Transform laserpoint to robot frame tf::Vector3 laser_point(cos(angle)*length, sin(angle)*length, 0.); tf::Vector3 laser_point_transformed = scan_transform_* laser_point; float x1 = laser_point_transformed.getX(); float y1 = laser_point_transformed.getY(); tf::Vector3 laser_point2(cos(angle)*length2, sin(angle)*length2, 0.); tf::Vector3 laser_point2_transformed = scan_transform_* laser_point2; float x2 = laser_point2_transformed.getX(); float y2 = laser_point2_transformed.getY(); add_line_to_img_(image, x1, y1, x2, y2, 0); } } return; }//add_scan_to_img void ImageGenerator::add_line_to_img_(std::vector& image, float x1, float y1, float x2, float y2, int value){ const bool steep = (fabs(y2 - y1) > fabs(x2 - x1)); if(steep) { std::swap(x1, y1); std::swap(x2, y2); } if(x1 > x2) { std::swap(x1, x2); std::swap(y1, y2); } const float dx = x2 - x1; const float dy = fabs(y2 - y1); float error = dx / 2.0f; const float ystep = (y1 < y2) ? resolution_*0.7 : -resolution_*0.7; float y = y1; for(float x=x1; x= 0 && index < (img_height_*(img_width_pos_ + img_width_neg_))){ image[index] = value; } error -= dy; if(error < 0) { y += ystep; error += dx; } } return; }//add_line_to_img_ int ImageGenerator::point_to_index_(double x, double y){ int y_index = (int) ceil((y + (((float)img_height_* resolution_))/2.0)/resolution_); int x_index = (int) ceil((x + (((float)img_width_neg_* resolution_)))/resolution_); int index = (y_index) * (img_width_pos_ + img_width_neg_) + (x_index); if (x_index < 0 || x_index >= (img_width_pos_ + img_width_neg_) || y_index < 0 || y_index > img_height_){ index = (img_width_pos_ + img_width_neg_) * img_height_ + 1; } return index; }//point_to_index_ double ImageGenerator::metric_dist(double x, double y){ double dist = sqrt(pow(x , 2) + pow(y , 2)); return dist; } //metric_dist int ImageGenerator::get_img_neg_width(){ return img_width_neg_; } int ImageGenerator::get_img_pos_width(){ return img_width_pos_; } int ImageGenerator::get_img_height(){ return img_height_; } float ImageGenerator::get_res(){ return resolution_; } }; // namespace rl_image_generator int main(int argc, char** argv){ ros::init(argc, argv, "image_generator"); ros::NodeHandle node; rl_image_generator::ImageGenerator ig(node); ros::WallRate r(100); while (ros::ok()) { ros::spinOnce(); r.sleep(); } return 0; }; ================================================ FILE: rl_local_planner/src/rl_local_planner.cpp ================================================ /* * @name rl_local_planner.cpp * @brief Connector of move_base and rl_agent using RL library. Forwards action of rl_agent * to move_base each time step. * @author Ronja Gueldenring * @date 2019/04/05 **/ #include #include PLUGINLIB_EXPORT_CLASS(rl_local_planner::RLLocalPlanner, nav_core::BaseLocalPlanner) namespace rl_local_planner { RLLocalPlanner::RLLocalPlanner(): nh_(){ }//RLLocalPlanner void RLLocalPlanner::initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros){ // params tf_ = tf; // getting params from param server nh_.getParam("rl_agent/robot_frame", robot_frame_); nh_.getParam("rl_agent/train_mode", rl_mode_); nh_.getParam("rl_agent/goal_theshold", goal_threshold_); // initializing class variables if (rl_mode_ == 1) goal_threshold_ = 0.0; is_action_new_ = false; done_ = false; // Subscriber agent_action_sub_ = nh_.subscribe("rl_agent/action", 1, &RLLocalPlanner::agent_action_callback_, this); done_sub_ = nh_.subscribe("rl_agent/done", 1, &RLLocalPlanner::done_callback_, this); //Publisher trigger_agent_pub = nh_.advertise("trigger_agent", 1, false); // Services std::string set_path_service_name = ros::this_node::getNamespace() + "/wp_generator/set_gobal_plan"; set_path_service_ = nh_.serviceClient(set_path_service_name); } //initialize bool RLLocalPlanner::isGoalReached(){ tf::StampedTransform transform; try{ tf_->lookupTransform(robot_frame_, path_frame_, ros::Time(0), transform); } catch (tf::TransformException ex){ ROS_ERROR("%s",ex.what()); ros::Duration(1.0).sleep(); } tf::Vector3 original_goal_transformed = transform * original_goal_; if(metric_dist(original_goal_transformed.getX(), original_goal_transformed.getY()) < goal_threshold_) return true; if(done_){ done_ = false; return true; } return false; } //isGoalReached bool RLLocalPlanner::setPlan(const std::vector& orig_plan) { // goal_threshold_ = goal_threshold; if ( orig_plan.size() < 1 ){ ROS_ERROR_STREAM("RLLocalPlanner: Got an empty plan"); return false; } geometry_msgs::PoseStamped original_goal = orig_plan.back(); original_goal_ = tf::Vector3(original_goal.pose.position.x, original_goal.pose.position.y, 0.); path_frame_ = original_goal.header.frame_id; done_ = false; rl_msgs::SetPath srv; srv.request.path.header.stamp = ros::Time::now(); srv.request.path.header.frame_id = path_frame_; srv.request.path.poses = orig_plan; if (!set_path_service_.call(srv)){ ROS_ERROR("Failed set path on waypoint generator."); return false; } return true; } //setPlan bool RLLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel){ // ROS_WARN("Velocity command"); // Trigger agent to compute next action std_msgs::Bool msg; msg.data = true; trigger_agent_pub.publish(msg); //Waiting for agents action ros::WallRate r(1000); while(!is_action_new_ && !done_){ r.sleep(); } is_action_new_ = false; //assigning new action if(done_){ action_.linear.x = 0.0; action_.angular.z = 0.0; } cmd_vel = action_; return true; }//computeVelocityCommands double RLLocalPlanner::metric_dist(double x, double y){ double dist = sqrt(pow(x , 2) + pow(y , 2)); return dist; }//metric_dist void RLLocalPlanner::agent_action_callback_(const geometry_msgs::Twist& cmd_vel){ action_ = cmd_vel; is_action_new_ = true; }//agent_action_callback_ void RLLocalPlanner::done_callback_(const std_msgs::Bool& done){ done_ = done.data; }//done_callback_ }; // namespace rl_local_planner ================================================ FILE: rl_local_planner/src/wp_generator.cpp ================================================ /* * @name wp_generator.cpp * @brief Generation of waypoints on the global path and * determination of the closest waypoints to the robot. * @author Ronja Gueldenring * @date 2019/04/05 **/ #include namespace rl_local_planner { WpGenerator::WpGenerator(const ros::NodeHandle& node_handle) : nh_{node_handle}{ // Get params of parameter server nh_.getParam("rl_agent/look_ahead_distance", look_ahead_distance_); nh_.getParam("rl_agent/wp_reached_dist", wp_reached_dist_); nh_.getParam("rl_agent/num_of_wps", num_of_wps_); nh_.getParam("rl_agent/robot_frame", robot_frame_); // Services std::string global_plan_service_name = ros::this_node::getName() + "/set_gobal_plan"; set_path_service = nh_.advertiseService(global_plan_service_name, &WpGenerator::path_callback_, this); //Publisher wp_pub_ = nh_.advertise("wp", 1, true); wp_reached_pub_ = nh_.advertise("wp_reached", 1, true); } bool WpGenerator::path_callback_(rl_msgs::SetPath::Request& request, rl_msgs::SetPath::Response& response){ if(request.path.poses.size() == 0){ return false; } path_frame_ = request.path.header.frame_id; geometry_msgs::PoseStamped goal_temp = request.path.poses.back(); goal_ = tf::Vector3(goal_temp.pose.position.x, goal_temp.pose.position.y, 0.); precalculate_waypoints_(request.path.poses); get_closest_wps(); return true; } void WpGenerator::precalculate_waypoints_(std::vector global_plan){ waypoints_.clear(); geometry_msgs::PoseStamped prev = global_plan[0]; double dist = 0.0; for(int i = 1; i < global_plan.size(); i++){ dist += metric_dist((prev.pose.position.x - global_plan[i].pose.position.x), (prev.pose.position.y - global_plan[i].pose.position.y)); if(dist > look_ahead_distance_){ dist = 0.0; waypoints_.push_back(global_plan[i]); } prev = global_plan[i]; } for(int i = 0; i < num_of_wps_ + 1; i++){ waypoints_.push_back(global_plan.back()); } return; } void WpGenerator::get_closest_wps(){ if(waypoints_.size() < num_of_wps_){ return; } //Prepare Waypoint-message that will be published rl_msgs::Waypoint msg; msg.header.frame_id = robot_frame_; msg.header.stamp = ros::Time::now(); msg.is_new.data = false; // Get Transform from path_frame_ to robot_frame tf::StampedTransform transform; try{ tf_.lookupTransform(robot_frame_, path_frame_, ros::Time(0), transform); } catch (tf::TransformException ex){ ROS_ERROR("%s",ex.what()); ros::WallDuration(1.0).sleep(); } //Find closest point on path tf::Vector3 closest_wp = get_transformed_wp_(waypoints_[0], transform); double dist = metric_dist(closest_wp.getX(), closest_wp.getY()); for(int i = 1; i < waypoints_.size(); i++){ tf::Vector3 temp_wp = get_transformed_wp_(waypoints_[i], transform); double temp_dist = metric_dist(temp_wp.getX(), temp_wp.getY()); if (temp_dist < dist){ dist = temp_dist; closest_wp = temp_wp; }else{ //Closest point on path found! int start_index = i - 1; //Is closest wp reached in a certain radius? if (dist < wp_reached_dist_){ msg.is_new.data = true; for (int k = 0; k < i; k++){ waypoints_.erase(waypoints_.begin()); } start_index = 0; } // Is closest waypoint already overtaken? Then take next one. tf::Vector3 second_wp = get_transformed_wp_(waypoints_[start_index + 1], transform); double dist_second_wp = metric_dist(temp_wp.getX(), temp_wp.getY()); if (dist + look_ahead_distance_ > dist_second_wp){ start_index +=1; } //Getting all sequencing waypoints. std::vector points; for(int j = start_index; j < start_index + num_of_wps_; j++){ geometry_msgs::Point p; if (j < waypoints_.size()){ tf::Vector3 wp = get_transformed_wp_(waypoints_[j], transform); p.x = wp.getX(); p.y = wp.getY(); points.push_back(p); } } msg.points = points; if (msg.is_new.data == true){ wp_reached_pub_.publish(msg); }else{ wp_pub_.publish(msg); } break; } } return; } tf::Vector3 WpGenerator::get_transformed_wp_(geometry_msgs::PoseStamped wp, tf::StampedTransform transform){ tf::Vector3 wp_vec(wp.pose.position.x, wp.pose.position.y, 0.); tf::Vector3 wp_vec_transformed = transform*wp_vec; return wp_vec_transformed; } double WpGenerator::metric_dist(double x, double y){ double dist = sqrt(pow(x , 2) + pow(y , 2)); return dist; }//metric_dist }; // namespace rl_local_planner int main(int argc, char** argv){ ros::init(argc, argv, "wp_generator"); ros::NodeHandle node; int rl_mode; std::string train_mode_topic = ros::this_node::getNamespace() + "/rl_agent/train_mode"; node.param(train_mode_topic, rl_mode, 1); rl_local_planner::WpGenerator wg(node); ros::WallRate r(100); while (ros::ok()) { wg.get_closest_wps(); ros::spinOnce(); r.sleep(); } return 0; }; ================================================ FILE: rl_msgs/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 2.8.3) project(rl_msgs) # Ensure we're using c++11 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -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 std_msgs geometry_msgs nav_msgs sensor_msgs message_generation ) ## System dependencies are found with CMake's conventions find_package(PkgConfig REQUIRED) ################ ## Build MSGS ## ################ add_message_files(FILES Waypoint.msg ) add_service_files(FILES MergeScans.srv SetPath.srv StateImageGenerationSrv.srv ) generate_messages( DEPENDENCIES std_msgs geometry_msgs nav_msgs sensor_msgs rl_msgs ) ################################### ## catkin specific configuration ## ################################### catkin_package( INCLUDE_DIRS CATKIN_DEPENDS std_msgs geometry_msgs nav_msgs sensor_msgs message_runtime ) ########### ## Build ## ########### include_directories( ${catkin_INCLUDE_DIRS} ) ================================================ FILE: rl_msgs/msg/Waypoint.msg ================================================ std_msgs/Header header geometry_msgs/Point[] points std_msgs/Bool is_new ================================================ FILE: rl_msgs/package.xml ================================================ rl_msgs 1.1.1 The rl_msgs package BSD ronja Ronja Gueldenring catkin cmake_modules message_generation std_msgs geometry_msgs sensor_msgs nav_msgs message_runtime ================================================ FILE: rl_msgs/srv/MergeScans.srv ================================================ sensor_msgs/LaserScan[] scans --- sensor_msgs/LaserScan merged_scan ================================================ FILE: rl_msgs/srv/SetPath.srv ================================================ nav_msgs/Path path ================================================ FILE: rl_msgs/srv/StateImageGenerationSrv.srv ================================================ sensor_msgs/LaserScan scan rl_msgs/Waypoint wps --- nav_msgs/OccupancyGrid img ================================================ FILE: start_scripts/entrypoint_ppo2.sh ================================================ #! /bin/bash #get config-params path_to_venv=$(awk -F "=" '/path_to_venv/ {print $2}' ../rl_bringup/config/path_config.ini) path_to_catkin_ws=$(awk -F "=" '/path_to_catkin_ws/ {print $2}' ../rl_bringup/config/path_config.ini) ros_ver=$(awk -F "=" '/ros_version/ {print $2}' ../rl_bringup/config/path_config.ini) # source ros stuff source /opt/ros/$ros_ver/setup.bash source $path_to_catkin_ws/devel/setup.bash source $path_to_venv/bin/activate agent_id=$1 num_sims=$2 INPUT=./training_params/ppo2_params.csv OLDIFS=$IFS IFS=, [ ! -f $INPUT ] && { echo "$INPUT file not found"; } init=false # Checking if roscore is running. If not roscore is started if ! rostopic list ; then screen -dmS roscore bash -c "source ./start_roscore.sh" screen -S roscore -X logfile screenlog_roscore.log screen -S roscore -X log sleep 4 fi # Loading training maps MAPS=./training_params/training_maps.csv maps=() while read map do if [ -z "$map" ]; then break fi maps+=($map) done < $MAPS while read agent_name total_timesteps policy gamma n_steps ent_coef learning_rate vf_coef max_grad_norm lam nminibatches noptepochs cliprange robot_radius rew_fnc num_stacks stack_offset disc_action_space normalize stage pretrained_model_path task_mode do if [ "$init" = false ] ; then init=true continue; fi if [ "$agent_name" = "$agent_id" ] || [ "$agent_id" = "all" ]; then echo "$agent_name" echo "$policy" # Starting num_sims simulations with different maps. i_map=0 for ((i=1;i<=$num_sims;i++)); do echo "${maps[i_map]}" screen -dmS ros_sim$i bash -c "source ./ros.sh sim$i $policy ${maps[i_map]}" screen -S ros_sim$i -X logfile screenlog_"$agent_name"_ros_sim$i.log screen -S ros_sim$i -X log i_map=$((i_map+1)) if [ "$i_map" -eq "${#maps[@]}" ] ; then i_map=0 fi sleep 5 done # Starting Reinforcement Learning training (PPO2) screen -dmS python bash -c "source ./ppo2_training.sh $agent_name $total_timesteps $policy $gamma $n_steps $ent_coef $learning_rate $vf_coef $max_grad_norm $lam $nminibatches $noptepochs $cliprange $robot_radius $rew_fnc $num_stacks $stack_offset $disc_action_space $normalize $stage $pretrained_model_path $task_mode $num_sims" screen -S python -X logfile screenlog_"$agent_name"_py.log screen -S python -X log # Wait until training is done while (screen -list | grep -q "python"); do echo "sleep" sleep 5 done # Old training setup is closed, so that new one can be started. for ((i=1;i<=$num_sims;i++)); do screen -X -S ros_sim$i quit done fi done < $INPUT #bash ================================================ FILE: start_scripts/ppo2_training.sh ================================================ #! /bin/sh #get config-params path_to_venv=$(awk -F "=" '/path_to_venv/ {print $2}' ../rl_bringup/config/path_config.ini) path_to_catkin_ws=$(awk -F "=" '/path_to_catkin_ws/ {print $2}' ../rl_bringup/config/path_config.ini) ros_ver=$(awk -F "=" '/ros_version/ {print $2}' ../rl_bringup/config/path_config.ini) # source ros stuff source /opt/ros/$ros_ver/setup.bash source $path_to_catkin_ws/devel/setup.bash source $path_to_venv/bin/activate # get params agent_name=$1 total_timesteps=$2 policy=$3 gamma=$4 n_steps=$5 ent_coef=$6 learning_rate=$7 vf_coef=$8 max_grad_norm=$9 lam=${10} nminibatches=${11} noptepochs=${12} cliprange=${13} robot_radius=${14} rew_fnc=${15} num_stacks=${16} stack_offset=${17} disc_action_space=${18} normalize=${19} stage=${20} pretrained_model_path=${21} task_mode=${22} num_sim=${23} # start rl_agent python ../rl_agent/scripts/train_scripts/train_ppo.py $agent_name $total_timesteps $policy $gamma $n_steps $ent_coef $learning_rate $vf_coef $max_grad_norm $lam $nminibatches $noptepochs $cliprange $robot_radius $rew_fnc $num_stacks $stack_offset $disc_action_space $normalize $stage $pretrained_model_path $task_mode $num_sim deactivate IFS=$OLDIFS ================================================ FILE: start_scripts/ros.sh ================================================ #! /bin/sh sim_name=$1 policy=$2 map=$3 #get config-params path_to_catkin_ws=$(awk -F "=" '/path_to_catkin_ws/ {print $2}' ../rl_bringup/config/path_config.ini) ros_ver=$(awk -F "=" '/ros_version/ {print $2}' ../rl_bringup/config/path_config.ini) # source ros stuff source /opt/ros/$ros_ver/setup.bash source $path_to_catkin_ws/devel/setup.bash map_path=$path_to_catkin_ws/src/drl_local_planner_ros_stable_baselines/flatland_setup/maps/$map echo "map_path" echo "$map_path" echo "$sim_name" if [ "$policy" = "CnnPolicy_multi_input_vel" ] || [ "$policy" = "CnnPolicy_multi_input_vel2" ] || [ "$policy" = "CnnPolicy" ]; then echo "$policy" roslaunch rl_bringup setup.launch ns:="$sim_name" rl_params:="rl_params_img" map_path:="$map_path" fi if [ "$policy" = "CNN1DPolicy" ] || [ "$policy" = "CNN1DPolicy_multi_input" ]; then echo "$policy" roslaunch rl_bringup setup.launch ns:="$sim_name" rl_params:="rl_params_scan" map_path:="$map_path" fi ================================================ FILE: start_scripts/start_roscore.sh ================================================ #! /bin/sh #get config-params path_to_catkin_ws=$(awk -F "=" '/path_to_catkin_ws/ {print $2}' ../rl_bringup/config/path_config.ini) ros_ver=$(awk -F "=" '/ros_version/ {print $2}' ../rl_bringup/config/path_config.ini) # source ros stuff source /opt/ros/$ros_ver/setup.bash source $path_to_catkin_ws/devel/setup.bash roscore ================================================ FILE: start_scripts/training_params/ppo2_params.csv ================================================ agent_name, total_timesteps, policy, gamma, n_steps, ent_coef, learning_rate, vf_coef, max_grad_norm,lam,nminibatches,noptepochs,cliprange, robot_radius, rew_fnc, num_stacks, stack_offset, disc_action_space, normalize, stage, pretrained_model_name, task_mode ppo2_1_raw_data_disc_0,30000000,CNN1DPolicy_multi_input,0.99,4800,0.005,0.0003,0.2,0.5,0.95,15,3,0.2,0.5,19,1,15,1,0,0,empty,ped ppo2_3_raw_data_disc_0,30000000,CNN1DPolicy_multi_input,0.99,4800,0.005,0.0003,0.2,0.5,0.95,15,3,0.2,0.5,19,3,15,1,0,0,empty,ped ppo2_1_raw_data_cont_0,30000000,CNN1DPolicy_multi_input,0.99,4800,0.005,0.0003,0.2,0.5,0.95,15,3,0.2,0.5,19,1,15,0,0,0,empty,ped ppo2_1_img_disc_1,30000000,CnnPolicy,0.99,4800,0.005,0.0003,0.2,0.5,0.95,15,3,0.2,0.5,19,1,15,1,0,0,empty,ped ppo2_3_img_disc_0,30000000,CnnPolicy,0.99,4800,0.005,0.0003,0.2,0.5,0.95,15,3,0.2,0.5,19,3,15,1,0,0,empty,ped ppo2_1_img_cont_0,30000000,CnnPolicy,0.99,4800,0.005,0.0003,0.2,0.5,0.95,15,3,0.2,0.5,19,1,15,0,0,0,empty,ped ppo2_1_raw_data_disc_0_pretrained,30000000,CNN1DPolicy_multi_input,0.99,4800,0.005,0.0003,0.2,0.5,0.95,15,3,0.2,0.46,19,1,15,1,0,1,ppo2_1_raw_data_disc_0,ped ppo2_1_img_disc_1_pretrained,30000000,CnnPolicy,0.99,4800,0.005,0.0003,0.2,0.5,0.95,15,3,0.2,0.46,19,1,15,1,0,1,ppo2_1_img_disc_0,ped ppo2_3_raw_data_disc_0_pretrained,30000000,CNN1DPolicy_multi_input,0.99,4800,0.005,0.0003,0.2,0.46,0.95,15,3,0.2,0.5,19,3,15,1,0,1,ppo2_3_raw_data_disc_0,ped ================================================ FILE: start_scripts/training_params/training_maps.csv ================================================ open_field_1 corridor_1 corridor_3 complex_map_1