[ { "path": ".gitignore", "content": "*.png\n*__pycache__*\n*.idea*\n*.pyc\n*screenlog*\n" }, { "path": ".rosinstall", "content": "- git:\n local-name: drl_local_planner_forks/flatland\n uri: https://github.com/RGring/flatland.git\n version: pedsim_integration\n- git:\n local-name: drl_local_planner_forks/pedsim\n uri: https://github.com/RGring/pedsim_ros.git\n version: rl_features\n- git:\n local-name: drl_local_planner_forks/navigation\n uri: https://github.com/RGring/navigation.git\n version: flatland_integration\n- git:\n local-name: drl_local_planner_forks/stable-baselines\n uri: https://github.com/RGring/stable-baselines.git\n version: added_1d_conv_layer" }, { "path": "LICENSE", "content": "BSD 3-Clause License\n\nCopyright (c) 2019, RGring\nAll rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are met:\n\n* Redistributions of source code must retain the above copyright notice, this\n list of conditions and the following disclaimer.\n\n* Redistributions in binary form must reproduce the above copyright notice,\n this list of conditions and the following disclaimer in the documentation\n and/or other materials provided with the distribution.\n\n* Neither the name of the copyright holder nor the names of its\n contributors may be used to endorse or promote products derived from\n this software without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\nAND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\nIMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE\nDISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE\nFOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL\nDAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR\nSERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\nCAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,\nOR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE\nOF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n" }, { "path": "README.md", "content": "# What is this repository for?\n* Setup to train a local planner with reinforcement learning approaches from [stable baselines](https://github.com/hill-a/stable-baselines) integrated ROS\n* Training in a simulator fusion of [Flatland](https://github.com/avidbots/flatland) and [pedsim_ros](https://github.com/srl-freiburg/pedsim_ros)\n* local planner has been trained on static and dynamic obstacles: [video](https://www.youtube.com/watch?v=nHvpO0hVnAg)\n* Link to [IROS Paper](http://ras.papercept.net/images/temp/IROS/files/0122.pdf)\n* Link to [Master Thesis](https://tams.informatik.uni-hamburg.de/publications/2019/MSc_Ronja_Gueldenring.pdf) for more in depth information.\n\n# Installation (Else: Docker below)\n\n1. Standart ROS setup (Code has been tested with ROS-kinetic on Ubuntu 16.04)\n\n2. Install additional packages\n ```\n apt-get update && apt-get install -y \\\n libqt4-dev \\\n libopencv-dev \\\n liblua5.2-dev \\\n virtualenv \\\n screen \\\n python3-dev \\\n ros-kinetic-tf2-geometry-msgs \\\n ros-kinetic-navigation \\\n ros-kinetic-rviz \n ```\n\n3. Setup repository: \n * Clone this repository in your src-folder of your catkin workspace\n ```\n cd /src/drl_local_planner_ros_stable_baselines\n cp .rosinstall ../\n cd ..\n rosws update\n cd \n catkin_make -DCMAKE_BUILD_TYPE=Release\n ```\n (please install missing packages)\n\n4. Setup virtual environment to be able to use python3 with ros (consider also requirements.txt)\n ```\n virtualenv /venv_p3 --python=python3\n source /venv_p3/bin/activate\n /venv_p3/bin/pip install \\\n pyyaml \\\n rospkg \\\n catkin_pkg \\\n exception \\\n numpy \\\n tensorflow==\"1.13.1\" \\\n gym \\\n pyquaternion \\ \n mpi4py \\\n matplotlib\n cd /src/drl_local_planner_forks/stable_baselines/\n /venv_p3/bin/pip install -e path_to_catkin_ws>/src/drl_local_planner_forks/stable-baselines/\n ```\n5. Set system-relevant variables \n * Modify all relevant pathes rl_bringup/config/path_config.ini\n\n\n# Example usage\n\n1. Train agent\n * Open first terminal (roscore): \n ```\n roscore\n ```\n * Open second terminal (simulationI:\n ```\n roslaunch rl_bringup setup.launch ns:=\"sim1\" rl_params:=\"rl_params_scan\"\n ```\n * Open third terminal (DRL-agent):\n ```\n source /bin/activate \n python rl_agent/scripts/train_scripts/train_ppo.py\n ```\n * Open fourth terminal (Visualization):\n ```\n roslaunch rl_bringup rviz.launch ns:=\"sim1\"\n ```\n\n2. Execute self-trained ppo-agent\n * Copy your trained agent in your \"path_to_models\"\n * Open first terminal: \n ```\n roscore\n ```\n * Open second terminal: \n ```\n roslaunch rl_bringup setup.launch ns:=\"sim1\" rl_params:=\"rl_params_scan\"\n ```\n * Open third terminal:\n ```\n source /venv_p3/bin/activate \n roslaunch rl_agent run_ppo_agent.launch mode:=\"train\"\n ```\n * Open fourth terminal: \n ```\n roslaunch rl_bringup rviz.launch ns:=\"sim1\"\n ```\n * Set 2D Navigation Goal in rviz\n\n# Run pretrained Agents\nNote: To be able to load the pretrained agents, you need to install numpy version 1.17.0.\n```\n/venv_p3/bin/pip install numpy==1.17\n```\n\n### Run agent trained on raw data, discrete action space, stack size 1\n1. Copy the example_agents in your \"path_to_models\"\n2. Open first terminal: \n ```\n roscore\n ```\n3. Open second terminal for visualization: \n ```\n roslaunch rl_bringup rviz.launch ns:=\"sim1\"\n ```\n4. Open third terminal: \n ```\n roslaunch rl_bringup setup.launch ns:=\"sim1\" rl_params:=\"rl_params_scan\"\n ```\n5. Open fourth terminal:\n ```\n source /venv_p3/bin/activate \n roslaunch rl_agent run_1_raw_disc.launch mode:=\"train\"\n ```\n### Run agent trained on raw data, discrete action space, stack size 3\n1. Step 1 - 4 are the same like in the first example\n2. Open fourth terminal:\n ```\n source /venv_p3/bin/activate \n roslaunch rl_agent run_3_raw_disc.launch mode:=\"train\"\n ```\n\n### Run agent trained on raw data, continuous action space, stack size 1\n1. Step 1 - 4 are the same like in the first example\n2. Open fourth terminal:\n ```\n source /venv_p3/bin/activate \n roslaunch rl_agent run_1_raw_cont.launch mode:=\"train\"\n ```\n\n### Run agent trained on image data, discrete action space, stack size 1\n1. Step 1 - 3 are the same like in the first example\n4. Open third terminal: \n ```\n roslaunch rl_bringup setup.launch ns:=\"sim1\" rl_params:=\"rl_params_img\"\n ```\n5. Open fourth terminal:\n ```\n source /venv_p3/bin/activate \n roslaunch rl_agent run_1_img_disc.launch mode:=\"train\"\n ```\n\n\n \n\n\n# Training in Docker\nI 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.\n\n0. Build the Docker image (This will unfortunately take about 15 minutes):\n```\ncd drl_local_planner_ros_stable_baselines/docker\n```\n```\ndocker build -t ros-drl_local_planner .\n```\n## Training from scratch\n1. In start_scripts/training_params/ppo2_params, define the agents training parameters.\n\n | Parameter | Desctiption |\n |-------------------------|--------------|\n | agent_name | Number of timestamps the agent will be trained. |\n | total_timesteps | Number of timestamps the agent will be trained. |\n | policy | see [PPO2 Doc](https://stable-baselines.readthedocs.io/en/master/modules/ppo2.html) |\n | gamma | see [PPO2 Doc](https://stable-baselines.readthedocs.io/en/master/modules/ppo2.html) |\n | n_steps | see [PPO2 Doc](https://stable-baselines.readthedocs.io/en/master/modules/ppo2.html) |\n | ent_coef | see [PPO2 Doc](https://stable-baselines.readthedocs.io/en/master/modules/ppo2.html) |\n | learning_rate | see [PPO2 Doc](https://stable-baselines.readthedocs.io/en/master/modules/ppo2.html) |\n | vf_coef | see [PPO2 Doc](https://stable-baselines.readthedocs.io/en/master/modules/ppo2.html) |\n | max_grad_norm | see [PPO2 Doc](https://stable-baselines.readthedocs.io/en/master/modules/ppo2.html) |\n | lam | see [PPO2 Doc](https://stable-baselines.readthedocs.io/en/master/modules/ppo2.html) |\n | nminibatches | see [PPO2 Doc](https://stable-baselines.readthedocs.io/en/master/modules/ppo2.html) |\n | noptepochs | see [PPO2 Doc](https://stable-baselines.readthedocs.io/en/master/modules/ppo2.html) |\n | cliprange | see [PPO2 Doc](https://stable-baselines.readthedocs.io/en/master/modules/ppo2.html) |\n | robot_radius | The radius if the robot footprint |\n | 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. |\n | num_stacks | State representation includes the current observation and (num_stacks - 1) previous observation. |\n | stack_offset | The number of timestamps between each stacked observation. |\n | disc_action_space | 0, if continuous action space. 1, if discrete action space. |\n | normalize | 0, if input should not be normalized. 1, if input should be normalized. |\n | 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. |\n | pretrained_model_name | If stage > 0 this agent will be loaded and training can be continued. |\n | task_mode | - \"ped\" for training on pedestrians only; \"static\" for training on static objects only; \"ped_static\" for training on both, static |\n\n\n\n2. There are some predefined agents. As example I will use the ppo2_1_raw_data_disc_0 in the training session.\n\n ```\n docker run --rm -d \\\n -v :/data \\\n -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 \\\n -e AGENT_NAME=ppo2_1_raw_data_disc_0 \\\n -e NUM_SIM_ENVS=4 \\\n ros-drl_local_planner\n ```\n\n3. 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.\n ```\n docker run --rm -d \\\n -v :/data \\\n -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 \\\n -e AGENT_NAME=ppo2_1_raw_data_disc_0 \\\n -e NUM_SIM_ENVS=4 \\\n --net=host \\\n ros-drl_local_planner\n ```\n Now you can display the different simulation environments:\n * Simulation 1:\n ```\n roslaunch rl_bringup rviz.launch ns:=\"sim1\"\n ```\n * Simulation 2:\n ```\n roslaunch rl_bringup rviz.launch ns:=\"sim2\"\n ```\n * etc. ...\n\n## Train with pre-trained agents\n### Run agent trained on raw data, discrete action space, stack size 1\n ```\n docker run --rm -d \\\n -v drl_local_planner_ros_stable_baselines/example_agents:/data/agents \\\n -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 \\\n -e AGENT_NAME=ppo2_1_raw_data_disc_0_pretrained \\\n -e NUM_SIM_ENVS=4 \\\n --net=host \\\n ros-drl_local_planner\n ```\n### Run agent trained on image data, discrete action space, stack size 1\n ```\n docker run --rm -d \\\n -v drl_local_planner_ros_stable_baselines/example_agents:/data/agents \\\n -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 \\\n -e AGENT_NAME=ppo2_1_img_disc_1_pretrained \\\n -e NUM_SIM_ENVS=4 \\\n --net=host \\\n ros-drl_local_planner\n ```\n\n\n\n \n \n" }, { "path": "docker/Dockerfile", "content": "FROM ros:kinetic-robot-xenial\n\nRUN apt-get update && apt-get install -y \\\n libqt4-dev \\\n libopencv-dev \\\n liblua5.2-dev \\\n virtualenv \\\n screen \\\n python3-dev \\\n git \\\n ros-kinetic-tf2-geometry-msgs \\\n ros-kinetic-navigation \\\n ros-kinetic-rviz \n\n# Building catkin_ws\nRUN mkdir -p /usr/catkin_ws/src\nWORKDIR /usr/catkin_ws/src\nRUN git clone https://github.com/RGring/drl_local_planner_ros_stable_baselines\nRUN cp drl_local_planner_ros_stable_baselines/.rosinstall .\nRUN rosws update\nRUN /bin/bash -c '. /opt/ros/kinetic/setup.bash; cd /usr/catkin_ws; catkin_make'\n\n#Creating virtualenv for python3\nRUN virtualenv /venv_p3 --python=python3\nRUN /venv_p3/bin/pip install \\\n pyyaml \\\n rospkg \\\n catkin_pkg \\\n exception \\\n numpy \\\n tensorflow==\"1.13.1\" \\\n gym \\\n pyquaternion \\ \n mpi4py \\\n matplotlib\nRUN /venv_p3/bin/pip install -e /usr/catkin_ws/src/drl_local_planner_forks/stable-baselines\n\n\nRUN mv /usr/catkin_ws/src/drl_local_planner_ros_stable_baselines/rl_bringup/config/path_config_docker.ini \\\n/usr/catkin_ws/src/drl_local_planner_ros_stable_baselines/rl_bringup/config/path_config.ini\n\nWORKDIR /usr/catkin_ws/src/drl_local_planner_ros_stable_baselines/start_scripts/\n\nENTRYPOINT ./entrypoint_ppo2.sh \"$AGENT_NAME\" \"$NUM_SIM_ENVS\"\n" }, { "path": "flatland_setup/CMakeLists.txt", "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(flatland_setup)\n\n## Compile as C++11, supported in ROS Kinetic and newer\n# add_compile_options(-std=c++11)\n\n## Find catkin macros and libraries\n## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)\n## is used, also find other catkin packages\nfind_package(catkin REQUIRED COMPONENTS\n roscpp\n rospy\n std_msgs\n)\n\n## System dependencies are found with CMake's conventions\n# find_package(Boost REQUIRED COMPONENTS system)\n\n\n## Uncomment this if the package has a setup.py. This macro ensures\n## modules and global scripts declared therein get installed\n## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html\n# catkin_python_setup()\n\n################################################\n## Declare ROS messages, services and actions ##\n################################################\n\n## To declare and build messages, services or actions from within this\n## package, follow these steps:\n## * Let MSG_DEP_SET be the set of packages whose message types you use in\n## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).\n## * In the file package.xml:\n## * add a build_depend tag for \"message_generation\"\n## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET\n## * If MSG_DEP_SET isn't empty the following dependency has been pulled in\n## but can be declared for certainty nonetheless:\n## * add a exec_depend tag for \"message_runtime\"\n## * In this file (CMakeLists.txt):\n## * add \"message_generation\" and every package in MSG_DEP_SET to\n## find_package(catkin REQUIRED COMPONENTS ...)\n## * add \"message_runtime\" and every package in MSG_DEP_SET to\n## catkin_package(CATKIN_DEPENDS ...)\n## * uncomment the add_*_files sections below as needed\n## and list every .msg/.srv/.action file to be processed\n## * uncomment the generate_messages entry below\n## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)\n\n## Generate messages in the 'msg' folder\n# add_message_files(\n# FILES\n# Message1.msg\n# Message2.msg\n# )\n\n## Generate services in the 'srv' folder\n# add_service_files(\n# FILES\n# Service1.srv\n# Service2.srv\n# )\n\n## Generate actions in the 'action' folder\n# add_action_files(\n# FILES\n# Action1.action\n# Action2.action\n# )\n\n## Generate added messages and services with any dependencies listed here\n# generate_messages(\n# DEPENDENCIES\n# std_msgs\n# )\n\n################################################\n## Declare ROS dynamic reconfigure parameters ##\n################################################\n\n## To declare and build dynamic reconfigure parameters within this\n## package, follow these steps:\n## * In the file package.xml:\n## * add a build_depend and a exec_depend tag for \"dynamic_reconfigure\"\n## * In this file (CMakeLists.txt):\n## * add \"dynamic_reconfigure\" to\n## find_package(catkin REQUIRED COMPONENTS ...)\n## * uncomment the \"generate_dynamic_reconfigure_options\" section below\n## and list every .cfg file to be processed\n\n## Generate dynamic reconfigure parameters in the 'cfg' folder\n# generate_dynamic_reconfigure_options(\n# cfg/DynReconf1.cfg\n# cfg/DynReconf2.cfg\n# )\n\n###################################\n## catkin specific configuration ##\n###################################\n## The catkin_package macro generates cmake config files for your package\n## Declare things to be passed to dependent projects\n## INCLUDE_DIRS: uncomment this if your package contains header files\n## LIBRARIES: libraries you create in this project that dependent projects also need\n## CATKIN_DEPENDS: catkin_packages dependent projects also need\n## DEPENDS: system dependencies of this project that dependent projects also need\ncatkin_package(\n# INCLUDE_DIRS include\n# LIBRARIES flatland_setup\n# CATKIN_DEPENDS roscpp rospy std_msgs\n# DEPENDS system_lib\n)\n\n###########\n## Build ##\n###########\n\n## Specify additional locations of header files\n## Your package locations should be listed before other locations\ninclude_directories(\n# include\n ${catkin_INCLUDE_DIRS}\n)\n\n## Declare a C++ library\n# add_library(${PROJECT_NAME}\n# src/${PROJECT_NAME}/flatland_setup.cpp\n# )\n\n## Add cmake target dependencies of the library\n## as an example, code may need to be generated before libraries\n## either from message generation or dynamic reconfigure\n# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})\n\n## Declare a C++ executable\n## With catkin_make all packages are built within a single CMake context\n## The recommended prefix ensures that target names across packages don't collide\n# add_executable(${PROJECT_NAME}_node src/flatland_setup_node.cpp)\n\n## Rename C++ executable without prefix\n## The above recommended prefix causes long target names, the following renames the\n## target back to the shorter version for ease of user use\n## e.g. \"rosrun someones_pkg node\" instead of \"rosrun someones_pkg someones_pkg_node\"\n# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX \"\")\n\n## Add cmake target dependencies of the executable\n## same as for the library above\n# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})\n\n## Specify libraries to link a library or executable target against\n# target_link_libraries(${PROJECT_NAME}_node\n# ${catkin_LIBRARIES}\n# )\n\n#############\n## Install ##\n#############\n\n# all install targets should use catkin DESTINATION variables\n# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html\n\n## Mark executable scripts (Python etc.) for installation\n## in contrast to setup.py, you can choose the destination\n# install(PROGRAMS\n# scripts/my_python_script\n# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}\n# )\n\n## Mark executables and/or libraries for installation\n# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node\n# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}\n# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}\n# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}\n# )\n\n## Mark cpp header files for installation\n# install(DIRECTORY include/${PROJECT_NAME}/\n# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}\n# FILES_MATCHING PATTERN \"*.h\"\n# PATTERN \".svn\" EXCLUDE\n# )\n\n## Mark other files for installation (e.g. launch and bag files, etc.)\n# install(FILES\n# # myfile1\n# # myfile2\n# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}\n# )\n\n#############\n## Testing ##\n#############\n\n## Add gtest based cpp test target and link libraries\n# catkin_add_gtest(${PROJECT_NAME}-test test/test_flatland_setup.cpp)\n# if(TARGET ${PROJECT_NAME}-test)\n# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})\n# endif()\n\n## Add folders to be run by python nosetests\n# catkin_add_nosetests(test)\n" }, { "path": "flatland_setup/maps/complex_map_1/map.yaml", "content": "image: complex_map_1.png\nresolution: 0.05\n# origin: [-1.1, -9.05, 0.0]\n# origin: [-0.5, -1.45, 0.0]\norigin: [0.0, 0.0, 0.0]\noccupied_thresh: 0.65\nfree_thresh: 0.196\nnegate: 0\n#mode: scale\n" }, { "path": "flatland_setup/maps/complex_map_1/pedsim_scenario.xml", "content": "\n\n\n -->\n \n\n\n \n \n \n \n \n" }, { "path": "flatland_setup/maps/complex_map_1/world.yaml", "content": "properties: \n # optional, defaults to 10, number of velocity iterations for the Box2D\n # physics solver\n velocity_iterations: 10\n\n # optional, defaults to 10, number of position iterations for the Box2D\n # physics solver\n position_iterations: 10\nlayers: # Support for arbitrary number of layers\n # - name: \"robot\" # So that sensors can trigger on just the robot\n - name: \"static\"\n map: \"map.yaml\" # leading / denotes absolute file path, otherwise relative\n color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary\n - name: \"ped\"\n # map: \"../no_map/map.yaml\" # leading / denotes absolute file path, otherwise relative\n color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary\n # - name: \"2d\" # layer 0 named \"2d\"\n # map: \"map.yaml\" # leading / denotes absolute file path, otherwise relative\n # color: [0, 1, 0, 1] # List of floats [r,g,b,a] to color debug boundary\n # - name: \"3d\"\n # map: \"map.yaml\" # leading / denotes absolute file path, otherwise relative\n # color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary\n\n" }, { "path": "flatland_setup/maps/complex_map_2/map.yaml", "content": "image: complex_map_2.png\nresolution: 0.05\n# origin: [-1.1, -9.05, 0.0]\n# origin: [-0.5, -1.45, 0.0]\norigin: [0.0, 0.0, 0.0]\noccupied_thresh: 0.65\nfree_thresh: 0.196\nnegate: 0\n#mode: scale\n" }, { "path": "flatland_setup/maps/complex_map_2/pedsim_scenario.xml", "content": "\n\n\n -->\n \n\n\n \n \n \n \n \n" }, { "path": "flatland_setup/maps/complex_map_2/world.yaml", "content": "properties: \n # optional, defaults to 10, number of velocity iterations for the Box2D\n # physics solver\n velocity_iterations: 10\n\n # optional, defaults to 10, number of position iterations for the Box2D\n # physics solver\n position_iterations: 10\nlayers: # Support for arbitrary number of layers\n # - name: \"robot\" # So that sensors can trigger on just the robot\n - name: \"static\"\n map: \"map.yaml\" # leading / denotes absolute file path, otherwise relative\n color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary\n - name: \"ped\"\n # map: \"../no_map/map.yaml\" # leading / denotes absolute file path, otherwise relative\n color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary\n # - name: \"2d\" # layer 0 named \"2d\"\n # map: \"map.yaml\" # leading / denotes absolute file path, otherwise relative\n # color: [0, 1, 0, 1] # List of floats [r,g,b,a] to color debug boundary\n # - name: \"3d\"\n # map: \"map.yaml\" # leading / denotes absolute file path, otherwise relative\n # color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary\n\n" }, { "path": "flatland_setup/maps/complex_map_3/map.yaml", "content": "image: complex_map_3.png\nresolution: 0.05\n# origin: [-1.1, -9.05, 0.0]\n# origin: [-0.5, -1.45, 0.0]\norigin: [0.0, 0.0, 0.0]\noccupied_thresh: 0.65\nfree_thresh: 0.196\nnegate: 0\n#mode: scale\n" }, { "path": "flatland_setup/maps/complex_map_3/pedsim_scenario.xml", "content": "\n\n\n -->\n \n\n\n \n \n \n \n \n" }, { "path": "flatland_setup/maps/complex_map_3/world.yaml", "content": "properties: \n # optional, defaults to 10, number of velocity iterations for the Box2D\n # physics solver\n velocity_iterations: 10\n\n # optional, defaults to 10, number of position iterations for the Box2D\n # physics solver\n position_iterations: 10\nlayers: # Support for arbitrary number of layers\n # - name: \"robot\" # So that sensors can trigger on just the robot\n - name: \"static\"\n map: \"map.yaml\" # leading / denotes absolute file path, otherwise relative\n color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary\n - name: \"ped\"\n # map: \"../no_map/map.yaml\" # leading / denotes absolute file path, otherwise relative\n color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary\n # - name: \"2d\" # layer 0 named \"2d\"\n # map: \"map.yaml\" # leading / denotes absolute file path, otherwise relative\n # color: [0, 1, 0, 1] # List of floats [r,g,b,a] to color debug boundary\n # - name: \"3d\"\n # map: \"map.yaml\" # leading / denotes absolute file path, otherwise relative\n # color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary\n\n" }, { "path": "flatland_setup/maps/corridor_1/map.yaml", "content": "image: corridor_1.png\nresolution: 0.05\n# origin: [-1.1, -9.05, 0.0]\n# origin: [-0.5, -1.45, 0.0]\norigin: [0.0, 0.0, 0.0]\noccupied_thresh: 0.65\nfree_thresh: 0.196\nnegate: 0\n#mode: scale\n" }, { "path": "flatland_setup/maps/corridor_1/pedsim_scenario.xml", "content": "\n\n\n -->\n \n\n\n \n \n \n \n \n" }, { "path": "flatland_setup/maps/corridor_1/world.yaml", "content": "properties: \n # optional, defaults to 10, number of velocity iterations for the Box2D\n # physics solver\n velocity_iterations: 10\n\n # optional, defaults to 10, number of position iterations for the Box2D\n # physics solver\n position_iterations: 10\nlayers: # Support for arbitrary number of layers\n # - name: \"robot\" # So that sensors can trigger on just the robot\n - name: \"static\"\n map: \"map.yaml\" # leading / denotes absolute file path, otherwise relative\n color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary\n - name: \"ped\"\n # map: \"../no_map/map.yaml\" # leading / denotes absolute file path, otherwise relative\n color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary\n # - name: \"2d\" # layer 0 named \"2d\"\n # map: \"map.yaml\" # leading / denotes absolute file path, otherwise relative\n # color: [0, 1, 0, 1] # List of floats [r,g,b,a] to color debug boundary\n # - name: \"3d\"\n # map: \"map.yaml\" # leading / denotes absolute file path, otherwise relative\n # color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary\n\n" }, { "path": "flatland_setup/maps/corridor_2/map.yaml", "content": "image: corridor_2.png\nresolution: 0.05\n# origin: [-1.1, -9.05, 0.0]\n# origin: [-0.5, -1.45, 0.0]\norigin: [0.0, 0.0, 0.0]\noccupied_thresh: 0.65\nfree_thresh: 0.196\nnegate: 0\n#mode: scale\n" }, { "path": "flatland_setup/maps/corridor_2/pedsim_scenario.xml", "content": "\n\n\n -->\n \n\n\n \n \n \n \n \n" }, { "path": "flatland_setup/maps/corridor_2/world.yaml", "content": "properties: \n # optional, defaults to 10, number of velocity iterations for the Box2D\n # physics solver\n velocity_iterations: 10\n\n # optional, defaults to 10, number of position iterations for the Box2D\n # physics solver\n position_iterations: 10\nlayers: # Support for arbitrary number of layers\n # - name: \"robot\" # So that sensors can trigger on just the robot\n - name: \"static\"\n map: \"map.yaml\" # leading / denotes absolute file path, otherwise relative\n color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary\n - name: \"ped\"\n # map: \"../no_map/map.yaml\" # leading / denotes absolute file path, otherwise relative\n color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary\n # - name: \"2d\" # layer 0 named \"2d\"\n # map: \"map.yaml\" # leading / denotes absolute file path, otherwise relative\n # color: [0, 1, 0, 1] # List of floats [r,g,b,a] to color debug boundary\n # - name: \"3d\"\n # map: \"map.yaml\" # leading / denotes absolute file path, otherwise relative\n # color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary\n\n" }, { "path": "flatland_setup/maps/corridor_3/map.yaml", "content": "image: corridor_3.png\nresolution: 0.05\n# origin: [-1.1, -9.05, 0.0]\n# origin: [-0.5, -1.45, 0.0]\norigin: [0.0, 0.0, 0.0]\noccupied_thresh: 0.65\nfree_thresh: 0.196\nnegate: 0\n#mode: scale\n" }, { "path": "flatland_setup/maps/corridor_3/pedsim_scenario.xml", "content": "\n\n\n -->\n \n\n\n \n \n \n \n \n" }, { "path": "flatland_setup/maps/corridor_3/world.yaml", "content": "properties: \n # optional, defaults to 10, number of velocity iterations for the Box2D\n # physics solver\n velocity_iterations: 10\n\n # optional, defaults to 10, number of position iterations for the Box2D\n # physics solver\n position_iterations: 10\nlayers: # Support for arbitrary number of layers\n # - name: \"robot\" # So that sensors can trigger on just the robot\n - name: \"static\"\n map: \"map.yaml\" # leading / denotes absolute file path, otherwise relative\n color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary\n - name: \"ped\"\n # map: \"../no_map/map.yaml\" # leading / denotes absolute file path, otherwise relative\n color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary\n # - name: \"2d\" # layer 0 named \"2d\"\n # map: \"map.yaml\" # leading / denotes absolute file path, otherwise relative\n # color: [0, 1, 0, 1] # List of floats [r,g,b,a] to color debug boundary\n # - name: \"3d\"\n # map: \"map.yaml\" # leading / denotes absolute file path, otherwise relative\n # color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary\n\n" }, { "path": "flatland_setup/maps/map_middle_complexity/map.yaml", "content": "image: map_middle_complexity.png\nresolution: 0.05\norigin: [0.0, 0.0, 0.0]\noccupied_thresh: 0.65\nfree_thresh: 0.196\nnegate: 0\n\n" }, { "path": "flatland_setup/maps/map_middle_complexity/pedsim_scenario.xml", "content": "\n\n\n \n \n \n \n \n \n \n \n \n \n \n \n \n\n \n \n \n \n\n \n \n \n \n \n\n \n \n \n \n\n \n \n \n \n\n \n \n \n \n\n \n \n \n \n\n \n \n \n \n \n \n \n\n\n -->\n \n\n\n \n \n \n \n \n" }, { "path": "flatland_setup/maps/map_middle_complexity/world.yaml", "content": "properties: \n # optional, defaults to 10, number of velocity iterations for the Box2D\n # physics solver\n velocity_iterations: 10\n\n # optional, defaults to 10, number of position iterations for the Box2D\n # physics solver\n position_iterations: 10\nlayers: # Support for arbitrary number of layers\n - name: \"ped\"\n color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary\n - name: \"static\"\n map: \"map.yaml\" # leading / denotes absolute file path, otherwise relative\n color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary\n " }, { "path": "flatland_setup/maps/no_map/map.yaml", "content": "# image: no_map.png\nimage: no_map.png\nresolution: 0.05\norigin: [-0.5, -1.45, 0.0]\noccupied_thresh: 0.65\nfree_thresh: 0.196\nnegate: 0\n#mode: scale\n" }, { "path": "flatland_setup/maps/open_field_1/map.yaml", "content": "image: open_field_1.png\nresolution: 0.05\n# origin: [-1.1, -9.05, 0.0]\n# origin: [-0.5, -1.45, 0.0]\norigin: [0.0, 0.0, 0.0]\noccupied_thresh: 0.65\nfree_thresh: 0.196\nnegate: 0\n#mode: scale\n" }, { "path": "flatland_setup/maps/open_field_1/pedsim_scenario.xml", "content": "\n\n\n -->\n \n\n\n \n \n \n \n \n" }, { "path": "flatland_setup/maps/open_field_1/world.yaml", "content": "properties: \n # optional, defaults to 10, number of velocity iterations for the Box2D\n # physics solver\n velocity_iterations: 10\n\n # optional, defaults to 10, number of position iterations for the Box2D\n # physics solver\n position_iterations: 10\nlayers: # Support for arbitrary number of layers\n # - name: \"robot\" # So that sensors can trigger on just the robot\n - name: \"static\"\n map: \"map.yaml\" # leading / denotes absolute file path, otherwise relative\n color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary\n - name: \"ped\"\n # map: \"../no_map/map.yaml\" # leading / denotes absolute file path, otherwise relative\n color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary\n # - name: \"2d\" # layer 0 named \"2d\"\n # map: \"map.yaml\" # leading / denotes absolute file path, otherwise relative\n # color: [0, 1, 0, 1] # List of floats [r,g,b,a] to color debug boundary\n # - name: \"3d\"\n # map: \"map.yaml\" # leading / denotes absolute file path, otherwise relative\n # color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary\n\n" }, { "path": "flatland_setup/maps/open_field_2/map.yaml", "content": "image: open_field_2.png\nresolution: 0.05\n# origin: [-1.1, -9.05, 0.0]\n# origin: [-0.5, -1.45, 0.0]\norigin: [0.0, 0.0, 0.0]\noccupied_thresh: 0.65\nfree_thresh: 0.196\nnegate: 0\n#mode: scale\n" }, { "path": "flatland_setup/maps/open_field_2/pedsim_scenario.xml", "content": "\n\n\n -->\n \n\n\n \n \n \n \n \n" }, { "path": "flatland_setup/maps/open_field_2/world.yaml", "content": "properties: \n # optional, defaults to 10, number of velocity iterations for the Box2D\n # physics solver\n velocity_iterations: 10\n\n # optional, defaults to 10, number of position iterations for the Box2D\n # physics solver\n position_iterations: 10\nlayers: # Support for arbitrary number of layers\n # - name: \"robot\" # So that sensors can trigger on just the robot\n - name: \"static\"\n map: \"map.yaml\" # leading / denotes absolute file path, otherwise relative\n color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary\n - name: \"ped\"\n # map: \"../no_map/map.yaml\" # leading / denotes absolute file path, otherwise relative\n color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary\n # - name: \"2d\" # layer 0 named \"2d\"\n # map: \"map.yaml\" # leading / denotes absolute file path, otherwise relative\n # color: [0, 1, 0, 1] # List of floats [r,g,b,a] to color debug boundary\n # - name: \"3d\"\n # map: \"map.yaml\" # leading / denotes absolute file path, otherwise relative\n # color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary\n\n" }, { "path": "flatland_setup/maps/open_field_3/agent_scenario.xml", "content": "\n\n\n\n\n\n\n\n\n\n\n\n\n \n \n \n \n\n\n\n \n \n\n\n\n\n\n\n \n \n \n \n\n\n\n \n \n \n \n" }, { "path": "flatland_setup/maps/open_field_3/map.yaml", "content": "image: open_field_3.png\nresolution: 0.05\n# origin: [-1.1, -9.05, 0.0]\n# origin: [-0.5, -1.45, 0.0]\norigin: [0.0, 0.0, 0.0]\noccupied_thresh: 0.65\nfree_thresh: 0.196\nnegate: 0\n#mode: scale\n" }, { "path": "flatland_setup/maps/open_field_3/pedsim_scenario.xml", "content": "\n\n\n -->\n \n\n\n \n \n \n \n \n" }, { "path": "flatland_setup/maps/open_field_3/world.yaml", "content": "properties: \n # optional, defaults to 10, number of velocity iterations for the Box2D\n # physics solver\n velocity_iterations: 10\n\n # optional, defaults to 10, number of position iterations for the Box2D\n # physics solver\n position_iterations: 10\nlayers: # Support for arbitrary number of layers\n # - name: \"robot\" # So that sensors can trigger on just the robot\n - name: \"static\"\n map: \"map.yaml\" # leading / denotes absolute file path, otherwise relative\n color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary\n - name: \"ped\"\n # map: \"../no_map/map.yaml\" # leading / denotes absolute file path, otherwise relative\n color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary\n # - name: \"2d\" # layer 0 named \"2d\"\n # map: \"map.yaml\" # leading / denotes absolute file path, otherwise relative\n # color: [0, 1, 0, 1] # List of floats [r,g,b,a] to color debug boundary\n # - name: \"3d\"\n # map: \"map.yaml\" # leading / denotes absolute file path, otherwise relative\n # color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary\n\n" }, { "path": "flatland_setup/objects/cylinder.model.yaml", "content": "# Round obstacle\n\nbodies: # List of named bodies\n - name: cylinder\n pose: [0, 0, 0] \n type: dynamic \n color: [0.2, 0.8, 0.2, 0.75] \n footprints:\n - type: circle\n radius: 0.5\n center: [0, 0]\n density: 0\n layers: [\"static\"]\n collision: false\n - type: circle\n radius: 0.02\n center: [0.2, 0.1]\n density: 0\n layers: [\"static\"]\n collision: false\n - type: circle\n radius: 0.02\n center: [0.2, -0.1]\n density: 0\n layers: [\"static\"]\n collision: false\n - type: circle\n radius: 0.02\n center: [-0.2, 0.1]\n density: 0\n layers: [\"static\"]\n collision: false\n - type: circle\n radius: 0.02\n center: [-0.2, -0.1]\n density: 0\n layers: [\"static\"]\n collision: false\n" }, { "path": "flatland_setup/objects/palett.model.yaml", "content": "bodies: # List of named bodies\n - name: pallet\n pose: [0, 0, 0] \n type: dynamic \n color: [0.2, 0.8, 0.2, 0.75] \n footprints:\n - type: polygon\n points: [[-0.6, -0.4], [-0.6, 0.4], [0.6, 0.4], [0.6, -0.4]]\n density: 0.0\n layers: [\"static\"]\n collision: false\n - name: dummy_for_laser\n pose: [0, 0, 0] \n type: dynamic \n color: [1.0, 1.0, 1.0, 0.75] \n footprints:\n - type: circle\n radius: 0.02\n center: [0.25, 0.15]\n density: 0.0\n layers: [\"static\"]\n collision: false\n - type: circle\n radius: 0.02\n center: [0.25, -0.15]\n density: 0.0\n layers: [\"static\"]\n collision: false\n - type: circle\n radius: 0.02\n center: [-0.25, 0.15]\n density: 0.0\n layers: [\"static\"]\n collision: false\n - type: circle\n radius: 0.02\n center: [-0.25, -0.15]\n density: 0.0\n layers: [\"static\"]\n collision: false" }, { "path": "flatland_setup/objects/person_single_circle.model.yaml", "content": "# Person\nbodies: # List of named bodies\n - name: base\n pose: [0, 0, 0] \n type: dynamic \n color: [1.0, 1.0, 1.0, 0.75] \n footprints:\n - type: circle\n radius: 0.001\n center: [0, 0]\n density: 0\n layers: []\n collision: false\n\n - name: left_leg\n pose: [0, 0, 0] \n type: dynamic \n color: [0.66, 0.0, 0, 0.75] \n footprints:\n - type: circle\n radius: 0.1\n center: [0.0, 0]\n density: 1\n # layers: [\"2d\", \"ped\"]\n layers: [\"ped\"]\n collision: false\n\n - name: right_leg\n pose: [0, 0, 0] \n type: dynamic \n color: [0.66, 0.0, 0, 0.75] \n footprints:\n - type: circle\n radius: 0.1\n center: [-0.0, 0]\n density: 1\n # layers: [\"2d\", \"ped\"]\n layers: [\"ped\"]\n collision: false\n\nplugins:\n - type: PedsimMovement\n name: pedsim_movement\n agent_topic: /pedsim_simulator/simulated_agents\n base_body: base\n left_leg_body: left_leg\n right_leg_body: right_leg\n update_rate: 40\n toggle_leg_movement: false\n leg_offset: 0.0 \n var_leg_offset: 0.0\n step_time: 0.3 #(in sec)\n var_step_time: 0.1\n leg_radius: 0.2 \n var_leg_radius: 0.03\n enabled: true\n\n # - type: ModelTfPublisher\n # name: tf_publisher_\n # exclude: [\"left_leg\", \"right_leg\"]\n # publish_tf_world: true\n\n\n" }, { "path": "flatland_setup/objects/person_two_legged.model.yaml", "content": "# Person\nbodies: # List of named bodies\n - name: base\n pose: [0, 0, 0] \n type: dynamic \n color: [1.0, 1.0, 1.0, 0.75] \n footprints:\n - type: circle\n radius: 0.001\n center: [0, 0]\n density: 0\n layers: []\n collision: false\n\n - name: left_leg\n pose: [0, 0, 0] \n type: dynamic \n color: [0.66, 0.0, 0, 0.75] \n footprints:\n - type: circle\n radius: 0.1\n center: [0.0, 0]\n density: 1\n # layers: [\"2d\", \"ped\"]\n layers: [\"ped\"]\n collision: false\n\n - name: right_leg\n pose: [0, 0, 0] \n type: dynamic \n color: [0.66, 0.0, 0, 0.75] \n footprints:\n - type: circle\n radius: 0.1\n center: [-0.0, 0]\n density: 1\n # layers: [\"2d\", \"ped\"]\n layers: [\"ped\"]\n collision: false\n\n\n# joints:\n# - type: weld\n# name: left_leg_weld\n# bodies: \n# - name: base\n# anchor: [0, 1]\n# - name: left_leg\n# anchor: [0, 0]\n\n# - type: weld\n# name: right_leg_weld\n# bodies: \n# - name: base\n# anchor: [0, -1]\n# - name: right_leg\n# anchor: [0, 0]\n\n\nplugins:\n - type: PedsimMovement\n name: pedsim_movement\n agent_topic: /pedsim_simulator/simulated_agents\n base_body: base\n left_leg_body: left_leg\n right_leg_body: right_leg\n update_rate: 40\n toggle_leg_movement: true \n leg_offset: 0.3 \n var_leg_offset: 0.05 \n step_time: 0.6 #(in sec)\n var_step_time: 0.1\n leg_radius: 0.1 \n var_leg_radius: 0.02\n\n # - type: ModelTfPublisher\n # name: tf_publisher_\n # exclude: [\"left_leg\", \"right_leg\"]\n # publish_tf_world: true\n\n\n" }, { "path": "flatland_setup/objects/wagon.model.yaml", "content": "# Round obstacle\n\nbodies: # List of named bodies\n - name: foot1\n pose: [0, 0, 0] \n type: dynamic \n color: [0.2, 0.8, 0.2, 0.75] \n footprints:\n - type: circle\n radius: 0.1\n center: [0.25, 0.4]\n density: 0\n layers: [\"static\"]\n collision: false\n - name: foot2\n pose: [0, 0, 0] \n type: dynamic \n color: [0.2, 0.8, 0.2, 0.75] \n footprints:\n - type: circle\n radius: 0.1\n center: [-0.25, 0.4]\n density: 0\n layers: [\"static\"]\n collision: false\n - name: foot3\n pose: [0, 0, 0] \n type: dynamic \n color: [0.2, 0.8, 0.2, 0.75] \n footprints:\n - type: circle\n radius: 0.1\n center: [-0.25, -0.4]\n density: 0\n layers: [\"static\"]\n collision: false\n - name: foot4\n pose: [0, 0, 0] \n type: dynamic \n color: [0.2, 0.8, 0.2, 0.75] \n footprints:\n - type: circle\n radius: 0.1\n center: [0.25, -0.4]\n density: 0\n layers: [\"static\"]\n collision: false\n\n" }, { "path": "flatland_setup/package.xml", "content": "\n\n flatland_setup\n 0.0.0\n The flatland_setup package\n\n \n \n \n ronja\n\n\n \n \n \n TODO\n\n\n \n \n \n \n\n\n \n \n \n \n\n\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n catkin\n roscpp\n rospy\n std_msgs\n roscpp\n rospy\n std_msgs\n roscpp\n rospy\n std_msgs\n\n\n \n \n \n\n \n\n" }, { "path": "flatland_setup/robot/robot1.model.yaml", "content": "bodies: # List of named bodies\n - name: base_footprint\n pose: [0, 0, 0] \n type: dynamic \n color: [0.5, 0.5, 0.5, 1.0] \n footprints:\n - type: polygon \n sensor: true\n points: [[-0.445, -0.29], [-0.445, 0.29], [0.445, 0.29], [0.445, -0.29]]\n density: 0\n layers: [\"static\"]\n collision: false\n\nplugins:\n - type: DiffDrive \n name: diff_drive \n body: base_footprint\n odom_frame_id: odom\n odom_pub: odom # topic odom is published on\n pub_rate: 10\n \n - type: Laser\n name: static_laser\n frame: static_laser_link\n topic: static_laser\n body: base_footprint\n broadcast_tf: true\n origin: [0.0, 0.0, 0.0]\n range: 10\n # angle: {min: -3.14159265359, max: 3.14159265359, increment: 0.05235987755}\n angle: {min: -3.14159265359, max: 3.14159265359, increment: 0.06981317007}\n noise_std_dev: 0.015\n update_rate: 10\n layers: [\"static\"]\n\n - type: Laser\n name: ped_laser\n frame: ped_laser_link\n topic: ped_laser\n body: base_footprint\n broadcast_tf: true\n origin: [0.0, 0.0, 0.0]\n range: 10\n # angle: {min: -3.14159265359, max: 3.14159265359, increment: 0.05235987755}\n angle: {min: -3.14159265359, max: 3.14159265359, increment: 0.06981317007}\n noise_std_dev: 0.015\n update_rate: 10\n layers: [\"ped\"]\n\n - type: ModelTfPublisher\n name: tf_publisher\n publish_tf_world: false\n # exclude: [\"approx_footprint\"]\n" }, { "path": "requirements.txt", "content": "absl-py==0.7.1\nalabaster==0.7.12\nargh==0.26.2\nastor==0.7.1\natari-py==0.1.7\nattrs==19.1.0\nBabel==2.6.0\ncatkin-pkg==0.4.11\ncertifi==2019.3.9\nchardet==3.0.4\nClick==7.0\ncloudpickle==0.8.1\nconfigparser==4.0.2\ncoverage==4.5.3\ncycler==0.10.0\ndill==0.2.9\ndocutils==0.14\nexception==0.1.0\nfuture==0.17.1\ngast==0.2.2\nglob2==0.6\ngrpcio==1.19.0\ngym==0.12.1\nh5py==2.9.0\nidna==2.8\nimagesize==1.1.0\nJinja2==2.10.1\njoblib==0.13.2\nKeras-Applications==1.0.7\nKeras-Preprocessing==1.0.9\nkiwisolver==1.0.1\nlivereload==2.6.0\nMarkdown==3.1\nMarkupSafe==1.1.1\nmatplotlib==3.0.3\nmock==2.0.0\nmore-itertools==7.0.0\nmpi4py==3.0.1\nnumpy==1.17.0\nopencv-python==4.0.0.21\npackaging==19.0\npandas==0.24.2\npathtools==0.1.2\npbr==5.1.3\nPillow==6.0.0\npkg-resources==0.0.0\npluggy==0.6.0\nport-for==0.3.1\nprogressbar2==3.39.3\nprotobuf==3.7.1\npy==1.8.0\npyglet==1.3.2\nPygments==2.3.1\nPyOpenGL==3.1.0\npyparsing==2.4.0\npyquaternion==0.9.5\npytest==3.5.1\npytest-cov==2.6.1\npython-dateutil==2.8.0\npython-utils==2.3.0\npytz==2018.9\nPyYAML==5.1\npyzmq==18.0.1\nrequests==2.21.0\nrospkg==1.1.7\nscipy==1.2.1\nseaborn==0.9.0\nsix==1.12.0\nsnowballstemmer==1.2.1\nSphinx==2.0.1\nsphinx-autobuild==0.7.1\nsphinx-rtd-theme==0.4.3\nsphinxcontrib-applehelp==1.0.1\nsphinxcontrib-devhelp==1.0.1\nsphinxcontrib-htmlhelp==1.0.1\nsphinxcontrib-jsmath==1.0.1\nsphinxcontrib-qthelp==1.0.2\nsphinxcontrib-serializinghtml==1.1.3\n-e git+git@github.com:RGring/stable-baselines.git@94ccad6d2da0c715d671f2f8f7dad22ef320cd5c#egg=stable_baselines\ntensorboard==1.13.1\ntensorflow==1.13.1\ntensorflow-estimator==1.13.0\ntermcolor==1.1.0\ntornado==6.0.2\ntqdm==4.31.1\nurllib3==1.24.1\nwatchdog==0.9.0\nWerkzeug==0.15.2\nzmq==0.0.0\n" }, { "path": "rl_agent/CMakeLists.txt", "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(rl_agent)\n\n## Compile as C++11, supported in ROS Kinetic and newer\n# add_compile_options(-std=c++11)\n\n# Get the information about this package's buildtime dependencies\n find_package(catkin REQUIRED\n COMPONENTS \n rospy \n roscpp \n message_generation \n std_msgs \n sensor_msgs \n laser_geometry\n geometry_msgs\n nav_msgs\n rl_msgs\n tf\n pcl_ros\n )\n\n find_package(PCL REQUIRED)\n\n catkin_python_setup()\n\n\n # Declare that this catkin package's runtime dependencies\n catkin_package(\n CATKIN_DEPENDS \n stable_baselines \n message_runtime \n std_msgs \n rl_msgs \n sensor_msgs \n geometry_msgs\n )\n\n include_directories(\n ${catkin_INCLUDE_DIRS}\n include\n scripts\n )\n\n link_directories(${catkin_LIBRARY_DIRS})\n\n add_executable(laser_scan_merger\n src/laser_scan_merger.cpp\n )\n\n add_dependencies(laser_scan_merger\n ${${PROJECT_NAME}_EXPORTED_TARGETS}\n ${catkin_EXPORTED_TARGETS}\n )\n\n target_link_libraries(laser_scan_merger\n ${catkin_LIBRARIES}\n )\n\n add_executable(tf_python\n src/tf_python.cpp\n )\n\n add_dependencies(tf_python\n ${${PROJECT_NAME}_EXPORTED_TARGETS}\n ${catkin_EXPORTED_TARGETS}\n )\n\n target_link_libraries(tf_python\n ${catkin_LIBRARIES}\n )\n\n catkin_install_python(PROGRAMS\n scripts/train_scripts/train_ppo.py\n scripts/run_scripts/run_ppo.py\n DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})\n \n install(DIRECTORY launch/\n DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch/\n )\n\n install(PROGRAMS\n start_scripts/run_launch_in_venv.sh\n start_scripts/run_launch_in_venv_robot.sh\n DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/start_scripts/\n )\n\n install(\n TARGETS\n laser_scan_merger\n tf_python\n DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}\n)" }, { "path": "rl_agent/include/rl_agent/laser_scan_merger.h", "content": "/*\n * @name\t \t laser_scan_merger.h\n * @brief\t \t Merges several scans to a single one with coordinate system in robot_frame\n * @author \tRonja Gueldenring\n * @date \t \t2019/04/05\n * @comment \tCode snippets are taken from: https://github.com/iralabdisco/ira_laser_tools\n **/\n\n#ifndef LASER_SCAN_MERGER_H\n#define LASER_SCAN_MERGER_H\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\nnamespace rl_agent {\n /**\n * This class merges several laserscan to one single laser scan.\n */\n class LaserScanMerger{\n public:\n LaserScanMerger(const ros::NodeHandle& node_handle);\n private:\n ros::NodeHandle nh_;\n laser_geometry::LaserProjection projector_;\n tf::TransformListener listener_;\n\n /**\n * \n * @brief Generates a Laserscan from a pointcloud\n * \n **/ \n sensor_msgs::LaserScanPtr pointcloud_to_laserscan(Eigen::MatrixXf points, pcl::PCLPointCloud2 *merged_cloud);\n\n /**\n * merge_service_ + merge_scan_callback\n * @brief Merges several laserscan to one.\n * \n **/ \n ros::ServiceServer merge_service_;\n bool merge_scan_callback(rl_msgs::MergeScans::Request& request, rl_msgs::MergeScans::Response& response);\n \n double angle_increment_, time_increment_, range_min_, range_max_, scan_time_;\n double max_height_, min_height_;\n std::string robot_frame_;\n };\n};\n#endif /* LASER_SCAN_MERGER_H */\n" }, { "path": "rl_agent/include/rl_agent/tf_python.h", "content": "/**\n * \n * Ronja Gueldenring\n * his class is needed, because rospy and python3.5 is not compatible regarding \n * the tf-package.\n * The class provides all transformation needed by rl_agent in python\n * \n**/\n\n#ifndef TF_PYTHON_H\n#define TF_PYTHON_H\n\n#include \n#include \n#include \n\nnamespace rl_agent {\n /**\n * @class TFPython\n * @brief this class is needed, because rospy and python3.5 is not compatible regarding \n * the tf-package.\n * The class provides all transformation needed by rl_agent in python\n *\n */\n class TFPython{\n public:\n TFPython(const ros::NodeHandle& node_handle);\n void robot_to_goal_transform();\n\n private:\n ros::NodeHandle nh_;\n\n geometry_msgs::PoseStamped goal_;\n ros::Subscriber goal_sub_;\n void goal_callback(const geometry_msgs::PoseStamped& goal);\n\n ros::Publisher transformed_goal_pub_;\n tf::TransformListener listener_;\n\n };\n};\n#endif /* TF_PYTHON_H */\n" }, { "path": "rl_agent/launch/run_1_img_disc.launch", "content": "\n \n \n" }, { "path": "rl_agent/launch/run_1_raw_cont.launch", "content": "\n \n \n" }, { "path": "rl_agent/launch/run_1_raw_disc.launch", "content": "\n \n \n" }, { "path": "rl_agent/launch/run_3_raw_disc.launch", "content": "\n \n \n" }, { "path": "rl_agent/launch/run_ppo_agent.launch", "content": "\n \n \n" }, { "path": "rl_agent/package.xml", "content": "\n\n rl_agent\n 0.0.0\n The rl_agent package\n\n \n \n \n ronja\n\n\n \n \n \n TODO\n\n\n \n \n \n \n\n\n \n \n \n \n\n\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n catkin\n rospy\n roscpp\n std_msgs\n rl_msgs\n sensor_msgs\n geometry_msgs\n message_generation\n stable_baselines\n rospy\n roscpp\n std_msgs\n geometry_msgs\n sensor_msgs\n rospy\n roscpp\n std_msgs\n rl_msgs\n message_runtime\n geometry_msgs\n sensor_msgs\n stable_baselines\n\n\n \n \n \n\n \n\n" }, { "path": "rl_agent/scripts/evaluation/analysis.py", "content": "'''\n @name: analysis.py\n @brief: Analysis of evaluation data\n @author: Ronja Gueldenring\n @version: 3.5\n @date: 2019/04/05\n'''\nimport os\nhome = os.path.expanduser(\"~\")\nimport rospkg\nfrom rl_agent.evaluation.Analysis_eval import Analysis\nimport pickle\nimport configparser\n\n\ndef analyse(complexity, evaluation_file_path, reward_file_path, save_path):\n analysis = Analysis()\n results = analysis.load_results(evaluation_file_path)\n print(\"loaded data: %s\"%evaluation_file_path)\n\n timesteps = analysis.get_timestep_list(results)\n # timesteps = analysis.reconstruct_timestep_array(timesteps)\n success, time_exceeded, collision = analysis.get_scores(results)\n\n\n if (complexity == \"train\"):\n [timesteps_tb, reward] = analysis.get_reward(reward_file_path)\n else:\n perc_success_drive = analysis.get_percentual_success_drive(results)\n\n path_ratio = analysis.get_path_length_ratio(results)\n\n speed = analysis.get_speed(results)\n\n print(\"saving results...\")\n\n # Collecting all results in a dict\n analysis_results = {}\n analysis_results[\"timesteps\"] = timesteps\n analysis_results[\"success\"] = success\n analysis_results[\"time_exceeded\"] = time_exceeded\n analysis_results[\"collision\"] = collision\n\n # Saving results\n if complexity == \"train\":\n analysis_results[\"timesteps_tb\"] = timesteps_tb\n analysis_results[\"reward\"] = reward\n else:\n analysis_results[\"perc_success_drive\"] = perc_success_drive\n analysis_results[\"path_ratio\"] = path_ratio\n analysis_results[\"speed\"] = speed\n\n with open('%s.pickle' % (save_path), 'wb') as handle:\n pickle.dump(analysis_results, handle, protocol=pickle.HIGHEST_PROTOCOL)\n\nif __name__ == '__main__':\n complexity = \"complex_map_3\" # train, simple, average, complex, follow_path,\n task_type = \"ped\" # static or ped\n no_episodes = 700\n agent_names = [\"ppo2_109\", \"ppo2_99\", \"ppo2_107\", \"ppo2_108\"]\n\n\n rospack = rospkg.RosPack()\n rl_bringup_path = rospack.get_path('rl_bringup')\n config = configparser.ConfigParser()\n config.read('%s/config/path_config.ini' % rl_bringup_path)\n path_to_eval_data_train = config['PATHES']['path_to_eval_data_train']\n path_to_eval_data_test = config['PATHES']['path_to_eval_data_test']\n\n\n\n for agent_name in agent_names:\n if complexity == \"train\":\n evaluation_file_path = \"%s/%s_training\"%(path_to_eval_data_train, agent_name)\n elif complexity == \"follow_path\":\n evaluation_file_path = \"%s/%s_following_path\"%(path_to_eval_data_test, agent_name)\n else:\n evaluation_file_path = \"%s/%s_%s_eval_set_%s_%d\" % (path_to_eval_data_test, agent_name, task_type, complexity, no_episodes)\n\n reward_file_path = \"%s/run_%s_reward\" % (path_to_eval_data_train, agent_name)\n\n # Saving results\n if complexity == \"train\":\n save_path = \"%s/%s_analysis\" % (path_to_eval_data_train, agent_name)\n elif complexity == \"follow_path\":\n save_path = \"%s/%s_analysis_follow_path\" % (path_to_eval_data_test, agent_name)\n else:\n save_path = \"%s/%s_%s_analysis_eval_set_%s_%s\" % (\n path_to_eval_data_test, agent_name, task_type, complexity, no_episodes)\n\n analyse(complexity, evaluation_file_path, reward_file_path, save_path)\n\n" }, { "path": "rl_agent/scripts/evaluation/evaluate_agent.py", "content": "'''\n @name: evaluate_agent.py\n @brief: Evaluates an agent according to a test set.\n @author: Ronja Gueldenring\n @version: 3.5\n @date: 2019/04/05\n'''\nimport os\nhome = os.path.expanduser(\"~\")\n\nimport rospy\nimport rospkg\nfrom multiprocessing import Process\nfrom scripts.run_scripts.run_ppo import run_ppo\nfrom rl_agent.env_utils.state_collector import StateCollector\nfrom rl_agent.evaluation.Evaluation import Evaluation\nimport time\nimport configparser\n\n\ndef evaluate(ns, sc, evaluation_set_path, save_path):\n rospy.init_node(\"evaluate_node\", anonymous=True)\n eval = Evaluation(sc, ns)\n time.sleep(2)\n eval.evaluate_set(evaluation_set_path, save_path)\n\n\nif __name__ == '__main__':\n task_type = \"ped\" # static, ped\n complexity = \"complex_map_3\" # simple, average, complex\n no_episodes = 700\n ns = \"sim1\"\n approach = \"PPO2\" # PPO1, PPO2\n policy = [\"CnnPolicy\", \"CNN1DPolicy_multi_input\", \"CnnPolicy\", \"CnnPolicy\"]\n disc_action_space = [False, True, True, True]\n agent_names = [\"ppo2_109\", \"ppo2_99\", \"ppo2_107\", \"ppo2_108\"]\n num_stacks = [1,1,1,3]\n stack_offset = 15\n\n rospack = rospkg.RosPack()\n rl_bringup_path = rospack.get_path('rl_bringup')\n config = configparser.ConfigParser()\n config.read('%s/config/path_config.ini' % rl_bringup_path)\n path_to_eval_sets = config['PATHES']['path_to_eval_sets']\n path_to_eval_data_test = config['PATHES']['path_to_eval_data_test']\n\n evaluation_set_name = \"%s_eval_set_%s_%d\"%(task_type, complexity, no_episodes)\n evaluation_set_path = \"%s/%s\"%(path_to_eval_sets, evaluation_set_name)\n mode = \"exec\"\n\n for i, agent_name in enumerate(agent_names):\n save_path = \"%s/%s_%s\" % (path_to_eval_data_test, agent_name, evaluation_set_name)\n sc = StateCollector(ns, \"eval\")\n\n p = Process(target=run_ppo, args=(config, sc, approach, agent_name , policy[i] , mode, task_type,\n stack_offset, num_stacks[i], True, False, disc_action_space[i], ns))\n p.start()\n\n print(\"Starting evaluation of agent %s with set %s\"%(agent_name, evaluation_set_name))\n print(\"--------------------------------------------------------------------------------------\")\n p_eval = Process(target=evaluate, args=(ns, sc, evaluation_set_path, save_path))\n p_eval.start()\n p_eval.join()\n p.terminate()\n\n\n" }, { "path": "rl_agent/scripts/evaluation/qualitative_plot.py", "content": "'''\n @name: qualitative_plot.py\n @brief: Generates a situation in rviz, that shows the agents qualitative behaviour.\n @author: Ronja Gueldenring\n @version: 3.5\n @date: 2019/04/05\n'''\nimport os\nhome = os.path.expanduser(\"~\")\n\nimport rospy\nimport rospkg\nfrom rl_agent.evaluation.Analysis_eval import Analysis\nfrom rl_agent.evaluation.Evaluation import Evaluation\nfrom rl_agent.env_utils.state_collector import StateCollector\nimport time\nimport configparser\n\n\nif __name__ == '__main__':\n rospy.init_node(\"qualitative_image_generation_node\", anonymous=True)\n\n ##########################################\n #### Training/Testin setup properties ####\n #### Please adjust. ####\n ##########################################\n ns = \"sim1\"\n complexity = \"average\" # simple, average, complex\n task_type = \"static\" # static, ped\n no_episodes = 3\n agent_name_1 = \"ppo2_foo_512\"\n agent_name_2 = \"\"\n\n rospack = rospkg.RosPack()\n rl_bringup_path = rospack.get_path('rl_bringup')\n config = configparser.ConfigParser()\n config.read('%s/config/path_config.ini' % rl_bringup_path)\n path_to_eval_sets = config['PATHES']['path_to_eval_sets']\n path_to_eval_data_test = config['PATHES']['path_to_eval_data_test']\n path_to_agent_1_results = \"%s/%s_static_eval_set_%s_%d\" % (path_to_eval_data_test, agent_name_1, complexity, no_episodes)\n path_to_agent_2_results = \"%s/%s_static_eval_set_%s_%d\" % (path_to_eval_data_test, agent_name_2, complexity, no_episodes)\n path_to_evaluation_set = \"%s/%s_eval_set_%s_%d\"%(path_to_eval_sets, task_type, complexity, no_episodes)\n print(\"Loading sets...\")\n\n analysis = Analysis()\n results_agent_2 = [] # Empty, if no second results are needed.\n results_agent_1 = analysis.load_results(path_to_agent_1_results)\n if(task_type == \"static\"):\n if agent_name_2 != \"\":\n results_agent_2 = analysis.load_results(path_to_agent_1_results)\n\n eval = Evaluation(StateCollector(ns, \"eval\"), ns)\n time.sleep(2)\n print(\"Printing set.\")\n len_eval_set = eval.load_evaluation_set(path_to_evaluation_set)\n\n for i in range(0, len_eval_set):\n episode_pos = 10\n while episode_pos >= 0:\n if (task_type == \"static\"):\n eval.generate_qualitative_static_image_rviz(results_agent_1, results_agent_2, i, episode_pos)\n elif (task_type == \"ped\"):\n eval.generate_qualitative_ped_image_rviz(results_agent_1, i, episode_pos)\n episode_pos = int(input(\"Pos of episode %d ? (-1 for plotting next episode)\" % i) or \"50\")\n\n\n" }, { "path": "rl_agent/scripts/evaluation/save_test_episodes.py", "content": "'''\n @name: save_test_episodes.py\n @brief: For saving n different episodes. Each episode needs to be rewiewed by the user and the\n following key press, trigger the followin action:\n [Y] Episode is added to the evaluation set\n [n] Episode is rejected\n [s] set is saved\n [e] quit program\n @author: Ronja Gueldenring\n @version: 3.5\n @date: 2019/04/05\n'''\n\nimport os\nhome = os.path.expanduser(\"~\")\n\nimport rospy\nimport rospkg\nimport pickle\nfrom rl_agent.env_utils.task_generator import TaskGenerator\nfrom rl_agent.env_utils.state_collector import StateCollector\nimport math\nfrom operator import itemgetter\nimport time\nimport configparser\n\n\n\nif __name__ == '__main__':\n rospy.init_node(\"episode_collector\", anonymous=True)\n\n ##########################################\n #### Training/Testin setup properties ####\n #### Please adjust. ####\n ##########################################\n ns = \"sim1\"\n name = \"\"\n task_type = \"ped\" # static, ped\n complexity = \"complex_map_1\" # simple, average, complex\n\n rospack = rospkg.RosPack()\n rl_bringup_path = rospack.get_path('rl_bringup')\n config = configparser.ConfigParser()\n config.read('%s/config/path_config.ini' % rl_bringup_path)\n path_to_eval_sets = config['PATHES']['path_to_eval_sets']\n save_path = \"%s/%s_eval_set_%s\"%(path_to_eval_sets, task_type, complexity)\n state_collector = StateCollector(ns, \"train\")\n t_g = TaskGenerator(ns, state_collector, 0.56)\n\n if name != \"\":\n with open('%s/%s.pickle' % (path_to_eval_sets, name), 'rb') as handle:\n episodes = pickle.load(handle)\n count = len(episodes)\n else:\n episodes = []\n count = 0\n\n # Sleeping so that py-Publisher has time to setup!\n time.sleep(1)\n while True:\n print(\"Episode details:\")\n\n if (task_type == \"static\"):\n d = t_g.set_random_static_task()\n print(\"\\t -Start: (%d, %d)\"%(d[\"start\"][0], d[\"start\"][1]))\n print(\"\\t -Goal: (%d, %d)\"%(d[\"goal\"][0], d[\"goal\"][1]))\n print(\"\\t -Obstacles\")\n for i in range(len(d[\"static_objects\"][\"x\"])):\n print(\"\\t \\t *%s: (%f, %f)\"% (d[\"static_objects\"][\"model_name\"][i], d[\"static_objects\"][\"x\"][i], d[\"static_objects\"][\"y\"][i]))\n print(\"\\n\")\n elif (task_type == \"ped\"):\n d = t_g.set_random_ped_task()\n for i in range(10):\n t_g.take_sim_step()\n time.sleep(0.05)\n\n print(\"\\t -Start: (%d, %d)\" % (d[\"start\"][0], d[\"start\"][1]))\n print(\"\\t -Goal: (%d, %d)\" % (d[\"goal\"][0], d[\"goal\"][1]))\n print(\"\\t -Peds\")\n ped_count = 0\n for ped in d[\"peds\"]:\n ped_poses = []\n for i in ped[2]:\n ped_poses.append(i)\n print(\"\\t\\t*Ped%d\"%ped_count)\n for i in ped_poses:\n print(\"\\t\\t \", i)\n ped_count +=1\n print(\"\\n\")\n\n else:\n print(\"wrong task_type!\")\n break\n\n path_length = 0\n if (len(d[\"path\"].poses) <=2):\n print(\"Path length <= 2\")\n continue\n old_pose = d[\"path\"].poses[0].pose.position\n for pose in d[\"path\"].poses[1:]:\n new_pose = pose.pose.position\n path_length += math.sqrt(math.pow((new_pose.x - old_pose.x), 2) + math.pow((new_pose.y - old_pose.y), 2))\n old_pose = new_pose\n d[\"path_length\"] = path_length\n print(\"path_length: %f\"%path_length)\n print(\"path_start: (%f, %f)\"%(d[\"path\"].poses[0].pose.position.x, d[\"path\"].poses[0].pose.position.y))\n\n char = str(input(\"Episode valid? [Y][n]\"))\n if (char == \"n\"):\n print(\"Episode rejected!\")\n elif (char == \"\" or char == \"y\" or char == \"Y\"):\n episodes.append(d)\n print(\"Added episode %d\"%count)\n count+=1\n elif (char == \"s\"):\n print(\"Saving episodes...\")\n episodes = sorted(episodes, key=itemgetter('path_length'))\n with open('%s_%d.pickle'%(save_path, count), 'wb') as handle:\n pickle.dump(episodes, handle, protocol=pickle.HIGHEST_PROTOCOL)\n elif (char == \"e\"):\n break\n" }, { "path": "rl_agent/scripts/run_scripts/run_ppo.py", "content": "#! /usr/bin/env python\n'''\n @name: run_ppo.py\n @brief: Trained agent is loaded and executed.\n @author: Ronja Gueldenring\n @version: 3.5\n @date: 2019/04/05\n'''\n\n\nimport os\nimport sys\nimport rospy\nimport rospkg\nimport configparser\nimport time\nimport numpy as np\nfrom rl_agent.env_wrapper.ros_env_cont_img import RosEnvContImg\nfrom rl_agent.env_wrapper.ros_env_cont_raw_data import RosEnvContRaw\nfrom rl_agent.env_wrapper.ros_env_disc_raw_data import RosEnvDiscRaw\nfrom rl_agent.env_wrapper.ros_env_cont_raw_scan_prep_wp import RosEnvContRawScanPrepWp\nfrom rl_agent.env_wrapper.ros_env_disc_raw_scan_prep_wp import RosEnvDiscRawScanPrepWp\nfrom rl_agent.env_wrapper.ros_env_cont_img_vel import RosEnvContImgVel\nfrom rl_agent.env_wrapper.ros_env_disc_img_vel import RosEnvDiscImgVel\nfrom rl_agent.env_wrapper.ros_env_disc_img import RosEnvDiscImg\nfrom rl_agent.env_utils.state_collector import StateCollector\n\n\nfrom stable_baselines.common.vec_env import DummyVecEnv, VecNormalize, VecFrameStack\nfrom stable_baselines.ppo2.ppo2 import PPO2\nfrom stable_baselines.ppo1.pposgd_simple import PPO1\n\ndef load_train_env(ns, state_collector, robot_radius, rew_fnc, num_stacks,\n stack_offset, debug, task_mode, rl_mode, policy, disc_action_space, normalize):\n # Choosing environment wrapper according to the policy\n if policy == \"CnnPolicy\" or policy == \"CnnLnLstmPolicy\" or policy == \"CnnLstmPolicy\":\n if disc_action_space:\n env_temp = RosEnvDiscImg\n else:\n env_temp = RosEnvContImg\n elif policy in [\"CNN1DPolicy\", \"CNN1DPolicy2\", \"CNN1DPolicy3\"]:\n if disc_action_space:\n env_temp = RosEnvDiscRawScanPrepWp\n else:\n env_temp = RosEnvContRawScanPrepWp\n elif policy == \"CNN1DPolicy_multi_input\":\n if disc_action_space:\n env_temp = RosEnvDiscRaw\n else:\n env_temp = RosEnvContRaw\n elif policy == \"CnnPolicy_multi_input_vel\" or policy == \"CnnPolicy_multi_input_vel2\":\n if disc_action_space:\n env_temp = RosEnvDiscImgVel\n else:\n env_temp = RosEnvContImgVel\n\n\n env_raw = DummyVecEnv([lambda: env_temp(ns, state_collector, stack_offset, num_stacks, robot_radius, rew_fnc, debug, rl_mode, task_mode)])\n\n if normalize:\n env = VecNormalize(env_raw, training=True, norm_obs=True, norm_reward=False, clip_obs=100.0, clip_reward=10.0,\n gamma=0.99, epsilon=1e-08)\n else:\n env = env_raw\n\n # Stack of data?\n if num_stacks > 1:\n env = VecFrameStack(env, n_stack=num_stacks, n_offset=stack_offset)\n\n return env\n\n\ndef run_ppo(config, state_collector, agent_name =\"ppo_99_8507750\", policy =\"CnnPolicy\", mode=\"train\", task_mode=\"static\",\n stack_offset=15, num_stacks=1, debug=True, normalize = True, disc_action_space = False, ns=\"\"):\n\n path_to_models = config['PATHES']['path_to_models']\n\n # Loading agent\n model = PPO2.load(\"%s/%s/%s.pkl\" % (path_to_models, agent_name, agent_name))\n\n print(\"Loaded %s\" % agent_name)\n print(\"--------------------------------------------------\")\n print(\"Normalize: \", normalize)\n print(\"Policy: %s\" % policy)\n print(\"Discrete action space: \", disc_action_space)\n print(\"Observation space size: \", model.observation_space.shape)\n print(\"Debug: \", debug)\n print(\"Number of stacks: %d, stack offset: %d\" % ( model.observation_space.shape[2], stack_offset))\n print(\"\\n\")\n\n\n #Loading environment\n env = load_train_env(ns, state_collector, 0.46, 19, num_stacks, stack_offset, debug, task_mode, mode, policy, disc_action_space, normalize)\n\n # Resetting environment\n if mode == \"train\" or mode == \"eval\":\n obs = env.reset()\n if mode == \"exec\" or mode == \"exec_rw\":\n if disc_action_space:\n obs, rewards, dones, info = env.step([5])\n else:\n obs, rewards, dones, info = env.step([[0.0, 0.0]])\n\n if debug:\n #Cummulative reward.\n cum_reward = 0\n while True:\n #Determining action vor given observation\n action, _states = model.predict(obs)\n\n # Clipping actions\n if not disc_action_space:\n action = np.maximum(np.minimum(model.action_space.high, action), model.action_space.low)\n\n #Executing action in environment\n obs, rewards, dones, info = env.step(action)\n\n if debug:\n cum_reward += rewards\n\n # Episode over?\n if dones:\n print(\"Episode finished with reward of %f.\"% cum_reward)\n cum_reward = 0\n\n time.sleep(0.0001)\n if rospy.is_shutdown():\n print('shutdown')\n break\n\n\nif __name__ == '__main__':\n\n rospack = rospkg.RosPack()\n rl_bringup_path = rospack.get_path('rl_bringup')\n config = configparser.ConfigParser()\n config.read('%s/config/path_config.ini'%rl_bringup_path)\n\n # for running from terminal (e.g. launch-file)\n if (len(sys.argv) > 1):\n ns = \"sim1\"\n sc = StateCollector(ns, str(sys.argv[3]))\n run_ppo(config, sc,\n ns=ns,\n agent_name=str(sys.argv[1]),\n policy=str(sys.argv[2]),\n mode=str(sys.argv[3]),\n debug=bool(int(sys.argv[4])),\n normalize=bool(int(sys.argv[5])),\n disc_action_space=bool(int(sys.argv[6])),\n task_mode=str(sys.argv[7]),\n num_stacks=int(sys.argv[8]))\n\n\n # for quick testing\n else:\n mode = \"train\"\n ns = \"sim1\"\n policy = \"CnnPolicy_multi_input_vel2\"\n agent_name = \"ppo2_35_8001000\"\n\n sc = StateCollector(ns, mode)\n\n run_ppo(config, sc,\n agent_name= agent_name,\n policy = policy,\n mode = mode,\n debug = True,\n normalize=False,\n disc_action_space=True,\n task_mode=\"ped\",\n num_stacks=4,\n stack_offset = 15,\n ns=ns)\n" }, { "path": "rl_agent/scripts/train_scripts/train_ppo.py", "content": "'''\n @name: train_ppo.py\n @brief: Starts a ppo2-training process. It is expected that move_base, simulation etc is started.(roslaunch rl_setup_bringup setup.launch)\n @author: Ronja Gueldenring\n @version: 3.5\n @date: 2019/04/05\n'''\nimport os\nimport sys\nimport rospy\nimport rospkg\nimport configparser\nfrom rl_agent.env_wrapper.ros_env_cont_img import RosEnvContImg\nfrom rl_agent.env_wrapper.ros_env_cont_raw_data import RosEnvContRaw\nfrom rl_agent.env_wrapper.ros_env_disc_raw_data import RosEnvDiscRaw\nfrom rl_agent.env_wrapper.ros_env_cont_img_vel import RosEnvContImgVel\nfrom rl_agent.env_wrapper.ros_env_disc_img_vel import RosEnvDiscImgVel\nfrom rl_agent.env_wrapper.ros_env_cont_raw_scan_prep_wp import RosEnvContRawScanPrepWp\nfrom rl_agent.env_wrapper.ros_env_disc_raw_scan_prep_wp import RosEnvDiscRawScanPrepWp\nfrom rl_agent.env_wrapper.ros_env_disc_img import RosEnvDiscImg\nfrom rl_agent.env_utils.state_collector import StateCollector\nfrom stable_baselines.common.vec_env import VecNormalize, SubprocVecEnv, VecFrameStack\nfrom rl_agent.evaluation.Evaluation import Evaluation\nfrom multiprocessing import Process\nimport random\nfrom rl_agent.common_custom_policies import *\nfrom stable_baselines.common.policies import *\n\nfrom stable_baselines.ppo2 import PPO2\nfrom stable_baselines.bench import Monitor\nfrom stable_baselines.results_plotter import load_results, ts2xy\nimport numpy as np\n\nbest_mean_reward, n_callback = -np.inf, 0\nagent_name = \"\"\npath_to_models = \"\"\n\n\ndef train_callback(_locals, _globals):\n \"\"\"\n Callback called at each step (for DQN an others) or after n steps (see ACER or PPO2)\n :param _locals: (dict)\n :param _globals: (dict)\n \"\"\"\n global n_callback, best_mean_reward, agent_name, path_to_models\n # Print stats every 1000 calls\n if (n_callback + 1) % 10 == 0:\n\n # Evaluate policy performance\n x, y = ts2xy(load_results('%s/%s/'%(path_to_models, agent_name)), 'timesteps')\n if len(x) > 0:\n mean_reward = np.mean(y[-100:])\n print(x[-1], 'timesteps')\n print(\"Best mean reward: {:.2f} - Last mean reward per episode: {:.2f}\".format(best_mean_reward, mean_reward))\n\n # New best model, you could save the agent here\n if mean_reward > best_mean_reward:\n best_mean_reward = mean_reward\n # Example for saving best model\n print(\"Saving new best model\")\n _locals['self'].save(path_to_models + '/%s/%s.pkl' % (agent_name, agent_name))\n n_callback += 1\n return True\n\ndef load_train_env(num_envs, robot_radius, rew_fnc, num_stacks, stack_offset, debug, task_mode, policy, disc_action_space, normalize):\n # Choosing environment wrapper according to the policy\n if policy == \"CnnPolicy\" or policy == \"CnnLnLstmPolicy\" or policy == \"CnnLstmPolicy\":\n if disc_action_space:\n env_temp = RosEnvDiscImg\n else:\n env_temp = RosEnvContImg\n elif policy == \"CNN1DPolicy\":\n if disc_action_space:\n env_temp = RosEnvDiscRawScanPrepWp\n else:\n env_temp = RosEnvContRawScanPrepWp\n elif policy == \"CNN1DPolicy_multi_input\":\n if disc_action_space:\n env_temp = RosEnvDiscRaw\n else:\n env_temp = RosEnvContRaw\n elif policy == \"CnnPolicy_multi_input_vel\" or policy == \"CnnPolicy_multi_input_vel2\":\n if disc_action_space:\n env_temp = RosEnvDiscImgVel\n else:\n env_temp = RosEnvContImgVel\n\n 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)])\n\n # Normalizing?\n if normalize:\n env = VecNormalize(env, training=True, norm_obs=True, norm_reward=False, clip_obs=100.0, clip_reward=10.0,\n gamma=0.99, epsilon=1e-08)\n else:\n env = env\n\n # Stack of data?\n if num_stacks > 1:\n env = VecFrameStack(env, n_stack=num_stacks, n_offset=stack_offset)\n\n return env\n\ndef train_agent_ppo2(config, agent_name, total_timesteps, policy,\n gamma=0.99, n_steps=128, ent_coef=0.01, learning_rate=0.00025,\n vf_coef=0.5, max_grad_norm=0.5, lam=0.95, nminibatches=4, noptepochs=4,\n cliprange=0.2, num_envs=1, robot_radius = 0.46, rew_fnc=3, num_stacks=1, stack_offset=15, disc_action_space = False,\n debug=False, normalize=False,\n stage=0, pretrained_model_name=\"\", task_mode=\"static\"):\n\n # Setting seed\n seed = random.randint(0,1000)\n np.random.seed(seed)\n tf.random.set_random_seed(seed)\n random.seed(seed)\n\n # Define pathes to store things\n path_to_tensorboard_log = config['PATHES']['path_to_tensorboard_log']\n global path_to_models\n path_to_models = config['PATHES']['path_to_models']\n\n agent_dir='%s/%s'%(path_to_models, agent_name)\n if not os.path.exists(agent_dir):\n os.makedirs(agent_dir)\n\n\n\n # Loading simulation environment\n env = load_train_env(num_envs,\n robot_radius,\n rew_fnc,\n num_stacks,\n stack_offset,\n debug,\n task_mode,\n policy,\n disc_action_space,\n normalize)\n\n\n\n if stage==0:\n model = PPO2(eval(policy), env, gamma=gamma,\n n_steps=n_steps, ent_coef=ent_coef,\n learning_rate=learning_rate, vf_coef=vf_coef, max_grad_norm=max_grad_norm,\n lam=lam, nminibatches=nminibatches, noptepochs=noptepochs,\n cliprange=cliprange, verbose=1,\n tensorboard_log='%s' % (path_to_tensorboard_log))\n else:\n # Pretrained model is loaded to continue training.\n model = PPO2.load(\"%s/%s/%s.pkl\" % (path_to_models, pretrained_model_name, pretrained_model_name), env,\n tensorboard_log='%s'%(path_to_tensorboard_log))\n\n # Document agent\n print(\"Starting PPO2 Training of agent: %s\" %(agent_name))\n print(\"------------------------------------------------------\")\n print(\"gamma \\t\\t\\t\\t %f\" %model.gamma)\n print(\"n_steps \\t\\t\\t %d\" %model.n_steps)\n print(\"ent_coef \\t\\t\\t %f\" %model.ent_coef)\n print(\"learning_rate \\t\\t\\t %f\" %learning_rate)\n print(\"vf_coef \\t\\t\\t %f\" %model.vf_coef)\n print(\"max_grad_norm \\t\\t\\t %f\" %model.max_grad_norm)\n print(\"lam \\t\\t\\t\\t %f\" %model.lam)\n print(\"nminibatches \\t\\t\\t %d\" %model.nminibatches)\n print(\"noptepochs \\t\\t\\t %d\" %model.noptepochs)\n print(\"cliprange \\t\\t\\t %f\" %cliprange)\n print(\"total_timesteps \\t\\t %d\" %total_timesteps)\n print(\"Policy \\t\\t\\t\\t %s\" %policy)\n print(\"reward_fnc \\t\\t\\t %d\" %rew_fnc)\n print(\"Normalized state: %d\" % normalize)\n print(\"discrete action space %d\" % disc_action_space)\n print(\"Number of stacks: %d, stack offset: %d\" % (num_stacks, stack_offset))\n print(\"\\n\")\n\n # Starting training\n reset_num_timesteps = False\n if stage==0:\n reset_num_timesteps = True\n\n model.learn(total_timesteps=total_timesteps, log_interval=100, callback=train_callback, tb_log_name=agent_name, reset_num_timesteps=reset_num_timesteps)\n\n # Saving final model\n model.save(\"%s/%s/%s\" % (path_to_models, agent_name, \"%s_stage_%d\" % (agent_name, stage)))\n print(\"Training finished.\")\n env.close()\n\ndef evaluate_during_training(ns, save_path, robot_radius):\n rospy.init_node(\"evaluate_node\", anonymous=True)\n eval = Evaluation(StateCollector(ns, \"train\"), ns, robot_radius=robot_radius)\n eval.evaluate_training(save_path)\n\nif __name__ == '__main__':\n record_evaluation_data = False\n\n rospack = rospkg.RosPack()\n rl_bringup_path = rospack.get_path('rl_bringup')\n config = configparser.ConfigParser()\n config.read('%s/config/path_config.ini'%rl_bringup_path)\n path_to_eval_data_train = config['PATHES']['path_to_eval_data_train']\n\n # for running via ./entrypoint_ppo2.sh\n if (len(sys.argv) > 1):\n agent_name = str(sys.argv[1])\n stage = int(sys.argv[20])\n\n record_processes = []\n if record_evaluation_data:\n save_path = \"%s/%s_training\" % (path_to_eval_data_train, str(sys.argv[1]))\n for i in range(int(sys.argv[23])):\n p = Process(target=evaluate_during_training, args=(\"sim%d\" % (i + 1), save_path, float(sys.argv[14])))\n p.start()\n record_processes.append(p)\n\n\n train_agent_ppo2(config, agent_name, int(sys.argv[2]), str(sys.argv[3]), gamma=float(sys.argv[4]),\n n_steps=int(sys.argv[5]), ent_coef=float(sys.argv[6]),\n learning_rate=float(sys.argv[7]), vf_coef=float(sys.argv[8]),\n max_grad_norm=float(sys.argv[9]), lam=float(sys.argv[10]),\n nminibatches=int(sys.argv[11]), noptepochs=int(sys.argv[12]),\n cliprange=float(sys.argv[13]), robot_radius=float(sys.argv[14]),\n rew_fnc=float(sys.argv[15]), num_stacks=int(sys.argv[16]),\n stack_offset=int(sys.argv[17]),\n disc_action_space=bool(int(sys.argv[18])), normalize=bool(int(sys.argv[19])),\n stage=stage,\n pretrained_model_name = str(sys.argv[21]),\n task_mode=str(sys.argv[22]), num_envs=int(sys.argv[23]))\n\n for p in record_processes:\n p.terminate()\n\n # for quick testing\n else:\n\n num_envs = 1\n stage = 0\n agent_name = \"ppo2_foo\"\n robot_radius = 0.5\n\n record_processes = []\n if record_evaluation_data:\n save_path = \"%s/%s_training\" % (path_to_eval_data_train, agent_name)\n for i in range(num_envs):\n p = Process(target=evaluate_during_training, args=(\"sim%d\"%(i+1), save_path, robot_radius))\n p.start()\n record_processes.append(p)\n\n train_agent_ppo2(config,\n agent_name,\n gamma=0.99,\n n_steps=128,\n ent_coef=0.005,\n learning_rate=0.00025,\n cliprange=0.2,\n total_timesteps=10000000,\n policy=\"CNN1DPolicy_multi_input\",\n num_envs=num_envs,\n nminibatches=1,\n noptepochs=1,\n debug=True,\n rew_fnc = 19,\n num_stacks= 3,\n stack_offset=5,\n disc_action_space=False,\n robot_radius = robot_radius,\n stage=stage,\n pretrained_model_name=\"ppo2_foo\",\n task_mode=\"ped\")\n\n for p in record_processes:\n p.terminate()\n" }, { "path": "rl_agent/setup.py", "content": "## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD\n\nfrom distutils.core import setup\nfrom catkin_pkg.python_setup import generate_distutils_setup\n\n# fetch values from package.xml\nsetup_args = generate_distutils_setup(\n packages=['rl_agent',\n 'rl_agent.env_wrapper',\n 'rl_agent.env_utils',\n 'rl_agent.evaluation'],\n package_dir={'': 'src'},\n)\n\nsetup(**setup_args)" }, { "path": "rl_agent/src/__init__.py", "content": "" }, { "path": "rl_agent/src/laser_scan_merger.cpp", "content": "/*\n * @name\t \tlaser_scan_merger.cpp\n * @brief\t \tMerges several scans to a single one with coordinate system in robot_frame\n * @author \tRonja Gueldenring\n * @date \t \t2019/04/05\n * @comment \tCode snippets are taken from: https://github.com/iralabdisco/ira_laser_tools\n **/\n#include \n\nnamespace rl_agent {\n\n\tLaserScanMerger::LaserScanMerger(const ros::NodeHandle& node_handle): nh_(node_handle){\n\t\tstd::string merge_service_name_ = ros::this_node::getName() + \"/merge_scans\";\n merge_service_ = nh_.advertiseService(\"merge_scans\", &LaserScanMerger::merge_scan_callback, this);\n nh_.getParam(\"rl_agent/robot_frame\", robot_frame_);\n\t\tmin_height_ = 0.0;\n\t\tmax_height_ = 0.25;\n\t}\n\n bool LaserScanMerger::merge_scan_callback(rl_msgs::MergeScans::Request& request, \n rl_msgs::MergeScans::Response& response){\n\n int num_laser_scan = request.scans.size();\n if (num_laser_scan <= 0){\n return false;\n }\n\n\t\t//Scan properties of first scan are saved for merged scan\n this->angle_increment_ = request.scans[0].angle_increment;\n\t this->time_increment_ = request.scans[0].time_increment;\n this->scan_time_ = request.scans[0].scan_time;\n\t this->range_min_ = request.scans[0].range_min;\n\t this->range_max_ = request.scans[0].range_max;\n\n\t\t// Each scan is transformed to a pointcloud\n pcl::PCLPointCloud2 clouds[num_laser_scan];\n for (int i = 0; i < num_laser_scan; i++){\n sensor_msgs::LaserScan scan_in = request.scans[i];\n if(!listener_.waitForTransform(scan_in.header.frame_id, robot_frame_,\n scan_in.header.stamp + ros::Duration().fromSec(scan_in.ranges.size()*scan_in.time_increment),\n ros::Duration(1.0))){\n ROS_WARN(\"Duration exceeded\");\n return false;\n }\n\n sensor_msgs::PointCloud cloud_temp_1;\n sensor_msgs::PointCloud2 cloud_temp_2;\n projector_.transformLaserScanToPointCloud(robot_frame_ , scan_in , cloud_temp_1 , listener_);\n sensor_msgs::convertPointCloudToPointCloud2(cloud_temp_1 , cloud_temp_2);\n pcl_conversions::toPCL(cloud_temp_2, clouds[i]);\n }\n\n\t\t// All pointclouds are concatenated\n pcl::PCLPointCloud2 merged_cloud = clouds[0];\n for(int i=1; iheader = pcl_conversions::fromPCL(merged_cloud->header);\n\t\toutput->header.frame_id = robot_frame_;\n\t\toutput->angle_min = -M_PI;\n\t\toutput->angle_max = M_PI;\n\t\toutput->angle_increment = this->angle_increment_;\n\t\toutput->time_increment = this->time_increment_;\n\t\toutput->scan_time = this->scan_time_;\n\t\toutput->range_min = this->range_min_;\n\t\toutput->range_max = this->range_max_;\n\n\t\tuint32_t ranges_size = std::ceil((output->angle_max - output->angle_min) / output->angle_increment);\n\t\toutput->ranges.assign(ranges_size, output->range_max + 1.0);\n\n\t\tfor(int i=0; irange_min * output->range_min;\n\t\t\tif (range_sq < range_min_sq_) {\n\t\t\t\tROS_DEBUG(\"rejected for range %f below minimum value %f. Point: (%f, %f, %f)\", range_sq, range_min_sq_, x, y, z);\n\t\t\t\tcontinue;\n\t\t\t}\n\n\t\t\tdouble angle = atan2(y, x);\n\t\t\tif (angle < output->angle_min || angle > output->angle_max)\n\t\t\t{\n\t\t\t\tROS_DEBUG(\"rejected for angle %f not in range (%f, %f)\\n\", angle, output->angle_min, output->angle_max);\n\t\t\t\tcontinue;\n\t\t\t}\n\t\t\tint index = (angle - output->angle_min) / output->angle_increment;\n\n\n\t\t\tif (output->ranges[index] * output->ranges[index] > range_sq)\n\t\t\t\toutput->ranges[index] = sqrt(range_sq);\n\t\t}\n\t\treturn output;\n\t}//pointcloud_to_laserscan\n}; // namespace rl_agent\n\nint main(int argc, char** argv){\n ros::init(argc, argv, \"laser_scan_merger\");\n ros::NodeHandle node;\n rl_agent::LaserScanMerger ls(node);\n\n while (ros::ok()) {\n ros::spin();\n }\n return 0;\n};" }, { "path": "rl_agent/src/rl_agent/__init__.py", "content": "" }, { "path": "rl_agent/src/rl_agent/common_custom_policies.py", "content": "'''\n @name: state_collector.py\n @brief: This class provides custom policies used in stable-baselines library\n @author: Ronja Gueldenring\n @version: 3.5\n @date: 2019/04/05\n'''\n\nfrom stable_baselines.common.policies import *\nimport stable_baselines.common.policies as common\nimport rospy\nimport numpy as np\n\nNS=\"sim1\"\n\n\n######################################\n###### CNN1DPolicy_multi_input #######\n######################################\n\ndef laser_cnn_multi_input(state, **kwargs):\n \"\"\"\n 1D Conv Network\n\n :param state: (TensorFlow Tensor) state input placeholder\n :param kwargs: (dict) Extra keywords parameters for the convolutional layers of the CNN\n :return: (TensorFlow Tensor) The CNN output layer\n \"\"\"\n # scan = tf.squeeze(state[:, : , 0:kwargs['laser_scan_len'] , :], axis=1)\n scan = tf.squeeze(state[:, : , 0:kwargs['laser_scan_len'] , :], axis=1)\n wps = tf.squeeze(state[:, :, kwargs['laser_scan_len']:, -1], axis=1)\n # goal = tf.math.multiply(goal, 6)\n\n kwargs_conv = {}\n activ = tf.nn.relu\n layer_1 = activ(conv1d(scan, 'c1d_1', n_filters=32, filter_size=5, stride=2, init_scale=np.sqrt(2), **kwargs_conv))\n layer_2 = activ(conv1d(layer_1, 'c1d_2', n_filters=64, filter_size=3, stride=2, init_scale=np.sqrt(2), **kwargs_conv))\n layer_2 = conv_to_fc(layer_2)\n layer_3 = activ(linear(layer_2, 'fc1', n_hidden=256, init_scale=np.sqrt(2)))\n temp = tf.concat([layer_3, wps], 1)\n layer_4 = activ(linear(temp, 'fc2', n_hidden=128, init_scale=np.sqrt(2)))\n return layer_4\n\n\n\nclass CNN1DPolicy_multi_input(common.FeedForwardPolicy):\n \"\"\"\n This class provides a 1D convolutional network for the Raw Data Representation\n \"\"\"\n def __init__(self, *args, **kwargs):\n kwargs[\"laser_scan_len\"] = rospy.get_param(\"%s/rl_agent/scan_size\"%NS, 360)\n super(CNN1DPolicy_multi_input, self).__init__(*args, **kwargs, cnn_extractor=laser_cnn_multi_input, feature_extraction=\"cnn\")\n\n\n\n##########################\n###### CNN1DPolicy #######\n##########################\ndef laser_cnn_stack(state, **kwargs):\n \"\"\"\n 1D Conv Network\n\n :param state: (TensorFlow Tensor) Scan input placeholder\n :param kwargs: (dict) Extra keywords parameters for the convolutional layers of the CNN\n :return: (TensorFlow Tensor) The CNN output layer\n \"\"\"\n\n scan = state[:, : , 0 , :]\n wps = tf.expand_dims(state[:, :, 1, -1], -1)\n scan_con_wps = tf.concat([scan, wps], -1)\n # goal = tf.math.multiply(goal, 6)\n\n kwargs_conv = {}\n activ = tf.nn.relu\n layer_1 = activ(conv1d(scan_con_wps, 'c1d_1', n_filters=32, filter_size=5, stride=2, init_scale=np.sqrt(2), **kwargs_conv))\n layer_2 = activ(conv1d(layer_1, 'c1d_2', n_filters=64, filter_size=3, stride=2, init_scale=np.sqrt(2), **kwargs_conv))\n layer_2 = conv_to_fc(layer_2)\n layer_3 = activ(linear(layer_2, 'fc1', n_hidden=256, init_scale=np.sqrt(2)))\n # print_goal = tf.print(wps)\n # with tf.control_dependencies([print_goal]):\n layer_4 = activ(linear(layer_3, 'fc2', n_hidden=128, init_scale=np.sqrt(2)))\n return layer_4\n\n\nclass CNN1DPolicy(common.FeedForwardPolicy):\n \"\"\"\n This class provides a 1D convolutional network for the Polar Representation\n \"\"\"\n def __init__(self, *args, **kwargs):\n super(CNN1DPolicy, self).__init__(*args, **kwargs, cnn_extractor=laser_cnn_stack, feature_extraction=\"cnn\")\n\n\n########################################\n###### CnnPolicy_multi_input_vel #######\n########################################\ndef nature_cnn_multi_input(state, **kwargs):\n \"\"\"\n CNN from Nature paper. state[0:-1, :, :] is the input image, while state[-1, :, :]\n provides additional information, that is concenated later with the output pf layer 3.\n It can be additional non-image information.\n\n :param scaled_images: (TensorFlow Tensor) Image input placeholder\n :param kwargs: (dict) Extra keywords parameters for the convolutional layers of the CNN\n :return: (TensorFlow Tensor) The CNN output layer\n \"\"\"\n img = state[:, 0:-1 , : , :]\n vel = state[:, -1, 0:kwargs['multi_input_size'], 0]\n vel = tf.math.multiply(vel, kwargs['max_image_value'])\n kwargs_conv = {}\n activ = tf.nn.relu\n layer_1 = activ(conv(img, 'c1', n_filters=32, filter_size=8, stride=4, init_scale=np.sqrt(2), **kwargs_conv))\n\n layer_2 = activ(conv(layer_1, 'c2', n_filters=64, filter_size=4, stride=2, init_scale=np.sqrt(2), **kwargs_conv))\n layer_3 = activ(conv(layer_2, 'c3', n_filters=64, filter_size=3, stride=1, init_scale=np.sqrt(2), **kwargs_conv))\n layer_3 = conv_to_fc(layer_3)\n concat_img_goal = tf.concat([layer_3, vel], 1)\n # For printing uncomment: velocity\n # print_vel = tf.print(vel)\n # with tf.control_dependencies([print_vel]):\n layer_4 = activ(linear(concat_img_goal, 'fc1', n_hidden=512, init_scale=np.sqrt(2)))\n return layer_4\n\n\nclass CnnPolicy_multi_input_vel(common.FeedForwardPolicy):\n \"\"\"\n This class provides a 2D convolutional network for the X-Image Speed Representation\n \"\"\"\n def __init__(self, *args, **kwargs):\n kwargs[\"multi_input_size\"] = 2\n kwargs[\"max_image_value\"] = 100\n super(CnnPolicy_multi_input_vel, self).__init__(*args, **kwargs, cnn_extractor=nature_cnn_multi_input, feature_extraction=\"cnn\")\n\n\n\n\n#########################################\n###### CnnPolicy_multi_input_vel2 #######\n#########################################\ndef cnn_multi_input(state, **kwargs):\n \"\"\"\n CNN from Nature paper. state[0:-1, :, :] is the input image, while state[-1, :, :]\n provides additional information, that is concenated later with the output pf layer 3.\n It can be additional non-image information.\n\n :param scaled_images: (TensorFlow Tensor) Image input placeholder\n :param kwargs: (dict) Extra keywords parameters for the convolutional layers of the CNN\n :return: (TensorFlow Tensor) The CNN output layer\n \"\"\"\n img = state[:, 0:-1 , : , :]\n vel = state[:, -1, 0:kwargs['multi_input_size'], -1]\n vel = tf.math.multiply(vel, kwargs['max_image_value'])\n kwargs_conv = {}\n activ = tf.nn.relu\n layer_1 = activ(conv(img, 'c1', n_filters=64, filter_size=8, stride=4, init_scale=np.sqrt(2), **kwargs_conv))\n\n layer_2 = activ(conv(layer_1, 'c2', n_filters=64, filter_size=4, stride=2, init_scale=np.sqrt(2), **kwargs_conv))\n layer_3 = activ(conv(layer_2, 'c3', n_filters=32, filter_size=3, stride=1, init_scale=np.sqrt(2), **kwargs_conv))\n layer_4 = activ(conv(layer_3, 'c4', n_filters=32, filter_size=2, stride=1, init_scale=np.sqrt(2), **kwargs_conv))\n layer_4 = conv_to_fc(layer_4)\n concat_img_goal = tf.concat([layer_4, vel], 1)\n # For printing uncomment: velocity\n # print_vel = tf.print(vel)\n # with tf.control_dependencies([print_vel]):\n layer_4 = activ(linear(concat_img_goal, 'fc1', n_hidden=512, init_scale=np.sqrt(2)))\n layer_5 = activ(linear(layer_4, 'fc2', n_hidden=216, init_scale=np.sqrt(2)))\n return layer_5\n\n\nclass CnnPolicy_multi_input_vel2(common.FeedForwardPolicy):\n \"\"\"\n This class provides a 2D convolutional network for the X-Image Speed Representation\n \"\"\"\n def __init__(self, *args, **kwargs):\n kwargs[\"multi_input_size\"] = 2\n kwargs[\"max_image_value\"] = 100\n super(CnnPolicy_multi_input_vel2, self).__init__(*args, **kwargs, cnn_extractor=cnn_multi_input, feature_extraction=\"cnn\")\n\n\n# Deprecated\nclass CnnPolicy_multi_input_vel3(common.FeedForwardPolicy):\n \"\"\"\n CnnPolicy_multi_input_vel3 == CnnPolicy_multi_input_vel2\n Is needed because some agents are trained on this policy-name and is still needed for execution mode\n \"\"\"\n def __init__(self, *args, **kwargs):\n kwargs[\"multi_input_size\"] = 2\n kwargs[\"max_image_value\"] = 100\n super(CnnPolicy_multi_input_vel3, self).__init__(*args, **kwargs, cnn_extractor=cnn_multi_input, feature_extraction=\"cnn\")\n\n" }, { "path": "rl_agent/src/rl_agent/env_utils/__init__.py", "content": "" }, { "path": "rl_agent/src/rl_agent/env_utils/debug_ros_env.py", "content": "'''\n @name: debug_ros_env.py\n @brief: This class provides debugging methods RL-relevant data.\n @author: Ronja Gueldenring\n @version: 3.5\n @date: 2019/04/05\n'''\n\n#ros-relevant\nimport rospy\nfrom sensor_msgs.msg import Image\nfrom std_msgs.msg import Float64\nfrom nav_msgs.msg import OccupancyGrid\nfrom visualization_msgs.msg import Marker\nfrom geometry_msgs.msg import PointStamped\nfrom sensor_msgs.msg import LaserScan\nfrom collections import deque\n\n#python-relevant\nimport numpy as np\n\n\nclass DebugRosEnv():\n \"\"\"\n This class serves as debugger for RL-relevant data like:\n - input state\n - rewards\n \"\"\"\n def __init__(self, ns, stack_offset):\n self.__ns = ns\n self.__stack_offset = stack_offset\n print(\"stack_offset: %d\"%self.__stack_offset)\n self.__input_images = deque(maxlen=4 * self.__stack_offset)\n\n #Input state\n self.__input_img_pub1 = rospy.Publisher('%s/state_image1' % (self.__ns), Image, queue_size=1)\n self.__input_img_pub2 = rospy.Publisher('%s/state_image2' % (self.__ns), Image, queue_size=1)\n self.__input_img_pub3 = rospy.Publisher('%s/state_image3' % (self.__ns), Image, queue_size=1)\n self.__input_img_pub4 = rospy.Publisher('%s/state_image4' % (self.__ns), Image, queue_size=1)\n self.__occ_grid_pub = rospy.Publisher('%s/rl_map' % (self.__ns), OccupancyGrid, queue_size=1)\n self.__input_scan_pub = rospy.Publisher('%s/state_scan' % (self.__ns), LaserScan, queue_size=1)\n\n # reward info\n self.__rew_pub = rospy.Publisher('%s/reward' % (self.__ns), Marker, queue_size=1)\n self.__rew_num_pub = rospy.Publisher('%s/reward_num' % (self.__ns), Float64, queue_size=1)\n\n # Waypoint info\n self.__wp_pub1 = rospy.Publisher('%s/wp_vis1' % (self.__ns), PointStamped, queue_size=1)\n self.__wp_pub2 = rospy.Publisher('%s/wp_vis2' % (self.__ns), PointStamped, queue_size=1)\n self.__wp_pub3 = rospy.Publisher('%s/wp_vis3' % (self.__ns), PointStamped, queue_size=1)\n self.__wp_pub4 = rospy.Publisher('%s/wp_vis4' % (self.__ns), PointStamped, queue_size=1)\n\n\n\n def show_wp(self, data):\n \"\"\"\n Publishing waypoints on the path (maximum 4).\n :param data: waypoint message\n \"\"\"\n msg = PointStamped()\n msg.header = data.header\n msg.point = data.points[0]\n self.__wp_pub1.publish(msg)\n if(len(data.points) > 1):\n msg.point = data.points[1]\n self.__wp_pub2.publish(msg)\n if (len(data.points) > 2):\n msg.point = data.points[2]\n self.__wp_pub3.publish(msg)\n if (len(data.points) > 3):\n msg.point = data.points[3]\n self.__wp_pub4.publish(msg)\n\n def show_image_stack(self, data):\n \"\"\"\n Publishing input image stack. Maximal 4 images are displayed in rviz.\n :param data: input matrix\n \"\"\"\n self.__input_images.appendleft(data)\n self.__input_img_pub1.publish(self.__data_to_image(self.__input_images[0]))\n if(len(self.__input_images) > self.__stack_offset):\n self.__input_img_pub2.publish(self.__data_to_image(self.__input_images[self.__stack_offset]))\n if (len(self.__input_images) > 2*self.__stack_offset):\n self.__input_img_pub3.publish(self.__data_to_image(self.__input_images[self.__stack_offset * 2]))\n if (len(self.__input_images) > 3*self.__stack_offset):\n self.__input_img_pub4.publish(self.__data_to_image(self.__input_images[self.__stack_offset * 3]))\n\n def __data_to_image(self, data):\n \"\"\"\n Transforms input state format to Image msg, displayable in rviz.\n :param data: input state\n \"\"\"\n msg = Image()\n msg.header.frame_id = \"/base_footprint\"\n msg.height = data.shape[1]\n msg.width = data.shape[0]\n msg.encoding = \"mono8\"\n msg.data = np.uint8(np.ndarray.flatten(data, order='F'))[::-1].tolist()\n return msg\n\n def show_input_image(self, data):\n \"\"\"\n Publishing single input matrix as image.\n Can be displayed in rviz.\n :param data: input matrix\n \"\"\"\n msg = self.__data_to_image(data)\n self.__input_img_pub1.publish(msg)\n\n def show_input_scan(self, scan):\n \"\"\"\n Publishing input scan for displaying in rviz\n :param data: input matrix\n \"\"\"\n self.__input_scan_pub.publish(scan)\n\n def show_input_occ_grid(self, state_image):\n \"\"\"\n Publishing occupancy grid.\n :param state_image: occupancy grid\n \"\"\"\n self.__occ_grid_pub.publish(state_image)\n\n def show_reward(self, reward):\n \"\"\"\n Publishing reward value as marker.\n :param reward\n \"\"\"\n # Publish reward as Marker\n msg = Marker()\n msg.header.frame_id = \"/base_footprint\"\n msg.ns = \"\"\n msg.id = 0\n msg.type = msg.TEXT_VIEW_FACING\n msg.action = msg.ADD\n\n msg.pose.position.x = 0.0\n msg.pose.position.y = 1.0\n msg.pose.position.z = 0.0\n msg.pose.orientation.x = 0.0\n msg.pose.orientation.y = 0.0\n msg.pose.orientation.z = 0.0\n msg.pose.orientation.w = 1.0\n\n msg.text = \"%f\"%reward\n\n msg.scale.x = 10.0\n msg.scale.y = 10.0\n msg.scale.z = 1.0\n\n msg.color.r = 0.3\n msg.color.g = 0.4\n msg.color.b = 1.0\n msg.color.a = 1.0\n self.__rew_pub.publish(msg)\n\n # Publish reward as number\n msg = Float64()\n msg.data = reward\n self. __rew_num_pub.publish(msg)\n\n" }, { "path": "rl_agent/src/rl_agent/env_utils/reward_container.py", "content": "'''\n @name: ros_env.py\n @brief: This class provides different reward functions and parametrizes them.\n @author: Ronja Gueldenring\n @version: 3.5\n @date: 2019/04/05\n'''\n\n\nimport numpy as np\nimport math\nimport rospy\n\n\n\nclass RewardContainer():\n \"\"\"\n This class provides different reward functions and parametrizes them.\n \"\"\"\n def __init__(self, ns, robot_radius, goal_radius, max_trans_vel):\n self.old_wps = []\n self.old_closest_wp = 0\n self.__update_rate = 1/rospy.get_param(\"%s/rl_agent/update_frequency\"%ns)\n\n self.__robot_radius = robot_radius\n self.__goal_radius = goal_radius\n self.__still_time = 0.0\n self.__max_trans_vel = max_trans_vel\n\n\n def reset(self, wps):\n \"\"\"\n If episode is reset, old waypoints need to be resetted here.\n \"\"\"\n num_wp = len(wps)\n self.old_wps = np.zeros(num_wp)\n for i in range(num_wp):\n self.old_wps[i] = self.mean_sqare_dist_(wps[i].x, wps[i].y)\n self.old_closest_wp = 0\n\n def rew_func_1(self, scan, wps, transformed_goal):\n '''\n Reward function designed for static training setup\n :param scan: laser scan of environment\n :param wps: next waypoints on path\n :param transformed_goal: final goal in robot frame\n :return: reward value\n '''\n min_scan_dist = np.amin(scan.ranges)\n\n if (min_scan_dist < (self.__robot_radius + 0.5) ):\n wp_approached_rew = self.__get_wp_approached(wps, 3.5, 2.5, 1.0)\n wp_approached_rew = 0\n else:\n wp_approached_rew = self.__get_wp_approached(wps, 3.5, 2.5, 1.0)\n\n obstacle_punish = self.__get_obstacle_punish(scan.ranges , 15, self.__robot_radius)\n goal_reached_rew = self.__get_goal_reached_rew(transformed_goal, 10)\n rew = (wp_approached_rew + obstacle_punish + goal_reached_rew)\n rew = self.__check_reward(rew, obstacle_punish, goal_reached_rew, 2.5)\n return rew\n\n def rew_func_19(self, static_scan, ped_scan_msg, wps, twist, transformed_goal):\n '''\n Reward function designed for dynamic training setup\n :param static_scan: laser scan containing information about static obstacles.\n :param ped_scan_msg: laser scan containing information about dynamic obstacles.\n :param wps: next waypoints on path\n :param twist: velocity of robot\n :param transformed_goal: final goal in robot frame\n :return: reward value\n '''\n standing_still_punish = 0\n if (abs(twist.twist.linear.x) < 0.001 and abs(twist.twist.angular.z) < 0.001):\n standing_still_punish = -0.001\n self.__still_time += 0.1\n else:\n self.__still_time = 0.0\n\n if (abs(twist.twist.linear.x) < 0.001 and abs(twist.twist.angular.z) > 0.001):\n standing_still_punish = -0.01\n\n wp_approached_rew = self.__get_wp_approached(wps, 5.5, 4.5, 0.0)\n\n # Did the agent bump into an obstacle?\n obstacle_punish_static = self.__get_obstacle_punish(static_scan.ranges, 7, self.__robot_radius)\n obstacle_punish_ped = 0\n if (self.__still_time < 0.8):\n obstacle_punish_ped = self.__get_obstacle_punish(ped_scan_msg.ranges, 7, 0.85)\n obstacle_punish = min(obstacle_punish_ped, obstacle_punish_static)\n\n # Did the agent reached the goal?\n goal_reached_rew = self.__get_goal_reached_rew(transformed_goal, 10)\n\n rew = (wp_approached_rew + obstacle_punish + goal_reached_rew + standing_still_punish)\n if (rew < -2.5):\n test = \"debug\"\n rew = self.__check_reward(rew, obstacle_punish, goal_reached_rew, 2.5)\n return rew\n\n def rew_func_2_1(self, static_scan, ped_scan_msg, wps, twist, transformed_goal):\n '''\n Reward function designed for dynamic training setup\n :param static_scan: laser scan containing information about static obstacles.\n :param ped_scan_msg: laser scan containing information about dynamic obstacles.\n :param wps: next waypoints on path\n :param twist: velocity of robot\n :param transformed_goal: final goal in robot frame\n :return: reward value\n '''\n standing_still_punish = 0\n if (abs(twist.twist.linear.x) < 0.001 and abs(twist.twist.angular.z) < 0.001):\n standing_still_punish = -0.001\n self.__still_time += 0.1\n else:\n self.__still_time = 0.0\n\n if (abs(twist.twist.linear.x) < 0.001 and abs(twist.twist.angular.z) > 0.001):\n standing_still_punish = -0.01\n min_scan_dist = np.amin(static_scan.ranges)\n if (min_scan_dist < (self.__robot_radius + 0.2)):\n wp_approached_rew = self.__get_wp_approached(wps, 5.5, 4.5, 0.8)\n wp_approached_rew = 0\n else:\n wp_approached_rew = self.__get_wp_approached(wps, 5.5, 4.5, 0.8)\n\n # Did the agent bump into an obstacle?\n obstacle_punish_static = self.__get_obstacle_punish(static_scan.ranges, 15, self.__robot_radius)\n obstacle_punish_ped = 0\n if (self.__still_time < 0.8):\n obstacle_punish_ped = self.__get_obstacle_punish(ped_scan_msg.ranges, 7, 0.85)\n obstacle_punish = min(obstacle_punish_ped, obstacle_punish_static)\n\n # Did the agent reached the goal?\n goal_reached_rew = self.__get_goal_reached_rew(transformed_goal, 10)\n\n rew = (wp_approached_rew + obstacle_punish + goal_reached_rew + standing_still_punish)\n if (rew < -2.5):\n test = \"debug\"\n rew = self.__check_reward(rew, obstacle_punish, goal_reached_rew, 2.5)\n return rew\n\n\n def rew_func_2_2(self, static_scan, ped_scan_msg, wps, twist, transformed_goal):\n '''\n Reward function designed for dynamic training setup\n :param static_scan: laser scan containing information about static obstacles.\n :param ped_scan_msg: laser scan containing information about dynamic obstacles.\n :param wps: next waypoints on path\n :param twist: velocity of robot\n :param transformed_goal: final goal in robot frame\n :return: reward value\n '''\n standing_still_punish = 0\n if (abs(twist.twist.linear.x) < 0.1):\n self.__still_time += 0.1\n else:\n self.__still_time = 0.0\n min_scan_dist = np.amin(static_scan.ranges)\n\n if (min_scan_dist < (self.__robot_radius + 0.2)):\n wp_approached_rew = self.__get_wp_approached(wps, 5.5, 4.5, 0.8)\n wp_approached_rew = 0\n else:\n wp_approached_rew = self.__get_wp_approached(wps, 5.5, 4.5, 0.8)\n\n # Did the agent bump into an obstacle?\n obstacle_punish_static = self.__get_obstacle_punish(static_scan.ranges, 15, self.__robot_radius)\n obstacle_punish_ped = 0\n if (self.__still_time < 0.8):\n obstacle_punish_ped = self.__get_obstacle_punish(ped_scan_msg.ranges, 7, 0.85)\n obstacle_punish = min(obstacle_punish_ped, obstacle_punish_static)\n\n # Did the agent reached the goal?\n goal_reached_rew = self.__get_goal_reached_rew(transformed_goal, 10)\n\n rew = (wp_approached_rew + obstacle_punish + goal_reached_rew + standing_still_punish)\n if (rew < -2.5):\n test = \"debug\"\n rew = self.__check_reward(rew, obstacle_punish, goal_reached_rew, 2.5)\n return rew\n\n\n def __ped_in_box_punish(self, ped_scan_msg, box_width, box_height_pos, box_height_neg, k):\n \"\"\"\n Returns a negative reward k if pedestrians are in the defined box\n [box_width x (box_height_pos + box_height_neg)}\n :param ped_scan_msg laserscan with pedestrian information only\n :param box_width width if box\n :param box_height_pos height of box in forward direction of the robot\n :param box_height_neg height of box in backward direction of the robot\n :param k reward constant\n :return: obs, reward, done, info\n \"\"\"\n if self.is_ped_in_box(ped_scan_msg, box_width, box_height_pos, box_height_neg):\n ped_punish = -k\n else:\n ped_punish = 0\n\n return ped_punish\n\n def is_ped_in_box(self, ped_scan_msg, box_width, box_height_pos, box_height_neg):\n '''\n Checks if pedestrian is in defined box [box_width x (box_height_pos + box_height_neg)]\n :param ped_scan_msg laserscan with pedestrian information only\n :param box_width width if box\n :param box_height_pos height of box in forward direction of the robot\n :param box_height_neg height of box in backward direction of the robot\n :return: True if pedestrian inside the defined box, else False.\n '''\n is_ped_in_box = False\n box_width = box_width * 2\n box_height_pos = box_height_pos\n box_height_neg = box_height_neg\n angle_min = ped_scan_msg.angle_min\n angle_increment = ped_scan_msg.angle_increment\n ped_punish = 0.0\n for i in range(len(ped_scan_msg.ranges)):\n angle = angle_min + i * angle_increment\n length = ped_scan_msg.ranges[i]\n x = math.cos(angle) * length\n y = math.sin(angle) * length\n if(x > -box_height_neg and x < box_height_pos and y > -box_width/2 and y < box_width/2):\n is_ped_in_box = True\n return is_ped_in_box\n\n def __check_reward(self, rew, obstacle_punish, goal_reached_rew, thresh):\n \"\"\"\n Checks if reward makes sense.\n :param rew final reward\n :param obstacle_punish reward for obstacle collision\n :param goal_reached_rew reward for reaching goal\n :param thresh [-thresh, +thresh[ the final reward should be inbetween, if no obstacle_punish==0 and goal_reached_rew==0\n :return: returns reward value if it makes sense, else 0\n \"\"\"\n if ((rew > thresh and goal_reached_rew == 0.0) or (rew < -thresh and obstacle_punish == 0.0)):\n rospy.loginfo(\"Wrong reward: %f\" % rew)\n return 0\n else:\n return rew\n\n def __get_turn_punish(self, w, fac, thresh):\n \"\"\"\n Returns negative reward if the robot turns.\n :param w roatational speed of the robot\n :param fac weight of reward punish for turning\n :param thresh rotational speed > thresh will be punished\n :return: returns reward for turning\n \"\"\"\n if abs(w) > thresh:\n return abs(w) * -fac\n else:\n return 0.0\n\n def __get_obstacle_punish(self, scan, k, radius):\n \"\"\"\n Returns negative reward if the robot collides with obstacles.\n :param scan containing obstacles that should be considered\n :param k reward constant\n :return: returns reward colliding with obstacles\n \"\"\"\n min_scan_dist = np.amin(scan)\n\n if min_scan_dist < radius:\n return -k\n else:\n return 0.0\n\n def __get_obstacle_punish_section(self, scan, k, perc):\n \"\"\"\n Returns negative reward if the robot collides with obstacles in section around the robot.\n :param scan containing obstacles that should be considered\n :param k reward constant\n :param perc percentage of the area around the robot, that should be considered.\n Example: 0.4 will consider the front view of 144 degree.\n :return: returns reward colliding with obstacles in defined section\n \"\"\"\n min_scan_dist_index = np.argmin(scan)\n scan_length = len(scan)\n lower_bound = int(scan_length/2 - perc*scan_length/2)\n upper_bound = int(scan_length/2 + perc*scan_length/2)\n if min_scan_dist_index > lower_bound and min_scan_dist_index < upper_bound and scan[min_scan_dist_index] < self.__robot_radius:\n return -k\n else:\n return 0.0\n\n def __get_goal_reached_rew(self, transformed_goal, k):\n \"\"\"\n Returns positive reward if the robot reaches the goal.\n :param transformed_goal goal position in robot frame\n :param k reward constant\n :return: returns reward colliding with obstacles\n \"\"\"\n dist_to_goal = self.mean_sqare_dist_(transformed_goal.position.x, transformed_goal.position.y)\n if dist_to_goal < self.__goal_radius:\n return k\n else:\n return 0.0\n\n\n def __get_wp_approached(self, wps, punish_fac, rew_fac, k):\n \"\"\"\n Returns positive reward if the robot approaches the next waypoint, else negative reward.\n :param wps next waypoints\n :param punish_fac weight for punishing disapproaching the closest waypoint\n :param rew_fac weight for rewarding approaching the closest waypoint\n :param k reward constant for reaching a waypoint\n :return: returns reward for approaching waypoints.\n \"\"\"\n num_wp = len(wps.points)\n dist_to_waypoint = np.zeros(num_wp)\n\n for i in range(num_wp):\n dist_to_waypoint[i] = self.mean_sqare_dist_(wps.points[i].x, wps.points[i].y)\n\n if wps.is_new.data:\n wp_approached = k\n else:\n diff = (self.old_wps[0] - dist_to_waypoint[0])\n if (abs(diff) > self.__max_trans_vel*self.__update_rate*4):\n diff = 0\n if (diff < 0):\n wp_approached = punish_fac * diff\n else:\n wp_approached = rew_fac * diff\n\n self.old_wps = dist_to_waypoint\n\n return wp_approached\n\n def mean_sqare_dist_(self, x, y):\n \"\"\"\n Computing mean square distance of x and y\n :param x, y\n :return: sqrt(x^2 + y^2)\n \"\"\"\n return math.sqrt(math.pow(x, 2) + math.pow(y, 2))\n\n" }, { "path": "rl_agent/src/rl_agent/env_utils/state_collector.py", "content": "'''\n @name: state_collector.py\n @brief: This class collects most recent relevant state data of the environment of the RL-agent.\n @author: Ronja Gueldenring\n @version: 3.5\n @date: 2019/04/05\n'''\n# python relevant\nimport numpy as np\nimport copy\n\n# ros-relevant\nimport rospy\nfrom sensor_msgs.msg import LaserScan\nfrom nav_msgs.msg import OccupancyGrid\nfrom rl_msgs.msg import Waypoint\nfrom rl_msgs.srv import StateImageGenerationSrv\nfrom geometry_msgs.msg import TwistStamped, PoseStamped, Pose\nfrom std_srvs.srv import SetBool\nfrom rl_msgs.srv import MergeScans\nimport time\n\nclass StateCollector():\n \"\"\"\n This class collects most recent relevant state data of the environment of the RL-agent.\n \"\"\"\n def __init__(self, ns, train_mode):\n # Class variables\n self.__mode = train_mode # Mode of RL-agent (Training or Executrion ?)\n self.__ns = ns # namespace of simulation environment\n self.__is__new = False # True, if waypoint reached\n self.__static_scan = LaserScan() # Laserscan only contains static objects\n self.__ped_scan = LaserScan() # Laserscan only contains pedestrians\n self.__f_scan = LaserScan()\n self.__f_scan.header.frame_id = \"base_footprint\"\n self.__b_scan = LaserScan()\n self.__b_scan.header.frame_id = \"base_footprint\"\n\n self.__img = OccupancyGrid() # input image\n self.__wps= Waypoint() # most recent Waypoints\n self.__twist = TwistStamped() # most recent velocity\n self.__goal = Pose() # most recent goal position in robot frame\n self.__state_mode = 2 # 0, if image as input state representation\n # 1, if stacked image representation in same frame\n # 2, if scan as input state representation\n\n\n\n # Subscriber\n self.wp_sub_ = rospy.Subscriber(\"%s/wp\" % (self.__ns), Waypoint, self.__wp_callback, queue_size=1)\n\n if [\"train\", \"eval\"].__contains__(self.__mode):\n # Info only avaible during evaluation and training\n self.wp_sub_reached_ = rospy.Subscriber(\"%s/wp_reached\" % (self.__ns), Waypoint, self.__wp_reached_callback, queue_size=1)\n\n self.static_scan_sub_ = rospy.Subscriber(\"%s/static_laser\" % (self.__ns), LaserScan, self.__static_scan_callback,\n queue_size=1)\n\n self.ped_scan_sub_ = rospy.Subscriber(\"%s/ped_laser\" % (self.__ns), LaserScan, self.__ped_scan_callback,\n queue_size=1)\n self.twist_sub_ = rospy.Subscriber(\"%s/twist\" % (self.__ns), TwistStamped, self.__twist_callback, queue_size=1)\n self.goal_sub_ = rospy.Subscriber(\"%s/rl_agent/robot_to_goal\" % (self.__ns), PoseStamped, self.__goal_callback, queue_size=1)\n else:\n self.static_scan_sub_ = rospy.Subscriber(\"%s/b_scan\" % (self.__ns), LaserScan,\n self.__b_scan_callback,\n queue_size=1)\n self.static_scan_sub_ = rospy.Subscriber(\"%s/f_scan\" % (self.__ns), LaserScan,\n self.__f_scan_callback,\n queue_size=1)\n\n # Service\n self.__img_srv = rospy.ServiceProxy('%s/image_generator/get_image' % (self.__ns), StateImageGenerationSrv)\n self.__sim_in_step = rospy.ServiceProxy('%s/is_in_step' % (self.__ns), SetBool)\n self.__merge_scans = rospy.ServiceProxy('%s/merge_scans' % (self.__ns), MergeScans)\n\n def get_state(self):\n \"\"\"\n Provides the most recent state with laserscan data, input image, waypoints, robots velocity, goal position\n :returns laser scan of static objects, ([] in execution mode)\n laserscan of pedestrians, ([] in execution mode)\n state image, ([] if raw data state mode)\n next waypoints,\n twist of the robot,\n goal position in robot frame\n \"\"\"\n if [\"train\", \"eval\"].__contains__(self.__mode):\n\n # Fully synchronized --> slows down simulation speed!\n # resp = self.__sim_in_step(True)\n # while(resp.success):\n # time.sleep(0.0001)\n # resp = self.__sim_in_step(True)\n # while self.__ped_scan.header.stamp.to_sec() + 0.11 < float(resp.message):\n # time.sleep(0.005)\n # while self.__static_scan.header.stamp.to_sec() + 0.11 < float(resp.message):\n # time.sleep(0.005)\n\n # start = time.time()\n static_scan_msg = self.__remove_nans_from_scan(self.__static_scan)\n ped_scan_msg = self.__remove_nans_from_scan(self.__ped_scan)\n # print(\"__remove_nans_from_scan: %f\" % (time.time() - start))\n\n # start = time.time()\n merged_scan = LaserScan()\n merged_scan.header.frame_id = \"base_footprint\"\n merged_scan.header.stamp = static_scan_msg.header.stamp\n merged_scan.ranges = np.minimum(static_scan_msg.ranges, ped_scan_msg.ranges)\n merged_scan.range_max = static_scan_msg.range_max\n merged_scan.range_min = static_scan_msg.range_min\n merged_scan.angle_increment = static_scan_msg.angle_increment\n merged_scan.angle_min = static_scan_msg.angle_min\n merged_scan.angle_max = static_scan_msg.angle_max\n # print(\"merge_scan: %f\" % (time.time() - start))\n # start = time.time()\n wp_cp = copy.deepcopy(self.__wps)\n self.__wps.is_new.data = False\n # print(\"deep_copy: %f\" % (time.time() - start))\n\n start = time.time()\n if (self.__state_mode == 0):\n # ToDo: Service call takes very long. Find more efficient solution!\n resp = self.__img_srv(merged_scan, wp_cp)\n self.__img = resp.img\n # print(\"img service call: %f\" % (time.time() - start))\n else:\n self.__img = []\n return static_scan_msg, ped_scan_msg, merged_scan, self.__img, wp_cp, self.__twist, self.__goal\n else:\n scans = []\n scans.append(self.__f_scan)\n scans.append(self.__b_scan)\n resp = self.__merge_scans(scans)\n merged_scan = resp.merged_scan\n wp_cp = copy.deepcopy(self.__wps)\n self.__wps.is_new.data = False\n if (self.__state_mode == 0):\n resp = self.__img_srv(merged_scan, wp_cp)\n self.__img = resp.img\n else:\n self.__img = []\n return [], [], merged_scan, self.__img, wp_cp, self.__twist, self.__goal\n\n def get_static_scan(self):\n \"\"\"\n Provides the most recent laser scan of static objects\n :return laser scan\n \"\"\"\n scan_msg = self.__remove_nans_from_scan(self.__static_scan)\n return scan_msg\n\n def set_state_mode(self, state_mode):\n self.__state_mode = state_mode\n\n def __remove_nans_from_scan(self, scan_msg):\n \"\"\"\n Replaces nan-values with maximum distance of scanner.\n :param scan_msg scan message, where nan-values need to be removed.\n \"\"\"\n scan = np.array(scan_msg.ranges)\n scan[np.isnan(scan)] = scan_msg.range_max\n scan_msg.ranges = scan\n return scan_msg\n\n def __static_scan_callback(self, data):\n self.__static_scan = data\n\n def __ped_scan_callback(self, data):\n self.__ped_scan = data\n\n def __f_scan_callback(self, data):\n self.__f_scan = data\n\n def __b_scan_callback(self, data):\n self.__b_scan = data\n\n def __wp_callback(self, data):\n if not self.__wps.is_new.data:\n self.__wps = data\n\n def __twist_callback(self, data):\n self.__twist = data\n\n def __goal_callback(self, data):\n self.__goal = data.pose\n\n def __is_wp_reached_callback(self, data):\n self.__is__new = data.data\n\n def __wp_reached_callback(self, data):\n self.__wps = data" }, { "path": "rl_agent/src/rl_agent/env_utils/task_generator.py", "content": "'''\n @name: task_generator.py\n @brief: This class generates random tasks for training.\n @author: Ronja Güldenring\n @version: 3.5\n @date: 2019/04/05\n'''\n\n# python-relevant\nimport time\nimport math\nimport random\nfrom pyquaternion import Quaternion\nimport numpy as np\nfrom collections import deque\nimport pickle\n\n#ros-relevant\nimport rospy\nimport rospkg\n\nfrom nav_msgs.msg import OccupancyGrid, Path\nfrom flatland_msgs.srv import MoveModel, DeleteModel, SpawnModel, Step, RespawnModels\nfrom flatland_msgs.msg import Model\nfrom actionlib_msgs.msg import GoalStatusArray\nfrom geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped, Pose2D\nfrom std_msgs.msg import Float64, Bool\nfrom geometry_msgs.msg import Twist, Point\nfrom pedsim_srvs.srv import SpawnPeds\nfrom pedsim_msgs.msg import Ped\nfrom std_srvs.srv import SetBool, Empty\n\nclass TaskGenerator():\n \"\"\"\n This class generates task for training. The following task generation is implemented:\n - generate random paths with static objects on it.\n - generate random paths with pedestrians on it.\n - generate random paths with pedestrians and static objects on it.\n - load predefined path (--> evaluation).\n \"\"\"\n def __init__(self, ns, state_collector, robot_radius):\n\n\n # Class variables\n self.NS = ns # namespace\n self.ROBOT_RADIUS = robot_radius # radius of the robot\n self.__update_rate = \\\n 1/rospy.get_param(\"%s/rl_agent/update_frequency\"%ns)\n self.__state_collector = state_collector # Collects state information\n self.__move_base_status_id = \"\" # recent id of move_base status\n self.__map = OccupancyGrid() # global map\n self.__path = Path() # global plan\n self.__static_objects = [] # static objects that has been spawned in the most recent task\n\n self.__ped_type = rospy.get_param(\"%s/rl_agent/ped_type\"%ns)\n # 0: Pedestrians don't avoid robot\n # 10: Pedestrians always avoid robot\n # 11: Pedestrians avoid robot if it stands still and after reaction time.\n\n self.__peds = [] # pedestrians that has been spawned\n\n self.__old_path_stamp = 0.0 # timestamp of last global plan\n self.init = True\n self.__flatland_path = rospkg.RosPack().get_path('flatland_setup')\n self.__static_object_types = {\"name\": [\"cylinder.model.yaml\", # different object types\n \"palett.model.yaml\",\n \"wagon.model.yaml\"], \"index\": [0, 0, 0]}\n\n\n self.__ped_file = \"person_two_legged.model.yaml\"\n # self.__ped_file = \"person_single_circle.model.yaml\"\n\n # Services\n self.__sim_step = rospy.ServiceProxy('%s/step' % self.NS, Step)\n self.__sim_pause = rospy.ServiceProxy('%s/pause' % self.NS, Empty)\n self.__sim_resume = rospy.ServiceProxy('%s/resume' % self.NS, Empty)\n self.__move_robot_to = rospy.ServiceProxy('%s/move_model' % self.NS, MoveModel)\n self.__delete_model = rospy.ServiceProxy('%s/delete_model' % self.NS, DeleteModel)\n self.__spawn_model = rospy.ServiceProxy('%s/spawn_model' % self.NS, SpawnModel)\n self.__respawn_models = rospy.ServiceProxy('%s/respawn_models' % self.NS, RespawnModels)\n self.__spawn_ped_srv = rospy.ServiceProxy('%s/pedsim_simulator/spawn_ped' % self.NS, SpawnPeds)\n self.__respawn_peds_srv = rospy.ServiceProxy('%s/pedsim_simulator/respawn_peds' % self.NS, SpawnPeds)\n self.__remove_all_peds_srv = rospy.ServiceProxy('%s/pedsim_simulator/remove_all_peds' % self.NS, SetBool)\n\n # Subscriber\n self.__goal_status_sub = rospy.Subscriber(\"%s/move_base/status\" % self.NS, GoalStatusArray,\n self.__goal_status_callback, queue_size=1)\n self.__map_sub = rospy.Subscriber(\"%s/map\" % self.NS, OccupancyGrid, self.__map_callback)\n self.__path_sub = rospy.Subscriber(\"%s/move_base/NavfnROS/plan\" % self.NS, Path, self.__path_callback)\n # self.__path_sub = rospy.Subscriber(\"%s/move_base/GlobalPlanner/plan\" % self.NS, Path, self.__path_callback)\n\n # Publisher\n self.__initialpose_pub = rospy.Publisher('%s/initialpose' % self.NS, PoseWithCovarianceStamped, queue_size=1)\n self.__goal_pub_ = rospy.Publisher('%s/move_base_simple/goal' % self.NS, PoseStamped, queue_size=1)\n self.__cmd_vel_pub = rospy.Publisher('%s/cmd_vel' % self.NS, Twist, queue_size=1)\n self.__done_pub = rospy.Publisher('%s/rl_agent/done' % self.NS, Bool, queue_size=1)\n self.__new_task_pub = rospy.Publisher('%s/rl_agent/new_task_started' % self.NS, Bool, queue_size=1)\n self.__resume = rospy.ServiceProxy('%s/resume' % (self.NS), Empty)\n\n #Clear world\n self.__init_object_remove()\n\n self.__time_to_set_ped_task = 0.0\n\n\n def set_task(self, task):\n \"\"\"\n Loading predefined task (e.g. during evaluation)\n :param task predefined task, that will be loaded\n :return True, if task is loaded successfully\n \"\"\"\n is_path_available = False\n count = 0\n while not is_path_available and count < 10:\n self.__spread_done()\n self.__stop_robot()\n if not task:\n return False\n\n # Setting path\n self.set_robot_pos(task[\"start\"][0], task[\"start\"][1], task[\"start\"][2])\n self.take_sim_step()\n self.__wait_for_laser()\n time.sleep(0.11)\n self.__publish_goal(task[\"goal\"][0], task[\"goal\"][1], task[\"goal\"][2])\n is_path_available = self.__is_new_path_available(task[\"goal\"], task[\"start\"])\n print(\"path not available...trying again...\")\n count+=1\n\n # Spawning obstacles\n self.remove_all_static_objects()\n self.__remove_all_peds()\n\n if 'static_objects' in task.keys():\n for i in range(len(task[\"static_objects\"][\"x\"])):\n 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])\n\n if 'peds' in task.keys():\n self.__respawn_peds(task[\"peds\"])\n self.__spread_new_task()\n return True\n\n def set_path(self):\n \"\"\"\n Generating a random path in environment.\n :return d task information containing start, goal and path\n \"\"\"\n self.__spread_done()\n d = {}\n d[\"start\"] = self.__set_random_robot_pos()\n d[\"goal\"] = self.__publish_random_goal_()\n self.__is_new_path_available(d[\"goal\"], d[\"start\"])\n d[\"path\"] = self.__path\n return d\n\n def set_random_static_task(self):\n \"\"\"\n Generating a random path with static obstacles on it.\n :return d task information containing start, goal, path and static objects\n \"\"\"\n # start = time.time()\n self.__spread_done()\n d = {}\n # Setting path\n self.__stop_robot()\n d[\"start\"] = self.__set_random_robot_pos()\n d[\"goal\"] = self.__publish_random_goal_()\n\n # Spawning new obstacles\n if self.__is_new_path_available(d[\"goal\"], d[\"start\"]):\n self.__spawn_random_static_objects()\n d[\"static_objects\"] = self.__static_objects\n d[\"path\"] = self.__path\n self.__spread_new_task()\n # print(\"Task generation took %f secs.\"%(time.time() - start))\n return d\n\n def set_random_ped_task(self):\n \"\"\"\n Generating a random path with pedestrians on it.\n :return d task information containing start, goal, path and pedestrians\n \"\"\"\n begin = time.time()\n self.__spread_done()\n d = {}\n self.__stop_robot()\n d[\"start\"] = self.__set_random_robot_pos()\n d[\"goal\"] = self.__publish_random_goal_()\n\n # if not self.__is_new_path_available(d[\"goal\"], d[\"start\"]):\n # d[\"goal\"] = self.__publish_random_goal_()\n #\n while not self.__is_new_path_available(d[\"goal\"], d[\"start\"]):\n self.__spread_done()\n time.sleep(0.1)\n self.__spread_done()\n time.sleep(0.1)\n\n self.__stop_robot()\n d = {}\n d[\"start\"] = self.__set_random_robot_pos()\n\n # Finding valid position on map in small radius\n valid = False\n count = 0\n while not valid:\n x = d[\"start\"][0] + random.uniform(3, math.floor(count / 10) + 5) * random.choice([-1, 1])\n y = d[\"start\"][1] + random.uniform(3, math.floor(count / 10) + 5) * random.choice([-1, 1])\n valid = self.__is_pos_valid(x, y, self.__map)\n count += 1\n self.__publish_goal(x, y, 0)\n d[\"goal\"] = [x, y, 0]\n\n\n self.__spawn_random_peds_on_path()\n\n d[\"peds\"] = self.__peds\n d[\"path\"] = self.__path\n self.__spread_new_task()\n self.__time_to_set_ped_task += time.time()-begin\n # if(self.__time_to_set_ped_task%50 < 0.5):\n # print(\"Time spend on setting random path in %s: %f\"%(self.NS, self.__time_to_set_ped_task))\n return d\n\n def set_random_short_ped_task(self):\n \"\"\"\n Generating a random short path (> 3m and < 5m) with pedestrians on it.\n We expect to increase the probability of a robot meeting a pedestrian.\n :return d task information containing start, goal, path and pedestrians\n \"\"\"\n self.__spread_done()\n self.__stop_robot()\n d = {}\n d[\"start\"] = self.__set_random_robot_pos()\n\n #Finding valid position on map in small radius\n valid = False\n count = 0\n while not valid:\n x = d[\"start\"][0] + random.uniform(3, math.floor(count/10) + 5)*random.choice([-1, 1])\n y = d[\"start\"][1] + random.uniform(3, math.floor(count/10) + 5)*random.choice([-1, 1])\n valid = self.__is_pos_valid(x, y, self.__map)\n count+=1\n self.__publish_goal(x, y, 0)\n d[\"goal\"] = [x, y, 0]\n\n while not self.__is_new_path_available(d[\"goal\"], d[\"start\"]):\n self.__spread_done()\n self.__stop_robot()\n d = {}\n d[\"start\"] = self.__set_random_robot_pos()\n\n # Finding valid position on map in small radius\n valid = False\n count = 0\n while not valid:\n x = d[\"start\"][0] + random.uniform(3, math.floor(count / 10) + 5) * random.choice([-1, 1])\n y = d[\"start\"][1] + random.uniform(3, math.floor(count / 10) + 5) * random.choice([-1, 1])\n valid = self.__is_pos_valid(x, y, self.__map)\n count += 1\n self.__publish_goal(x, y, 0)\n d[\"goal\"] = [x, y, 0]\n\n\n\n self.__spawn_random_peds_on_path()\n\n d[\"peds\"] = self.__peds\n d[\"path\"] = self.__path\n self.__spread_new_task()\n return d\n\n def set_random_static_ped_task(self):\n \"\"\"\n Generating a random path with pedestrians and static obstacles on it.\n :return d task information containing start, goal, path, pedestrians and static objects\n \"\"\"\n # Setting path\n self.__spread_done()\n d = {}\n self.__stop_robot()\n d[\"start\"] = self.__set_random_robot_pos()\n d[\"goal\"] = self.__publish_random_goal_()\n\n # Spawning new obstacles\n if self.__is_new_path_available(d[\"goal\"], d[\"start\"]):\n self.__spawn_random_static_objects()\n self.__spawn_random_peds_on_path()\n d[\"peds\"] = self.__peds\n d[\"path\"] = self.__path\n d[\"static_objects\"] = self.__static_objects\n self.__spread_new_task()\n return d\n\n def take_sim_step(self):\n \"\"\"\n Executing one simulation step of 0.1 sec\n \"\"\"\n msg = Float64()\n msg.data = self.__update_rate\n rospy.wait_for_service('%s/step' % self.NS)\n self.__sim_step(msg)\n return\n\n def __set_random_robot_pos(self):\n \"\"\"\n Moving robot to random position (x, y, theta) in simulation\n :return: robot position [x, y, theta]\n \"\"\"\n success = False\n while not success:\n x, y, theta = self.__get_random_pos_on_map(self.__map)\n self.set_robot_pos(x, y, theta)\n scan = self.__wait_for_laser()\n min_obstacle_dist = np.amin(scan)\n if min_obstacle_dist > (self.ROBOT_RADIUS + 0.1):\n success = True\n return x, y, theta\n\n def set_robot_pos(self, x, y, theta):\n \"\"\"\n Move robot to position (x, y, theta) in simulation\n :param x x-position of the robot\n :param y y-position of the robot\n :param theta theta-position of the robot\n \"\"\"\n pose = Pose2D()\n pose.x = x\n pose.y = y\n pose.theta = theta\n rospy.wait_for_service('%s/move_model' % self.NS)\n self.__move_robot_to('robot_1', pose)\n self.take_sim_step()\n self.__pub_initial_position(x, y, theta)\n\n def __wait_for_laser(self):\n \"\"\"\n Waiting for most recent laserscan to get available\n :return most recent laser scan data\n \"\"\"\n ts = self.__state_collector.get_static_scan().header.stamp\n self.take_sim_step()\n scan = self.__state_collector.get_static_scan()\n begin = time.time()\n while len(scan.ranges) == 0 or scan.header.stamp <= ts:\n rospy.logdebug(\"Waiting for laser scan to get available.\")\n if(time.time() - begin > 1):\n self.take_sim_step()\n time.sleep(0.00001)\n scan = self.__state_collector.get_static_scan()\n return scan.ranges\n\n def __pub_initial_position(self, x, y, theta):\n \"\"\"\n Publishing new initial position (x, y, theta) --> for localization\n :param x x-position of the robot\n :param y y-position of the robot\n :param theta theta-position of the robot\n \"\"\"\n initpose = PoseWithCovarianceStamped()\n initpose.header.stamp = rospy.get_rostime()\n initpose.header.frame_id = \"map\"\n initpose.pose.pose.position.x = x\n initpose.pose.pose.position.y = y\n quaternion = self.__yaw_to_quat(theta)\n\n initpose.pose.pose.orientation.w = quaternion[0]\n initpose.pose.pose.orientation.x = quaternion[1]\n initpose.pose.pose.orientation.y = quaternion[2]\n initpose.pose.pose.orientation.z = quaternion[3]\n self.__initialpose_pub.publish(initpose)\n return\n\n def __publish_random_goal_(self):\n \"\"\"\n Publishing new random goal [x, y, theta] for global planner\n :return: goal position [x, y, theta]\n \"\"\"\n x, y, theta = self.__get_random_pos_on_map(self.__map)\n self.__publish_goal(x, y, theta)\n return x, y, theta\n\n def __publish_goal(self, x, y, theta):\n \"\"\"\n Publishing goal (x, y, theta)\n :param x x-position of the goal\n :param y y-position of the goal\n :param theta theta-position of the goal\n \"\"\"\n self.__old_path_stamp = self.__path.header.stamp\n goal = PoseStamped()\n goal.header.stamp = rospy.get_rostime()\n goal.header.frame_id = \"map\"\n goal.pose.position.x = x\n goal.pose.position.y = y\n quaternion = self.__yaw_to_quat(theta)\n goal.pose.orientation.w = quaternion[0]\n goal.pose.orientation.x = quaternion[1]\n goal.pose.orientation.y = quaternion[2]\n goal.pose.orientation.z = quaternion[3]\n self.__goal_pub_.publish(goal)\n return\n\n def __get_random_pos_on_map(self, map):\n \"\"\"\n Find a valid (free) random position (x, y, theta) on the map\n :param map\n :return: x, y, theta\n \"\"\"\n map_width = map.info.width * map.info.resolution + map.info.origin.position.x\n map_height = map.info.height * map.info.resolution + map.info.origin.position.y\n x = random.uniform(0.0 , map_width)\n y = random.uniform(0.0, map_height)\n while not self.__is_pos_valid(x, y, map):\n x = random.uniform(0.0, map_width)\n y = random.uniform(0.0, map_height)\n\n theta = random.uniform(-math.pi, math.pi)\n return x, y, theta\n\n def __is_pos_valid(self, x, y, map):\n \"\"\"\n Checks if position (x,y) is a valid position on the map.\n :param x x-position\n :param y y-position\n :param map\n :return: True if position is valid\n \"\"\"\n cell_radius = int((self.ROBOT_RADIUS + 0.1)/map.info.resolution)\n y_index = int((y-map.info.origin.position.y)/map.info.resolution)\n x_index = int((x-map.info.origin.position.x)/map.info.resolution)\n\n for i in range(x_index-cell_radius, x_index+cell_radius, 1):\n for j in range(y_index-cell_radius, y_index+cell_radius, 1):\n index = j * map.info.width + i\n if index >= len(map.data):\n return False\n try:\n val = map.data[index]\n except IndexError:\n print(\"IndexError: index: %d, map_length: %d\"%(index, len(map.data)))\n return False\n if val != 0:\n return False\n return True\n\n def __spread_done(self):\n \"\"\"\n Pulishing True on done-pub\n \"\"\"\n self.__done_pub.publish(Bool(True))\n\n def __spread_new_task(self):\n \"\"\"\n Pulishing True if new task is set. Ready to be solved\n \"\"\"\n self.__new_task_pub.publish(Bool(True))\n\n def __stop_robot(self):\n \"\"\"\n Stops robot.\n \"\"\"\n # self.__agent_action_pub.publish(Twist())\n self.__cmd_vel_pub.publish(Twist())\n return\n\n def __spawn_random_static_objects(self):\n \"\"\"\n Spawns a random number of static objects randomly on the path.\n \"\"\"\n max_num_obstacles = int(len(self.__path.poses) / 150)\n self.__static_object_types[\"index\"] = [0, 0, 0]\n models = []\n\n if max_num_obstacles == 0:\n num_static_obstacles = 0\n else:\n num_static_obstacles = random.randint(1, max_num_obstacles)\n for i in range(num_static_obstacles):\n model_type = random.randint(0, len(self.__static_object_types[\"name\"])-1)\n model_name = self.__static_object_types[\"name\"][model_type]\n [x, y] = self.__generate_rand_pos_on_path(self.__path.poses, 100, 1.0)\n theta = random.uniform(-math.pi, math.pi)\n model = Model()\n model.yaml_path = \"%s/objects/%s\" % (self.__flatland_path, model_name)\n model.name = \"%s_%d\"%(model_name.split('.')[0], self.__static_object_types[\"index\"][model_type])\n model.ns = \"stat_obj_%d\" % i\n model.pose = Pose2D()\n model.pose.x = x\n model.pose.y = y\n model.pose.theta = theta\n models.append(model)\n self.__static_object_types[\"index\"][model_type] +=1\n # self.spawn_object(model_name, i, x, y, theta)\n self.respawn_static_objects(models)\n return\n\n def respawn_static_objects(self, models):\n \"\"\"\n Respawning a new scene of static objects. Objects from previous tasks are reused\n and simply removed to the appropriate object position.\n More efficient, because several models are spawned with one service call.\n :param models list of models\n \"\"\"\n old_model_names = []\n for old_model in self.__static_objects:\n old_model_names.append(old_model.name)\n rospy.wait_for_service('%s/respawn_models' % self.NS)\n try:\n self.__respawn_models.call(old_model_names, models)\n except (TypeError):\n print('Spawn object: TypeError.')\n return\n except (rospy.ServiceException):\n print('Spawn object: rospy.ServiceException. Closing serivce')\n try:\n self.__spawn_model.close()\n except AttributeError:\n print('Spawn object close(): AttributeError.')\n return\n return\n except AttributeError:\n print('Spawn object: AttributeError.')\n return\n self.__static_objects = models\n\n\n def spawn_object(self, model_name, index, x, y, theta):\n \"\"\"\n Spawns one static object.\n :param model_name object type\n :param x x-position of the object\n :param y y-position of the object\n :param theta orientation of the object\n \"\"\"\n srv = SpawnModel()\n srv.yaml_path = \"%s/objects/%s\" % (self.__flatland_path, model_name)\n srv.name = \"stat_obj_%d\" % index\n srv.ns = \"stat_obj_%d\" % index\n temp = Pose2D()\n temp.x = x\n temp.y = y\n temp.theta = theta\n srv.pose = temp\n rospy.wait_for_service('%s/spawn_model' % self.NS)\n try:\n self.__spawn_model.call(srv.yaml_path, srv.name, srv.ns, srv.pose)\n except (TypeError):\n print('Spawn object: TypeError.')\n return\n except (rospy.ServiceException):\n print('Spawn object: rospy.ServiceException. Closing serivce')\n try:\n self.__spawn_model.close()\n except AttributeError:\n print('Spawn object close(): AttributeError.')\n return\n return\n except AttributeError:\n print('Spawn object: AttributeError.')\n return\n\n stat_obj = Model()\n stat_obj.yaml_path = srv.yaml_path\n stat_obj.name = srv.name\n stat_obj.ns = srv.ns\n stat_obj.pose = srv.pose\n self.__static_objects.append(stat_obj)\n return\n\n\n def remove_all_static_objects(self):\n \"\"\"\n Removes all static objects, that has been spawned so far\n \"\"\"\n for i in self.__static_objects:\n srv = DeleteModel()\n srv.name = i.name\n rospy.wait_for_service('%s/delete_model' % self.NS)\n ret = self.__delete_model.call(srv.name)\n\n self.__static_objects = []\n\n def __init_object_remove(self):\n \"\"\"\n Removes all objects that might be left overs from previous training sessions.\n Is supposed to be called in the constructor.\n \"\"\"\n # This is necessary in case old objects are still present in flatland\n if len(self.__static_objects) == 0:\n for type in self.__static_object_types[\"name\"]:\n for i in range(5):\n srv = DeleteModel()\n srv.name = \"%s_%d\"%(type.split('.')[0], i)\n rospy.wait_for_service('%s/delete_model' % self.NS)\n ret = self.__delete_model.call(srv.name)\n if not ret.success:\n break\n ret.success = True\n person_num = 1\n while ret.success:\n srv = DeleteModel()\n srv.name = \"person_%d\" % (person_num)\n rospy.wait_for_service('%s/delete_model' % self.NS)\n ret = self.__delete_model.call(srv.name)\n person_num += 1\n self.__remove_all_peds()\n self.__static_objects = []\n\n\n def __remove_all_peds(self):\n \"\"\"\n Removes all pedestrians, that has been spawned so far\n \"\"\"\n srv = SetBool()\n srv.data = True\n rospy.wait_for_service('%s/pedsim_simulator/remove_all_peds' % self.NS)\n self.__remove_all_peds_srv.call(srv.data)\n self.__peds = []\n return\n\n def spawn_random_peds_in_world(self, n):\n \"\"\"\n Spawning n random pedestrians in the whole world.\n :param n number of pedestrians that will be spawned.\n \"\"\"\n ped_array = []\n for i in range(1, n):\n waypoints = np.array([], dtype=np.int64).reshape(0, 3)\n [x, y, theta] = self.__get_random_pos_on_map(self.__map)\n waypoints = np.vstack([waypoints, [x, y, 0.3]])\n if random.uniform(0.0, 1.0) < 0.8:\n for j in range(4):\n dist = 0\n while dist < 4:\n [x2, y2, theta2] = self.__get_random_pos_on_map(self.__map)\n dist = self.__mean_sqare_dist_((waypoints[-1,0] - x2), (waypoints[-1,1] - y2))\n waypoints = np.vstack([waypoints, [x2, y2, 0.3]])\n ped_array.append(i, [x, y, 0.0], waypoints)\n self.__respawn_peds(ped_array)\n\n def __spawn_random_peds_on_path(self):\n \"\"\"\n Spawns a random number of pedestrians randomly near the path.\n Pedestrians either cross the path, walk along the path or stand around.\n \"\"\"\n\n if(len(self.__path.poses) == 0):\n return\n max_num_peds = max(1, int(len(self.__path.poses)/300))\n\n if(max_num_peds == 0):\n num_peds = 0\n else:\n num_peds = random.randint(1,max_num_peds)\n id = 1\n ped_array = []\n # Pedestrians along the path\n for i in range(math.ceil(num_peds*0.8)):\n ped_array.append(self.__get_random_ped_along_path(id))\n id += 1\n\n # Pedestrians crossing path\n for i in range(math.ceil(num_peds)):\n ped_array.append(self.__get_random_crossing_ped(id))\n id += 1\n self.__respawn_peds(ped_array)\n return ped_array\n\n def __get_random_ped_along_path(self, id):\n \"\"\"\n Spawning a pedestrian walking along the path. The pedestrian's waypoints are generated randomply\n \"\"\"\n start_pos = self.__generate_rand_pos_on_path(self.__path.poses, 100, 2)\n start_pos.append(0.0)\n\n waypoints = np.array([], dtype=np.int64).reshape(0, 3)\n # if random.uniform(0.0, 1.0) < 0.85:\n waypoints = np.vstack([waypoints, [start_pos[0], start_pos[1], 0.3]])\n\n wp = self.__generate_rand_pos_on_path(self.__path.poses, 100, 2)\n dist = self.__mean_sqare_dist_((wp[0] - start_pos[0]), (wp[1] - start_pos[1]))\n count = 0\n\n #Trying to find a waypoint with a minimum distance of 5.\n count_thresh = 50\n dist_thresh = 5\n while (dist < dist_thresh and count < count_thresh):\n wp = self.__generate_rand_pos_on_path(self.__path.poses, 100, 2)\n dist = self.__mean_sqare_dist_((wp[0] - start_pos[0]), (wp[1] - start_pos[1]))\n count += 1\n\n if (count < count_thresh):\n # Found waypoint with a minimum distance of 5m\n wp.append(0.3)\n waypoints = np.vstack([waypoints, wp])\n else:\n # Didn't find waypoint with a minimum distance of 5m --> pedestrian will stand around.\n waypoints = np.vstack([waypoints, [start_pos[0], start_pos[1], 0.3]])\n return [id, start_pos, waypoints]\n # self.__spawn_ped(start_pos, waypoints, id)\n\n def __get_random_crossing_ped(self, id):\n \"\"\"\n Spawning a pedestrian crossing the path. The pedestrian's waypoints are generated randomply\n \"\"\"\n try:\n pos_index = random.randint(0, len(self.__path.poses) - 2)\n except ValueError:\n print(\"Path length < 2. No crossing pedestrians are spawned.\")\n return []\n\n try:\n path_pose = self.__path.poses[pos_index]\n path_pose_temp = self.__path.poses[pos_index + 1]\n except IndexError:\n print(\"IndexError from retrieving path pose.\")\n return []\n\n angle = math.atan2((path_pose_temp.pose.position.y - path_pose.pose.position.y),\n (path_pose_temp.pose.position.x - path_pose.pose.position.x)) + math.pi / 2\n\n start_pos = self.__generate_rand_pos_near_pos(path_pose, 4, angle + math.pi)\n start_pos.append(0.0)\n\n waypoints = np.array([], dtype=np.int64).reshape(0, 3)\n wp1 = self.__generate_rand_pos_near_pos(path_pose, 4, angle + math.pi)\n waypoints = np.vstack([waypoints, [wp1[0], wp1[1], 0.3]])\n\n wp2 = self.__generate_rand_pos_near_pos(path_pose, 4, angle)\n dist = self.__mean_sqare_dist_((wp2[0] - wp1[0]), (wp2[1] - wp1[1]))\n\n #Trying to find a waypoint with a minimum distance of 5.\n count = 0\n count_thresh = 50\n dist_thresh = 10\n # while (dist < dist_thresh and count < count_thresh):\n wp2 = self.__generate_rand_pos_near_pos(path_pose, 7, angle)\n dist = self.__mean_sqare_dist_((wp2[0] - wp1[0]), (wp2[1] - wp1[1]))\n count += 1\n\n # if (count < count_thresh):\n # Found waypoint with a minimum distance of 5m\n wp2.append(0.3)\n waypoints = np.vstack([waypoints, wp2])\n\n rand = random.uniform(0.0, 1.0)\n start_pos = [wp1[0] + rand * (wp2[0] - wp1[0]), wp1[1] + rand * (wp2[1] - wp1[1]), 0.0]\n return [id, start_pos, waypoints]\n # self.__spawn_ped(start_pos, waypoints, id)\n\n def __respawn_peds(self, peds):\n \"\"\"\n Spawning one pedestrian in the simulation.\n :param start_pos start position of the pedestrian.\n :param wps waypoints the pedestrian is supposed to walk to.\n :param id id of the pedestrian.\n \"\"\"\n srv = SpawnPeds()\n srv.peds = []\n for ped in peds:\n msg = Ped()\n msg.id = ped[0]\n msg.pos = Point()\n msg.pos.x = ped[1][0]\n msg.pos.y = ped[1][1]\n msg.pos.z = ped[1][2]\n msg.type = self.__ped_type\n msg.number_of_peds = 1\n msg.yaml_file = self.__ped_file\n msg.waypoints = []\n for pos in ped[2]:\n p = Point()\n p.x = pos[0]\n p.y = pos[1]\n p.z = pos[2]\n msg.waypoints.append(p)\n srv.peds.append(msg)\n\n rospy.wait_for_service('%s/pedsim_simulator/respawn_peds' % self.NS)\n try:\n # self.__spawn_ped_srv.call(srv.peds)\n self.__respawn_peds_srv.call(srv.peds)\n except rospy.ServiceException:\n print('Spawn object: rospy.ServiceException. Closing serivce')\n try:\n self.__spawn_model.close()\n except AttributeError:\n print('Spawn object close(): AttributeError.')\n return\n self.__peds = peds\n return\n\n def __generate_rand_pos_on_path(self, path, range, max_r):\n \"\"\"\n Generating a random position on/near a path.\n :param path the path on that the position is generated\n :param range the range from the beginning and end of the path,\n that is not considered during generation\n :param max_r the radius around the path, where the position is generated\n \"\"\"\n try:\n path_pose = random.choice(path[range:-range])\n except IndexError:\n try:\n path_pose = path[range]\n except IndexError:\n return [0, 0]\n pos_on_map = False\n while not pos_on_map:\n alpha = 2 * math.pi * random.random()\n r = max_r * math.sqrt(random.random())\n x = r * math.cos(alpha) + path_pose.pose.position.x\n y = r * math.sin(alpha) + path_pose.pose.position.y\n pos_on_map = self.__is_pos_valid(x, y, self.__map)\n pos_on_map = True\n return [x,y]\n\n def __is_new_path_available(self, goal, start):\n \"\"\"\n Waiting for path to be published.\n :return True if path is available\n\n \"\"\"\n is_available = False\n begin = time.time()\n while (time.time() - begin) < 0.1:\n if self.__path.header.stamp <= self.__old_path_stamp or len(self.__path.poses) == 0:\n time.sleep(0.0001)\n else:\n is_available = True\n self.__old_path_stamp = self.__path.header.stamp\n break\n dist_goal = 10\n dist_start = 10\n\n if len(self.__path.poses) > 2:\n #Checking of correct path has been published\n dist_goal = self.__mean_sqare_dist_((goal[0] - self.__path.poses[-1].pose.position.x),\n (goal[1] - self.__path.poses[-1].pose.position.y))\n dist_start = self.__mean_sqare_dist_((start[0] - self.__path.poses[0].pose.position.x),\n (start[1] - self.__path.poses[0].pose.position.y))\n if not is_available or dist_goal > 0.5 or dist_start > 0.5:\n is_available = False\n print(\"path not valid!\")\n return is_available\n\n def __generate_rand_pos_near_pos(self, path_pose, max_r, alpha):\n \"\"\"\n Generating a random position2 close to another position1 within a radius of max_r.\n :param path_pose position1\n :param max_r radius in that position2 is generated\n :param alpha approx angle between position1 and position2\n :return [x, y] position2\n\n \"\"\"\n\n pos_on_map = False\n r = max_r\n while not pos_on_map:\n if (random.random() > 0.5):\n alpha = alpha + random.random() * 45 * math.pi/180\n else:\n alpha = alpha - random.random() * 45 * math.pi/180\n\n x = r * math.cos(alpha) + path_pose.pose.position.x\n y = r * math.sin(alpha) + path_pose.pose.position.y\n pos_on_map = self.__is_pos_valid(x, y, self.__map)\n r -= 0.3\n\n return [x, y]\n\n def __map_callback(self, data):\n \"\"\"\n Receiving map from map topic\n :param map data\n :return:\n \"\"\"\n self.__map = data\n return\n\n def __path_callback(self, data):\n self.__path = data\n\n def __goal_status_callback(self, data):\n \"\"\"\n Recovery method for stable learning:\n Checking goal status callback from global planner.\n If goal is not valid, new goal will be published.\n :param status_callback\n \"\"\"\n if len(data.status_list) > 0:\n last_element = data.status_list[-1]\n if last_element.status == 4:\n if(self.__move_base_status_id != last_element.goal_id.id):\n # self.set_random_static_task()\n self.__move_base_status_id = last_element.goal_id.id\n\n return\n\n def __mean_sqare_dist_(self, x, y):\n \"\"\"\n Computing mean square distance of x and y\n :param x, y\n :return: sqrt(x^2 + y^2)\n \"\"\"\n return math.sqrt(math.pow(x, 2) + math.pow(y, 2))\n\n def __yaw_to_quat(self, yaw):\n \"\"\"\n Computing corresponding quaternion q to angle yaw [rad]\n :param yaw\n :return: q\n \"\"\"\n q = Quaternion(axis=[0, 0, 1], angle=yaw)\n return q.elements" }, { "path": "rl_agent/src/rl_agent/env_wrapper/__init__.py", "content": "" }, { "path": "rl_agent/src/rl_agent/env_wrapper/ros_env.py", "content": "'''\n @name: ros_env.py\n @brief: This (abstract) class is a simulation environment wrapper.\n It communicates with the BaseLocalPlanner in ROS and\n provides all relevant methods for the RL-library stable baselines.\n @author: Ronja Gueldenring\n @version: 3.5\n @date: 2019/04/05\n'''\n\n\n# python relevant\nimport math\nimport random\nimport numpy as np\nimport gym\nimport time\n\n# ros-relevant\nimport rospy\n\n# messages\nfrom std_msgs.msg import Bool\nfrom sensor_msgs.msg import LaserScan\nfrom rl_msgs.msg import Waypoint\nfrom geometry_msgs.msg import TwistStamped, Twist, Pose\nfrom nav_msgs.msg import OccupancyGrid\n\n# Helper classes\nfrom rl_agent.env_utils.debug_ros_env import DebugRosEnv\nfrom rl_agent.env_utils.reward_container import RewardContainer\nfrom rl_agent.env_utils.task_generator import TaskGenerator\nclass RosEnvAbs(gym.Env):\n '''\n This (abstract) class is a simulation environment wrapper.\n It communicates with the BaseLocalPlanner in ROS and\n provides all relevant methods for the RL-library stable baselines.\n '''\n 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):\n super(RosEnvAbs, self).__init__()\n rospy.init_node(\"ros_env_%s\"%ns, anonymous=True)\n\n #Setting random seed.\n seed = random.randint(0,1000)\n random.seed(seed)\n np.random.seed(seed)\n\n # Class variables\n self.STATE_SIZE = state_size\n self.observation_space = observation_space\n self.ACTION_SIZE = action_size\n self.action_space = action_space\n\n self.NS = ns # namespace\n self.MODE = execution_mode # mode: TRAIN, EXEC, EXEC_RW, EVAL\n\n self.input_img_ = OccupancyGrid() # occupancy grid containing state information\n self.twist_ = TwistStamped() # speed of robot\n self.__trigger = False # triggers agent to get state and compute action\n self.debug_ = debug # enable debugging\n self.done_ = True # Episode done?\n self.merged_scan_ = LaserScan()\n\n if self.MODE == \"train\" or self.MODE == \"eval\":\n self.__task_mode = task_mode # \"ped\", \"static\", \"toggle_ped_static\", \"ped_static\"\n self.__toggle = True\n\n self.GOAL_RADIUS = goal_radius # radius, when goal is reached\n self.WP_RADIUS = wp_radius # radius, when waypoint is reached\n self.ROBOT_RADIUS = robot_radius # radius of the robot\n self.REWARD_FUNC = reward_fnc # which reward function should be used.\n\n self.__is_env_closed = False\n self.__num_iterations = 0 # counting number of iterations for each episode\n self.__transformed_goal = Pose() # Goal in robot frame\n self.wp_ = Waypoint() # next waypoints on global plan\n self.static_scan_ = LaserScan() # most recent static Laserscan\n self.ped_scan_ = LaserScan() # most recent pedestrian Laserscan\n\n\n\n # Helper Classes\n self.__state_collector = state_collector\n if self.debug_:\n self.debugger_ = DebugRosEnv(self.NS, stack_offset)\n if self.MODE == \"train\" or self.MODE == \"eval\":\n if len(self.action_space.shape) == 0:\n self.__reward_cont = RewardContainer(self.NS, robot_radius, goal_radius, self.v_max_)\n else:\n self.__reward_cont = RewardContainer(self.NS, robot_radius, goal_radius, self.action_space.high[0])\n self.__task_generator = TaskGenerator(self.NS, self.__state_collector, self.ROBOT_RADIUS)\n\n # Subscriber\n self.__trigger_sub = rospy.Subscriber(\"%s/trigger_agent\" % (self.NS), Bool, self.__trigger_callback)\n\n # Publisher\n self.__agent_action_pub = rospy.Publisher('%s/rl_agent/action' % (self.NS), Twist, queue_size=1)\n\n # Sleeping so that py-Publisher has time to setup!\n time.sleep(2)\n\n def step(self, action):\n \"\"\"\n Action is forwarded to simulation. As reaction a new state is received.\n Depending on state observation, reward and done is computed.\n :return: obs, reward, done, info\n \"\"\"\n # Publishing action\n # start = time.time()\n action = self.get_cmd_vel_(action)\n self.__agent_action_pub.publish(action)\n # print(\"publish cmd_vel: %f\"%(time.time() - start))\n\n # waiting for robot-cycle to end\n begin = time.time()\n while not self.__trigger:\n if self.MODE == \"train\" or self.MODE == \"eval\":\n # Detecting if pipeline is broke --> resetting\n if (time.time() - begin) > 20.0:\n rospy.logerr(\"%s, step(): Timeout while waiting for local planner.\" % (self.NS))\n self.reset()\n self.__agent_action_pub.publish(action)\n begin = time.time()\n time.sleep(0.00001)\n self.__trigger = False\n # print(\"waiting for BaseLocalPlanner: %f\"%(time.time() - begin))\n\n # start = time.time()\n self.__collect_state()\n # print(\"__collect_state: %f\"%(time.time() - start))\n\n # start = time.time()\n obs = self.get_observation_()\n # print(\"get_observation_: %f\"%(time.time() - start))\n info = {}\n if self.MODE == \"train\" or self.MODE == \"train_demo\" or self.MODE == \"eval\":\n # start = time.time()\n #info = [self.__num_iterations]\n action = np.array([action.linear.x, action.angular.z])\n reward = self.__compute_reward(action)\n self.done_, done_reason = self.__is_done(self.__num_iterations, self.static_scan_.ranges, self.ped_scan_.ranges, reward)\n info[\"done_reason\"] = done_reason\n # print(\"reward, done, ...: %f\" % (time.time() - start))\n else:\n reward = 0\n self.done_ = False\n\n return obs, reward, self.done_, info\n\n def close(self):\n \"\"\"\n Function executed when closing the environment.\n Use it for closing GUIS and other systems that need closing.\n :return:\n \"\"\"\n self.__stop_robot()\n __is_env_closed = True\n\n def __set_task(self):\n \"\"\"\n The task is set according to self.__task_mode and self.MODE.\n The different task_modes are the following:\n - ped: Spawning only pedestrians on path\n - static: Spawning only static obstacles on path\n - toggle_ped_static: Spawning alternating static obstacles and pedestrians\n - ped_static: Spawning static obstacles and pedestrians at the same time\n \"\"\"\n if (self.MODE == \"train\" or self.MODE == \"eval\"):\n if self.__task_mode == \"ped\":\n self.__task_generator.set_random_ped_task()\n elif self.__task_mode == \"ped_short\":\n self.__task_generator.set_random_short_ped_task()\n elif self.__task_mode == \"static\":\n self.__task_generator.set_random_static_task()\n elif self.__task_mode == \"toggle_ped_static\":\n if self.__toggle:\n self.__task_generator.set_random_static_task()\n else:\n self.__task_generator.set_random_ped_task()\n self.__toggle = (not self.__toggle)\n elif self.__task_mode == \"ped_static\":\n self.__task_generator.set_random_static_ped_task()\n\n\n def reset(self):\n \"\"\"\n Resetting simulation environment. That means, the robots position and goal\n are set randomly. Furthermore obstacles are spawned on that path.\n :return: initial observation of episode\n \"\"\"\n if self.MODE != \"train\" and self.MODE != \"train_demo\" and self.MODE != \"eval\":\n return self.get_observation_()\n\n # resetting task\n self.__set_task()\n self.__agent_action_pub.publish(Twist())\n\n # waiting for planner to be ready for its first action\n begin = time.time()\n while not self.__trigger and not rospy.is_shutdown():\n if (time.time() - begin) > 20.0:\n rospy.logerr(\"%s, reset(): Timeout while waiting for local planner.\" % (self.NS))\n self.__set_task()\n begin = time.time()\n time.sleep(0.001)\n\n self.__trigger = False\n\n # reseting internal state values\n self.__num_iterations = 0\n\n # Initializing state variables\n self.__task_generator.take_sim_step()\n self.__collect_state()\n self.__reward_cont.reset(self.wp_.points)\n\n return self.get_observation_()\n\n def get_observation_(self):\n \"\"\"\n Function returns state that will be fed to the rl-agent\n It includes laserscan data, waypoint information, ...\n :param\n :return: state\n \"\"\"\n raise NotImplementedError(\"Not implemented!\")\n\n def __collect_state(self):\n \"\"\"\n State is collected.\n It can include the following information\n image containing laser data and waypoint\n goal in robot frame\n velocity of robot\n raw laser scans\n waypoints in robot frame.\n \"\"\"\n [self.static_scan_, self.ped_scan_, self.merged_scan_, self.input_img_, self.wp_, self.twist_, self.__transformed_goal] = self.__state_collector.get_state()\n if(self.debug_):\n self.debugger_.show_wp(self.wp_)\n\n def __compute_reward(self, action):\n \"\"\"\n Reward function gives feedback on taken action of the agent.\n :param\n :return: reward\n \"\"\"\n if self.REWARD_FUNC == 1:\n reward = self.__reward_cont.rew_func_1(self.merged_scan_, self.wp_, self.__transformed_goal)\n elif self.REWARD_FUNC == 2.1:\n reward = self.__reward_cont.rew_func_2_1(self.static_scan_, self.ped_scan_, self.wp_, self.twist_,\n self.__transformed_goal)\n elif self.REWARD_FUNC == 2.2:\n reward = self.__reward_cont.rew_func_2_2(self.static_scan_, self.ped_scan_, self.wp_, self.twist_, self.__transformed_goal)\n elif self.REWARD_FUNC == 19:\n reward = self.__reward_cont.rew_func_19(self.static_scan_, self.ped_scan_, self.wp_, self.twist_,\n self.__transformed_goal)\n\n else:\n raise NotImplementedError\n\n if self.debug_:\n self.debugger_.show_reward(reward)\n return reward\n\n def __is_done(self, num_iterations, static_scan, ped_scan, reward):\n \"\"\"\n Checks if end of episode is reached. It is reached,\n if maximum number of episodes is reached,\n if the goal is reached,\n if the robot collided with obstacle\n if the reward function returns a high negative value.\n :param current state\n :return: reward\n \"\"\"\n # Is goal reached?\n dist_to_goal = self.mean_sqare_dist_(self.__transformed_goal.position.x,\n self.__transformed_goal.position.y)\n # Obstacle collision?\n min_obstacle_dist = np.amin(self.merged_scan_.ranges)\n\n # Ped in box?\n # is_ped_in_box = self.__reward_cont.is_ped_in_box(self.ped_scan_, 0.66, 0.86, 0.46)\n if self.MODE == \"eval\":\n max_iteration = 2000\n else:\n max_iteration = 650\n\n if dist_to_goal < self.GOAL_RADIUS:\n return [True, 1]\n if min_obstacle_dist < self.ROBOT_RADIUS:\n return [True, 2]\n elif reward < -5:\n return [True, 5]\n elif num_iterations > max_iteration:\n return [True, 3]\n # elif self.MODE == \"train\" and self.mean_sqare_dist_(self.wp_.points[0].x, self.wp_.points[0].y) > 4:\n # return [True, 4]\n\n self.__num_iterations += 1\n return [False, 0]\n\n\n def __trigger_callback(self, data):\n \"\"\"\n If trigger is True, robot executed action successfully and provides a new state.\n The agent can now determine the next action.\n \"\"\"\n\n self.__trigger = data.data\n return\n\n def mean_sqare_dist_(self, x, y):\n \"\"\"\n Computing mean square distance of x and y\n :param x, y\n :return: sqrt(x^2 + y^2)\n \"\"\"\n return math.sqrt(math.pow(x, 2) + math.pow(y, 2))\n\n def get_cmd_vel_(self, action):\n \"\"\"\n Decoding action (from rl_agent) to cmd_vel message\n :param action\n :return: cmd_vel message\n \"\"\"\n raise NotImplementedError(\"Not implemented!\")\n\n def get_action_list(self):\n \"\"\"\n Getter for the action list. Is empty, when continuous action space\n :return: action list\n \"\"\"\n raise NotImplementedError(\"Not implemented!\")\n\n def get_goal_radius(self):\n \"\"\"\n Getter for goal radius.\n :return: goal radius\n \"\"\"\n return self.GOAL_RADIUS\n\n def get_wp_radius(self):\n \"\"\"\n Getter for waypoint radius.\n :return: waypoint radius\n \"\"\"\n return self.WP_RADIUS\n\n def get_state_size(self):\n \"\"\"\n Getter for state size.\n :return: state size\n \"\"\"\n return self.STATE_SIZE\n\n def get_action_size(self):\n \"\"\"\n Getter for action size. Is inf if continuous\n :return: action size\n \"\"\"\n return self.ACTION_SIZE\n" }, { "path": "rl_agent/src/rl_agent/env_wrapper/ros_env_cont_img.py", "content": "'''\n @name: ros_env_cont_img.py\n @brief: This class is a simulation environment wrapper for\n the X-Image Representation\n with continuous action space.\n @author: Ronja Gueldenring\n @version: 3.5\n @date: 2019/04/05\n'''\n\n# ros-relevant\nimport rospy\n\n# python relevant\nimport numpy as np\nfrom gym import spaces\n\n#custom classes\nfrom rl_agent.env_wrapper.ros_env_img import RosEnvImg\n\n# Messages\nfrom geometry_msgs.msg import Twist\n\n\n# Parameters\nGOAL_RADIUS = 0.4\nWAYPOINT_RADIUS = 0.2\n\nclass RosEnvContImg(RosEnvImg):\n '''\n This class is a simulation environment wrapper for\n the X-Image Representation\n with continuous action space.\n '''\n\n 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\"):\n img_width = rospy.get_param(\"%s/rl_agent/img_width_pos\"%ns) + rospy.get_param(\"%s/rl_agent/img_width_neg\"%ns)\n img_height = rospy.get_param(\"%s/rl_agent/img_height\"%ns)\n state_size = (img_height, img_width, 1)\n observation_space = spaces.Box(low=0, high=100, shape=state_size, dtype=np.float)\n action_space = spaces.Box(low=np.array([0.0, -1.2]), high=np.array([0.8, 1.2]), dtype=np.float)\n # action_space = spaces.Box(low=np.array([0.0, 0.0]), high=np.array([0.0, 0.0]), dtype=np.float)\n 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)\n\n def get_cmd_vel_(self, action):\n vel_msg = Twist()\n vel_msg.linear.x = action[0]\n vel_msg.angular.z = action[1]\n return vel_msg\n\n def get_action_list(self):\n action_list = []\n return action_list\n" }, { "path": "rl_agent/src/rl_agent/env_wrapper/ros_env_cont_img_vel.py", "content": "'''\n @name: ros_env_cont_img_vel.py\n @brief: This class is a simulation environment wrapper for\n the X-Image Speed Representation\n with continuous action space.\n @author: Ronja Gueldenring\n @version: 3.5\n @date: 2019/04/05\n'''\n\n# ros-relevant\nimport rospy\n\n# python relevant\nimport numpy as np\nfrom gym import spaces\n\n#custom classes\nfrom rl_agent.env_wrapper.ros_env_img_vel import RosEnvImgVel\n\n# Messages\nfrom geometry_msgs.msg import Twist\n\n\n# Parameters\nGOAL_RADIUS = 0.4\nWAYPOINT_RADIUS = 0.2\n\n\nclass RosEnvContImgVel(RosEnvImgVel):\n '''\n This class is a simulation environment wrapper for\n the X-Image Speed Representation\n with continuous action space.\n '''\n 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\"):\n img_width = rospy.get_param(\"%s/rl_agent/img_width_pos\" % ns) + rospy.get_param(\n \"%s/rl_agent/img_width_neg\" % ns)\n img_height = rospy.get_param(\"%s/rl_agent/img_height\" % ns)\n state_size = (img_height + 1, img_width , 1)\n observation_space = spaces.Box(low=0, high=100, shape=state_size, dtype=np.float)\n action_space = spaces.Box(low=np.array([0.0, -0.7]), high=np.array([0.5, 0.7]), dtype=np.float)\n 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)\n\n def get_cmd_vel_(self, action):\n vel_msg = Twist()\n vel_msg.linear.x = action[0]\n vel_msg.angular.z = action[1]\n return vel_msg\n\n def get_action_list(self):\n action_list = []\n return action_list\n" }, { "path": "rl_agent/src/rl_agent/env_wrapper/ros_env_cont_raw_data.py", "content": "'''\n @name: ros_env_cont_raw_data.py\n @brief: This class is a simulation environment wrapper for\n the Raw Representation\n with continuous action space.\n @author: Ronja Gueldenring\n @version: 3.5\n @date: 2019/04/05\n'''\n\n\nimport rospy\n# python relevant\nimport numpy as np\nfrom gym import spaces\n\n# custom classes\nfrom rl_agent.env_wrapper.ros_env_raw_data import RosEnvRaw\n\n# messages\nfrom geometry_msgs.msg import Twist\n\n\n# Parameters\nGOAL_RADIUS = 0.4\nWAYPOINT_RADIUS = 0.2\n\nclass RosEnvContRaw(RosEnvRaw):\n '''\n This class is a simulation environment wrapper for\n the X-Image Representation\n with continuous action space.\n '''\n 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\"):\n state_size_t = rospy.get_param(\"%s/rl_agent/scan_size\"% ns) + rospy.get_param(\"%s/rl_agent/num_of_wps\"%ns)*2\n state_size = (1, state_size_t, 1)\n observation_space = spaces.Box(low=0, high=6, shape=state_size, dtype=np.float)\n action_space = spaces.Box(low=np.array([0.0, -0.5]), high=np.array([0.5, 0.5]), dtype=np.float)\n\n 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)\n\n def get_cmd_vel_(self, action):\n vel_msg = Twist()\n vel_msg.linear.x = action[0]\n vel_msg.angular.z = action[1]\n return vel_msg\n\n def get_action_list(self):\n action_list = []\n return action_list\n" }, { "path": "rl_agent/src/rl_agent/env_wrapper/ros_env_cont_raw_scan_prep_wp.py", "content": "'''\n @name: ros_env_cont_raw_scan_prep_wo.py\n @brief: This class is a simulation environment wrapper for\n the Polar Representation\n with continuous action space.\n @author: Ronja Gueldenring\n @version: 3.5\n @date: 2019/04/05\n'''\n\nimport rospy\n# python relevant\nimport numpy as np\nfrom gym import spaces\n\n# custom classes\nfrom rl_agent.env_wrapper.ros_env_raw_scan_prep_wp import RosEnvRawScanPrepWp\n\n# messages\nfrom geometry_msgs.msg import Twist\n\n\n# Parameters\nGOAL_RADIUS = 0.4\nWAYPOINT_RADIUS = 0.2\n\nclass RosEnvContRawScanPrepWp(RosEnvRawScanPrepWp):\n '''\n This class is a simulation environment wrapper for\n the Polar Representation\n with continuous action space.\n '''\n 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\"):\n state_size_t = rospy.get_param(\"%s/rl_agent/scan_size\"% ns)\n state_size = (state_size_t,2, 1)\n observation_space = spaces.Box(low=0, high=6, shape=state_size, dtype=np.float)\n action_space = spaces.Box(low=np.array([0.0, -0.5]), high=np.array([0.5, 0.5]), dtype=np.float)\n\n 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)\n\n def get_cmd_vel_(self, action):\n vel_msg = Twist()\n vel_msg.linear.x = action[0]\n vel_msg.angular.z = action[1]\n return vel_msg\n\n def get_action_list(self):\n action_list = []\n return action_list\n" }, { "path": "rl_agent/src/rl_agent/env_wrapper/ros_env_disc_img.py", "content": "'''\n @name: ros_env_cont_img.py\n @brief: This class is a simulation environment wrapper for\n the X-Image Representation\n with discrente action space.\n @author: Ronja Gueldenring\n @version: 3.5\n @date: 2019/04/05\n'''\n\n# ros-relevant\nimport rospy\n\n# python relevant\nimport numpy as np\nfrom gym import spaces\n\n# custom classes\nfrom rl_agent.env_wrapper.ros_env_img import RosEnvImg\n\n# messages\nfrom geometry_msgs.msg import Twist\n\n# Parameters\nACTION_SIZE = 6\nGOAL_RADIUS = 0.4\nWAYPOINT_RADIUS = 0.2\n\n\nclass RosEnvDiscImg(RosEnvImg):\n '''\n This class is a simulation environment wrapper for\n the X-Image Representation\n with discrente action space.\n '''\n def __init__(self, ns, state_collector, stack_offset, stack_size, robot_radius, reward_fnc, debug, execution_mode=\"train\", task_mode=\"static\"):\n img_width = rospy.get_param(\"%s/rl_agent/img_width_pos\"%ns) + rospy.get_param(\"%s/rl_agent/img_width_neg\"%ns)\n img_height = rospy.get_param(\"%s/rl_agent/img_height\"%ns)\n state_size = (img_height, img_width, 1)\n observation_space = spaces.Box(low=0, high=100, shape=state_size, dtype=np.float)\n\n self.action = np.array([0.0, 0.0])\n self.v_max_ = 0.8 # ?1.5?\n self.w_max_ = 1.2\n self.__possible_actions = {\n 0: [0.0, -self.w_max_],\n 1: [self.v_max_, 0.0],\n 2: [0.0, self.w_max_],\n 3: [self.v_max_, self.w_max_/2],\n 4: [self.v_max_, -self.w_max_/2],\n 5: [0.0, 0.0],\n }\n action_size = len(self.__possible_actions)\n action_space = spaces.Discrete(action_size)\n super(RosEnvDiscImg, self).__init__(ns, state_collector, execution_mode, task_mode, state_size,\n observation_space, stack_offset, action_size, action_space, debug, GOAL_RADIUS,\n WAYPOINT_RADIUS, robot_radius, reward_fnc)\n\n def get_cmd_vel_(self, action):\n encoded_action = self.__encode_action(action)\n vel_msg = Twist()\n vel_msg.linear.x = encoded_action[0]\n vel_msg.angular.z = encoded_action[1]\n return vel_msg\n\n def __encode_action(self, action):\n return self.__possible_actions.get(action, 5)\n\n def get_action_list(self):\n action_list = []\n for i in range(self.ACTION_SIZE):\n action_list.append(self.__encode_action(i))\n return action_list\n\n" }, { "path": "rl_agent/src/rl_agent/env_wrapper/ros_env_disc_img_vel.py", "content": "'''\n @name: ros_env_cont_img_vel.py\n @brief: This class is a simulation environment wrapper for\n the X-Image Speed Representation\n with discrete action space.\n @author: Ronja Gueldenring\n @version: 3.5\n @date: 2019/04/05\n'''\n\n# ros-relevant\nimport rospy\n\n# python relevant\nimport numpy as np\nfrom gym import spaces\n\n#custom classes\nfrom rl_agent.env_wrapper.ros_env_img_vel import RosEnvImgVel\n\n# Messages\nfrom geometry_msgs.msg import Twist\n\n\n# Parameters\nGOAL_RADIUS = 0.4\nWAYPOINT_RADIUS = 0.2\n\nclass RosEnvDiscImgVel(RosEnvImgVel):\n '''\n This class is a simulation environment wrapper for\n the X-Image Speed Representation\n with discrete action space.\n '''\n 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\"):\n img_width = rospy.get_param(\"%s/rl_agent/img_width_pos\" % ns) + rospy.get_param(\n \"%s/rl_agent/img_width_neg\" % ns)\n img_height = rospy.get_param(\"%s/rl_agent/img_height\" % ns)\n state_size = (img_height + 1, img_width, 1)\n observation_space = spaces.Box(low=0, high=100, shape=state_size, dtype=np.float)\n\n self.v_max_ = 0.8 # ?1.5?\n self.w_max_ = 1.2\n self.__possible_actions = {\n 0: [0.0, -self.w_max_],\n 1: [self.v_max_, 0.0],\n 2: [0.0, self.w_max_],\n 3: [self.v_max_, self.w_max_/2],\n 4: [self.v_max_, -self.w_max_/2],\n 5: [0.0, 0.0]\n # 6: [0.09, 0.0]\n }\n action_size = len(self.__possible_actions)\n action_space = spaces.Discrete(action_size)\n 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)\n\n def get_cmd_vel_(self, action):\n encoded_action = self.__encode_action(action)\n vel_msg = Twist()\n vel_msg.linear.x = encoded_action[0]\n vel_msg.angular.z = encoded_action[1]\n return vel_msg\n\n def __encode_action(self, action):\n return self.__possible_actions.get(action, 5)\n\n def get_action_list(self):\n action_list = []\n for i in range(self.ACTION_SIZE):\n action_list.append(self.__encode_action(i))\n return action_list\n" }, { "path": "rl_agent/src/rl_agent/env_wrapper/ros_env_disc_raw_data.py", "content": "'''\n @name: ros_env_cont_raw_data.py\n @brief: This class is a simulation environment wrapper for\n the Raw Representation\n with discrete action space.\n @author: Ronja Gueldenring\n @version: 3.5\n @date: 2019/04/05\n'''\n\n# python relevant\nimport numpy as np\nfrom gym import spaces\nimport rospy\n# custom classes\nfrom rl_agent.env_wrapper.ros_env_raw_data import RosEnvRaw\n\n# messages\nfrom geometry_msgs.msg import Twist\n\n\n# Parameters\nGOAL_RADIUS = 0.4\nWAYPOINT_RADIUS = 0.2\n\n\n\nclass RosEnvDiscRaw(RosEnvRaw):\n '''\n This class is a simulation environment wrapper for\n the Raw Representation\n with discrete action space.\n '''\n 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\"):\n state_size_t = rospy.get_param(\"%s/rl_agent/scan_size\"% ns) + rospy.get_param(\"%s/rl_agent/num_of_wps\"%ns)*2\n state_size = (1, state_size_t, 1)\n observation_space = spaces.Box(low=0, high=10, shape=state_size, dtype=np.float)\n\n self.action = np.array([0.0, 0.0])\n self.v_max_ = 0.8 # ?1.5?\n self.w_max_ = 1.2\n self.__possible_actions = {\n 0: [0.0, -self.w_max_],\n 1: [self.v_max_, 0.0],\n 2: [0.0, self.w_max_],\n 3: [self.v_max_, self.w_max_ / 2],\n 4: [self.v_max_, -self.w_max_ / 2],\n 5: [0.0, 0.0]\n # 6: [0.09, 0.0],\n }\n action_size = len(self.__possible_actions)\n\n action_space = spaces.Discrete(action_size)\n 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)\n self.action = np.array([0.0, 0.0])\n\n def get_cmd_vel_(self, action):\n encoded_action = self.__encode_action(action)\n vel_msg = Twist()\n vel_msg.linear.x = encoded_action[0]\n vel_msg.angular.z = encoded_action[1]\n return vel_msg\n\n def __encode_action(self, action):\n return self.__possible_actions.get(action, 5)\n\n def get_action_list(self):\n action_list = []\n for i in range(self.ACTION_SIZE):\n action_list.append(self.__encode_action(i))\n return action_list\n" }, { "path": "rl_agent/src/rl_agent/env_wrapper/ros_env_disc_raw_scan_prep_wp.py", "content": "'''\n @name: ros_env_disc_raw_scan_prep_wo.py\n @brief: This class is a simulation environment wrapper for\n the Polar Representation\n with discrete action space\n @author: Ronja Gueldenring\n @version: 3.5\n @date: 2019/04/05\n'''\n\n# python relevant\nimport numpy as np\nfrom gym import spaces\nimport rospy\n# custom classes\nfrom rl_agent.env_wrapper.ros_env_raw_scan_prep_wp import RosEnvRawScanPrepWp\n\n# messages\nfrom geometry_msgs.msg import Twist\n\n\n# Parameters\nGOAL_RADIUS = 0.4\nWAYPOINT_RADIUS = 0.2\n\nclass RosEnvDiscRawScanPrepWp(RosEnvRawScanPrepWp):\n '''\n This class is a simulation environment wrapper for\n the Polar Representation\n with discrete action space\n '''\n 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\"):\n state_size_t = rospy.get_param(\"%s/rl_agent/scan_size\"% ns)\n state_size = (state_size_t,2, 1)\n observation_space = spaces.Box(low=0, high=10, shape=state_size, dtype=np.float)\n\n self.v_max_ = 0.8 # ?1.5?\n self.w_max_ = 1.2\n self.__possible_actions = {\n 0: [0.0, -self.w_max_],\n 1: [self.v_max_, 0.0],\n 2: [0.0, self.w_max_],\n 3: [self.v_max_, self.w_max_ / 2],\n 4: [self.v_max_, -self.w_max_ / 2],\n 5: [0.0, 0.0],\n }\n action_size = len(self.__possible_actions)\n action_space = spaces.Discrete(action_size)\n 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)\n self.action = np.array([0.0, 0.0])\n\n def get_cmd_vel_(self, action):\n encoded_action = self.__encode_action(action)\n vel_msg = Twist()\n vel_msg.linear.x = encoded_action[0]\n vel_msg.angular.z = encoded_action[1]\n return vel_msg\n\n\n def __encode_action(self, action):\n return self.__possible_actions.get(action, 5)\n\n def get_action_list(self):\n action_list = []\n for i in range(self.ACTION_SIZE):\n action_list.append(self.__encode_action(i))\n return action_list\n" }, { "path": "rl_agent/src/rl_agent/env_wrapper/ros_env_img.py", "content": "'''\n @name: ros_env_img.py\n @brief: This (abstract) class is a simulation environment wrapper for\n the X-Image Representation.\n @author: Ronja Gueldenring\n @version: 3.5\n @date: 2019/04/05\n'''\n\n\n# python relevant\nimport numpy as np\n\n# custom classes\nfrom rl_agent.env_wrapper.ros_env import RosEnvAbs\n\n# ros-relevant\nimport rospy\n\nclass RosEnvImg(RosEnvAbs):\n '''\n This (abstract) class is a simulation environment wrapper for\n the X-Image Representation.\n '''\n 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):\n state_collector.set_state_mode(0)\n 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)\n\n\n def get_observation_(self):\n \"\"\"\n Function returns state that will be fed to the rl-agent\n It includes\n the laserscan and the waypoint information stored in an image.\n :return: state\n \"\"\"\n obs = np.zeros(self.STATE_SIZE, dtype=np.float)\n obs[:,:,0] = np.array(self.input_img_.data).reshape((self.STATE_SIZE[0:2]))\n\n if self.debug_:\n self.debugger_.show_input_occ_grid(self.input_img_)\n self.debugger_.show_input_image(obs[:,:,0])\n return obs\n\n" }, { "path": "rl_agent/src/rl_agent/env_wrapper/ros_env_img_vel.py", "content": "'''\n @name: ros_env_img_vel.py\n @brief: This (abstract) class is a simulation environment wrapper for\n the X-Image Speed Representation.\n @author: Ronja Gueldenring\n @version: 3.5\n @date: 2019/04/05\n'''\n\n# python relevant\nimport numpy as np\n\n# custom classes\nfrom rl_agent.env_wrapper.ros_env import RosEnvAbs\n\n# ros-relevant\nimport rospy\n\nclass RosEnvImgVel(RosEnvAbs):\n '''\n This (abstract) class is a simulation environment wrapper for\n the X-Image Speed Representation.\n '''\n 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):\n state_collector.set_state_mode(0)\n 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)\n\n\n def get_observation_(self):\n \"\"\"\n Function returns state that will be fed to the rl-agent\n It includes\n the laserscan and waypoint data in form of an image,\n the last speed of the robot, that is raw and stored in the last row in the state matrix.\n :return: state\n \"\"\"\n obs = np.zeros(self.STATE_SIZE, dtype=np.float)\n recent_img = np.array(self.input_img_.data).reshape((self.STATE_SIZE[0] - 1, self.STATE_SIZE[1]))\n obs[0:-1,:,0] = recent_img\n obs[-1, 0:2, 0] = [self.twist_.twist.linear.x, self.twist_.twist.angular.z]\n if self.debug_:\n self.debugger_.show_input_occ_grid(self.input_img_)\n self.debugger_.show_image_stack(obs[:-1, :, :])\n return obs\n\n" }, { "path": "rl_agent/src/rl_agent/env_wrapper/ros_env_raw_data.py", "content": "'''\n @name: ros_env_raw_data.py\n @brief: This (abstract) class is a simulation environment wrapper for\n the Raw Representation.\n @author: Ronja Gueldenring\n @version: 3.5\n @date: 2019/04/05\n'''\n\n# python relevant\nimport numpy as np\nimport rospy\n\n# custom classes\nfrom rl_agent.env_wrapper.ros_env import RosEnvAbs\nfrom sensor_msgs.msg import LaserScan\n\nclass RosEnvRaw(RosEnvAbs):\n '''\n This (abstract) class is a simulation environment wrapper for\n the Raw Representation.\n '''\n 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):\n state_collector.set_state_mode(2)\n\n 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)\n self.__scan_size = rospy.get_param(\"%s/rl_agent/scan_size\"%ns)\n\n def get_observation_(self):\n \"\"\"\n Function returns state that will be fed to the rl-agent\n It includes\n the raw laser scan data,\n the raw waypoint data.\n :return: state\n \"\"\"\n waypoint = self.wp_\n num_of_wps = len(waypoint.points)\n state = np.ones(self.STATE_SIZE, dtype=np.float)\n\n # add laserscan\n state[0, 0:self.__scan_size, 0] = self.merged_scan_.ranges\n\n # add goal position\n wp_index = self.STATE_SIZE[1] - num_of_wps * 2\n for i in range(num_of_wps):\n state[0, (wp_index + i*2):(wp_index + i*2 + 2),0] = [waypoint.points[i].x, waypoint.points[i].y]\n\n # Discretize to a resolution of 5cm.\n state = np.round(np.divide(state, 0.05))*0.05\n if self.debug_:\n debug_scan = LaserScan()\n debug_scan.header = self.merged_scan_.header\n debug_scan.angle_min = self.merged_scan_.angle_min\n debug_scan.angle_max = self.merged_scan_.angle_max\n debug_scan.angle_increment = self.merged_scan_.angle_increment\n debug_scan.range_max = 7.0\n debug_scan.ranges = state[0, 0:self.__scan_size, 0]\n self.debugger_.show_input_scan(debug_scan)\n return state\n" }, { "path": "rl_agent/src/rl_agent/env_wrapper/ros_env_raw_scan_prep_wp.py", "content": "'''\n @name: ros_env_raw_scan_prep_wo.py\n @brief: This class is a simulation environment wrapper for\n the Polar Representation.\n @author: Ronja Gueldenring\n @version: 3.5\n @date: 2019/04/05\n'''\n\n# python relevant\nimport numpy as np\nimport math\n# ros-relevant\nimport rospy\n# custom classes\nfrom rl_agent.env_wrapper.ros_env import RosEnvAbs\nfrom sensor_msgs.msg import LaserScan\n\nclass RosEnvRawScanPrepWp(RosEnvAbs):\n '''\n This class is a simulation environment wrapper for\n the Polar Representation.\n '''\n 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):\n state_collector.set_state_mode(2)\n 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)\n self.__res = rospy.get_param(\"%s/rl_agent/resolution\"%ns)\n\n def get_observation_(self):\n \"\"\"\n Function returns state that will be fed to the rl-agent\n It includes\n the raw laser scan data,\n the waypoint data in with the same format as the laser scan data.\n The distance of the waypoint is saved\n at the appropriate angle position in the vector.\n :return: state\n \"\"\"\n waypoint = self.wp_\n num_of_wps = len(waypoint.points)\n\n state = np.ones(self.STATE_SIZE, dtype=np.float)\n\n # add laserscan\n state[ :, 0, 0] = self.merged_scan_.ranges\n\n # generate wp-vector\n wp_vector = np.zeros(self.STATE_SIZE[0])\n for i in range(num_of_wps):\n dist = math.sqrt(math.pow(waypoint.points[i].x, 2) + math.pow(waypoint.points[i].y, 2))\n angle = math.atan2(waypoint.points[i].y, waypoint.points[i].x) + math.pi\n wp_vector[math.floor(angle/self.merged_scan_.angle_increment)] = dist\n state[:,1,0] = wp_vector\n\n # Discretize to a resolution of 5cm.\n state = np.round(np.divide(state, self.__res))*self.__res\n if self.debug_:\n debug_scan = LaserScan()\n # debug_scan.header.frame_id = self.merged_scan_.header.frame_id\n debug_scan.header = self.merged_scan_.header\n debug_scan.angle_min = self.merged_scan_.angle_min\n debug_scan.angle_max = self.merged_scan_.angle_max\n debug_scan.angle_increment = self.merged_scan_.angle_increment\n debug_scan.range_max = 7.0\n debug_scan.ranges = state[:, 0, 0]\n self.debugger_.show_scan_stack(debug_scan)\n return state\n" }, { "path": "rl_agent/src/rl_agent/evaluation/Analysis_eval.py", "content": "'''\n @name: Analysis_eval.py\n @brief: The class can be used to analyse the produced evaluation_data generated by Evaluation.py\n @author: Ronja Gueldenring\n @version: 3.5\n @date: 2019/04/05\n'''\n\nimport rospkg\nimport pickle\nimport math\nimport csv\n\nclass Analysis():\n def __init__(self):\n rospack = rospkg.RosPack()\n self.__rl_agent_path = rospack.get_path('rl_agent') # absolute path rl_agent-package\n\n def load_results(self, filename):\n results = []\n with open('%s.pickle'%(filename), 'rb') as fr:\n try:\n while True:\n d = pickle.load(fr)\n results.append(d)\n except EOFError:\n pass\n return results\n\n def get_timestep_list(self, results):\n timesteps = []\n for result in results:\n timesteps.append(result[\"timestep\"])\n\n return timesteps\n\n def reconstruct_timestep_array(self, timesteps):\n previous_ts = timesteps[0]\n break_indices = []\n\n for i, ts in enumerate(timesteps):\n if (previous_ts - ts) >1000:\n break_indices.append(i)\n previous_ts = ts\n\n break_indices.append((len(timesteps)))\n for k in range(len(break_indices)-1):\n base_timestep = timesteps[break_indices[k]-1]\n for j in range(break_indices[k], break_indices[k+1]):\n timesteps[j] = base_timestep + timesteps[j]\n\n return timesteps\n\n\n\n\n\n def get_scores(self, results):\n \"\"\"\n Analyses the success rate for all n episodes in results.\n :param results: list of n results\n :returns: success-rate, time_exceeded-rate, collision-rate (all lists)\n \"\"\"\n success_vec = []\n time_exceeded_vec = []\n collision_vec = []\n\n for i in range(0, len(results)):\n exit = results[i][\"success\"]\n collision = 0\n time_exceeded = 0\n success = 0\n if exit == -1:\n collision = 1\n elif exit == 0:\n time_exceeded = 1\n elif exit == 1:\n success = 1\n\n success_vec.append(success)\n time_exceeded_vec.append(time_exceeded)\n collision_vec.append(collision)\n \n return success_vec, time_exceeded_vec, collision_vec\n\n\n def get_percentual_success_drive(self, results):\n \"\"\"\n Computes the percentage of successful path following for each episode\n :param results: list of n results\n :return list of percentages of successful path following\n \"\"\"\n percentage_vec = []\n for result in results:\n if len(result[\"global_path\"].poses) == 0:\n percentage_vec.append(1)\n else:\n if result[\"success\"] == 1:\n percentage_vec.append(1)\n\n else:\n i = self.__closest_path_pos(result[\"global_path\"], result[\"driven_path\"].poses[-1].pose)\n percentage_vec.append(i / len(result[\"global_path\"].poses))\n return percentage_vec\n\n def get_path_length_ratio(self, results):\n \"\"\"\n Computes the path length ratio (driven path/global path)\n :param results: list of n results\n :return list of the path length ratios\n \"\"\"\n path_length_ratio_vec = []\n for result in results:\n if len(result[\"global_path\"].poses) == 0:\n path_length_ratio_vec.append(1)\n else:\n i = len(result[\"global_path\"].poses)\n if result[\"success\"] != 1:\n i = self.__closest_path_pos(result[\"global_path\"], result[\"driven_path\"].poses[-1].pose)\n global_path_length = self.__get_path_length(result[\"global_path\"].poses[:i])\n driven_path_length = self.__get_path_length(result[\"driven_path\"].poses)\n if global_path_length <= 0:\n path_length_ratio_vec.append(1)\n else:\n path_length_ratio_vec.append(driven_path_length/global_path_length)\n\n return path_length_ratio_vec\n\n def get_speed(self, results):\n \"\"\"\n Computes the average speed for each episode\n :param results: list of n results\n :return list of the average speed\n \"\"\"\n speed_vec = []\n for result in results:\n if len(result[\"global_path\"].poses) == 0:\n speed_vec.append(1.0)\n else:\n i = len(result[\"global_path\"].poses)\n if (result[\"success\"] != 1):\n i = self.__closest_path_pos(result[\"global_path\"], result[\"driven_path\"].poses[-1].pose)\n path_length = self.__get_path_length(result[\"global_path\"].poses[:i])\n speed = path_length/result[\"time\"].to_sec()\n # Handling outliers\n if (speed > 1.0):\n speed_vec.append(1.0)\n else:\n speed_vec.append(path_length/result[\"time\"].to_sec())\n return speed_vec\n\n def get_reward(self, filename):\n \"\"\"\n Getter for the reward for each episode\n :param results: list of n results\n :param filename: filename of the .csv-file, where the reward is saved.\n :return list of the rewards\n \"\"\"\n timesteps = []\n reward = []\n line_count = 0\n with open('%s.csv'%(filename), 'r') as csvfile:\n reader = csv.reader(csvfile, delimiter=',')\n\n for row in reader:\n if (line_count == 0):\n line_count += 1\n continue\n timesteps.append(float(row[1]))\n reward.append(float(row[2]))\n\n return [timesteps, reward]\n\n def __closest_path_pos(self, path, pose):\n \"\"\"\n Computes the path_pose on the path that is closest to pose\n :param path\n :param pose\n :return path_pose\n \"\"\"\n index = 0\n count = 0\n dist = 10\n for path_pose in path.poses:\n temp_dist = self.__mean_square_dist((pose.position.x - path_pose.pose.position.x),\n (pose.position.y - path_pose.pose.position.y))\n if (temp_dist < dist):\n dist = temp_dist\n index = count\n count += 1\n\n if (dist > 3):\n index = 0\n return index\n\n def __mean_square_dist(self, x, y):\n \"\"\"\n Computes the mean-square distance\n :param x\n :param y\n :return sqrt(x^2 + y^2)\n \"\"\"\n return math.sqrt(math.pow(x, 2) + math.pow(y, 2))\n\n\n def __get_path_length(self, poses):\n \"\"\"\n Computes the length of a path\n :param poses: poses on the path\n :return length of the path\n \"\"\"\n if len(poses) <= 1:\n return 0\n path_length = 0\n old_pose =poses[0].pose.position\n for pose in poses[1:]:\n new_pose = pose.pose.position\n path_length += self.__mean_square_dist((new_pose.x - old_pose.x), (new_pose.y - old_pose.y))\n old_pose = new_pose\n return path_length\n\n\n" }, { "path": "rl_agent/src/rl_agent/evaluation/Evaluation.py", "content": "'''\n @name: Analysis_eval.py\n @brief: The class records relevant data of the agent during driving. The data\n can be later used for analysing the training process.\n @author: Ronja Gueldenring\n @version: 3.5\n @date: 2019/04/05\n'''\n\nimport rospkg\nimport rospy\nimport numpy as np\nfrom rl_agent.env_utils.task_generator import TaskGenerator\nfrom nav_msgs.msg import Odometry, Path\nfrom sensor_msgs.msg import LaserScan\nfrom std_msgs.msg import Bool\nimport pickle\nimport time\nimport math\nfrom geometry_msgs.msg import PoseStamped, Twist, Point\nfrom flatland_msgs.msg import DebugTopicList\nfrom rosgraph_msgs.msg import Clock\nfrom pedsim_msgs.msg import AgentStates\nfrom visualization_msgs.msg import MarkerArray, Marker\n\nclass Evaluation():\n '''\n The class records relevant data of the agent during driving. The data\n can be later used for analyising the training process.\n '''\n def __init__(self, state_collector, ns, robot_radius = 0,robot_width = 0.58, robot_height = 0.89):\n self.__robot_radius = robot_radius # robot radius\n self.__robot_height = robot_height # robot width\n self.__robot_width = robot_width # robot heigth\n self.__odom = Odometry() # most recently published odometry of the robot\n self.__path = Path() # most recently published path\n self.__done = False # is episode done?\n self.__new_task_started = False # has a new task started?\n self.__state_collector = state_collector # for getting relevant state values of the robot\n self.__rl_agent_path = rospkg.RosPack().get_path('rl_agent') # absolute path rl_agent-package\n self.__flatland_topics = [] # list of flatland topics\n self.__timestep = 0 # actual timestemp of training\n self.__NS = ns\n self.MODE = rospy.get_param(\"%s/rl_agent/train_mode\", 1)\n self.__clock = Clock().clock\n self.__task_generator = TaskGenerator(self.__NS, self.__state_collector, 0.46)\n self.__recent_agent_states = []\n\n # Subscriber for getting data\n #self.__odom_sub = rospy.Subscriber(\"%s/odom\"%self.__NS, Odometry, self.__odom_callback, queue_size=1)\n self.__global_plan_sub = rospy.Subscriber(\"%s/move_base/NavfnROS/plan\"%self.__NS, Path, self.__path_callback, queue_size=1)\n # self.__path_sub = rospy.Subscriber(\"%s/move_base/GlobalPlanner/plan\" % self.__NS, Path, self.__path_callback, queue_size=1)\n self.__done_sub = rospy.Subscriber(\"%s/rl_agent/done\"%self.__NS, Bool, self.__done_callback, queue_size=1)\n self.__new_task_sub = rospy.Subscriber('%s/rl_agent/new_task_started'%self.__NS, Bool, self.__new_task_callback, queue_size=1)\n self.__flatland_topics_sub = rospy.Subscriber(\"%s/flatland_server/debug/topics\"%self.__NS, DebugTopicList, self.__flatland_topic_callback, queue_size=1)\n self.__agent_action_sub = rospy.Subscriber('%s/rl_agent/action'%self.__NS, Twist, self.__trigger_callback)\n self.__ped_sub = rospy.Subscriber('%s/pedsim_simulator/simulated_agents' % self.__NS, AgentStates, self.__ped_callback)\n if self.MODE == 1 or self.MODE == 0:\n self.clock_sub = rospy.Subscriber('%s/clock' % self.__NS, Clock, self.__clock_callback)\n\n # Publisher for generating qualitative image\n self.__driven_path_pub = rospy.Publisher('%s/rl_eval/driven_path'%self.__NS, Path, queue_size=1)\n self.__driven_path_pub2 = rospy.Publisher('%s/rl_eval/driven_path2'%self.__NS, Path, queue_size=1)\n self.__global_path_pub = rospy.Publisher('%s/rl_eval/global_path'%self.__NS, Path, queue_size=1)\n self.__agent_pub = rospy.Publisher('%s/rl_eval/viz_agents'%self.__NS, MarkerArray, queue_size=1)\n\n def load_evaluation_set(self, eval_set_path):\n \"\"\"\n Loading evaluation set with name eval_set_name.\n :param eval_data_path: path to evaluation data\n :param eval_set_name: name of evaluation set (.pickle)\n :return: evaluation_set as list\n \"\"\"\n with open('%s.pickle'%(eval_set_path), 'rb') as handle:\n self.__evaluation_set = pickle.load(handle)\n return len(self.__evaluation_set)\n\n def evaluate_set(self, evaluation_set_path, save_path):\n \"\"\"\n Evaluates an agent with a evaluation_set and saves results in evaluation_data/test\n :param eval_data_path: path to evaluation data\n :param evaluation_set_name: name of evaluation set (.pickle)\n :param agent_name: name of the agent (.pkl)\n \"\"\"\n task_generator = TaskGenerator(self.__NS, self.__state_collector, 0.46)\n self.load_evaluation_set(evaluation_set_path)\n results = []\n i = 0\n for task in self.__evaluation_set:\n print(\"Evaluating task %d...\"%i)\n task_generator.set_task(task)\n self.__done = False\n result = self.evaluate_episode(False)\n results.append(result)\n i += 1\n with open('%s.pickle' % (save_path), 'ab') as handle:\n pickle.dump(result, handle, protocol=pickle.HIGHEST_PROTOCOL)\n\n def evaluate_episode(self, train):\n \"\"\"\n Evaluates current episode\n :return result of the episode.\n\n \"\"\"\n result = {}\n if not train:\n result[\"global_path\"] = self.__path\n result[\"agent_states\"] = []\n\n done = False\n max_time = len(self.__path.poses)/10*2 # in secs\n if not self.MODE == 2:\n start_time = self.__clock\n else:\n start_time = rospy.get_rostime()\n driven_route = Path()\n driven_route.header = self.__path.header\n poses = []\n while not done:\n [static_scan, ped_scan, merged_scan, img, wp, twist, goal] = self.__state_collector.get_state()\n min_obstacle_dist = np.amin(merged_scan.ranges)\n dist_to_goal = math.sqrt(math.pow(goal.position.x, 2) + math.pow(goal.position.y, 2))\n\n # Check if task over\n pose = PoseStamped()\n pose.header = self.__odom.header\n pose.pose = self.__odom.pose.pose\n poses.append(pose)\n if not self.MODE == 2:\n now = self.__clock\n else:\n now = rospy.get_rostime()\n # Check if task over\n if min_obstacle_dist <= self.__robot_radius or \\\n self.__rect_robot_collision(static_scan, ped_scan, self.__robot_width, self.__robot_height):\n rospy.loginfo(\"Robot collided with obstacle.\")\n done = True\n result[\"success\"] = -1\n elif dist_to_goal < 0.65:\n rospy.loginfo(\"Goal reached.\")\n done = True\n result[\"success\"] = 1\n elif self.__done or (now - start_time).to_sec() > max_time:\n rospy.loginfo(\"Time exceeded.\")\n done = True\n result[\"success\"] = 0\n if (not train):\n result[\"agent_states\"].append(self.__recent_agent_states)\n self.__sleep(0.1)\n\n # Info that we don't want to capture during training\n if not train:\n result[\"num_stat_obj\"] = 0\n result[\"num_peds\"] = 0\n\n #Counting number of static objects and number of dynamic objects (pedestrians)\n for topic in self.__flatland_topics:\n if topic.find(\"stat_obj\") != -1:\n result[\"num_stat_obj\"] +=1\n continue\n if topic.find(\"person\") != -1:\n result[\"num_peds\"] +=1\n\n driven_route.poses = poses\n result[\"time\"] = self.__clock - start_time\n result[\"driven_path\"] = driven_route\n result[\"timestep\"] = self.__timestep\n return result\n\n def __sleep(self, secs):\n \"\"\"\n Sleep method, that takes into account the namespace we are in.\n As we don't run the python3.5-scripts as rosnodes, manual redirecting necessary.\n :return result of the episode.\n\n \"\"\"\n if not self.MODE == 2:\n now = self.__clock\n while (self.__clock - now).to_sec() < secs:\n time.sleep(0.0001)\n else:\n # time.sleep(secs)\n rospy.sleep(secs)\n\n def evaluate_training(self, save_path):\n \"\"\"\n Evaluates an agent during training. Results are saved in /evaluation_data/train\n :param agent_name: name of the agent (.pkl)\n \"\"\"\n\n while True:\n self.__timestep -= 1\n\n #Waiting for new task\n while not self.__new_task_started:\n time.sleep(0.001)\n self.__done = False\n self.__new_task_started = False\n result = self.evaluate_episode(True)\n with open('%s.pickle' % (save_path), 'ab') as handle:\n pickle.dump(result, handle, protocol=pickle.HIGHEST_PROTOCOL)\n\n\n def show_paths(self, result):\n \"\"\"\n Publishs the driven and global path of the result\n :param result\n \"\"\"\n self.__driven_path_pub.publish(result[\"driven_path\"])\n self.__global_path_pub.publish(result[\"global_path\"])\n\n def generate_qualitative_static_image_rviz(self, results, results_comp, i_task, i_pos):\n \"\"\"\n Generates a qualitative image of the result of an evaluation set. Furthermore a second agent\n can be plotted to compare both in the image (generated in rviz)\n :param evaluation_set_name: name of the used evaluation set\n :param results: results of first agent\n :param results_comp: results of second agent\n :param i_task: index of episode in evaluation set that should be displayed\n :param i_pos: position where robot should be displayed.\n \"\"\"\n if len(results) != len(self.__evaluation_set):\n print(\"Error: results and evaluations_set need to have the same length. They don't fit.\")\n return\n self.__global_path_pub.publish(results[i_task][\"global_path\"])\n self.__driven_path_pub.publish(results[i_task][\"driven_path\"])\n if len(results) == len(results_comp):\n self.__driven_path_pub2.publish(results_comp[i_task][\"driven_path\"])\n\n x_robot = results[i_task][\"driven_path\"].poses[i_pos].pose.position.y\n y_robot = results[i_task][\"driven_path\"].poses[i_pos].pose.position.x\n theta = math.atan2(results[i_task][\"driven_path\"].poses[i_pos + 4].pose.position.y - x_robot,\n results[i_task][\"driven_path\"].poses[i_pos + 4].pose.position.x - y_robot)\n self.__task_generator.set_robot_pos(y_robot, x_robot, theta)\n self.__task_generator.remove_all_static_objects()\n task = self.__evaluation_set[i_task]\n\n if 'static_objects' in task.keys():\n for i in range(len(task[\"static_objects\"][\"x\"])):\n self.__task_generator.spawn_object(task[\"static_objects\"][\"model_name\"][i], i, task[\"static_objects\"][\"x\"][i],\n task[\"static_objects\"][\"y\"][i], task[\"static_objects\"][\"theta\"][i])\n\n # Making visible by taking a sim_step\n self.__task_generator.take_sim_step()\n\n def generate_qualitative_ped_image_rviz(self, results, i_task, i_pos):\n \"\"\"\n Generates a dynamic qualitative image of the result of a pedestrian evaluation set.\n The state of the pedestrians and the robot is plotted at position i_pos.\n :param evaluation_set_name: name of the used evaluation set\n :param results: results of first agent\n :param results_comp: results of second agent\n :param i_task: index of episode in evaluation set that should be displayed\n :param i_pos: position where robot should be displayed.\n \"\"\"\n\n if (i_pos >= len(results[i_task][\"success\"])-4):\n i_pos = len(results[i_task][\"success\"])-5\n self.generate_qualitative_static_image_rviz(results, [], i_task, i_pos)\n agent_states = results[i_task][\"agent_states\"][i_pos]\n marker_array = MarkerArray()\n for i, agent_state in enumerate(agent_states):\n marker_array.markers.append(Marker())\n marker_array.markers[i*3] = Marker()\n marker_array.markers[i*3].header.frame_id = \"/map\"\n marker_array.markers[i*3].id = i*3\n marker_array.markers[i*3].action = 0\n marker_array.markers[i*3].type = 0\n marker_array.markers[i*3].scale.x = 0.07\n marker_array.markers[i*3].scale.y = 0.2\n marker_array.markers[i*3].scale.z = 0.3\n marker_array.markers[i*3].color.a = 0.5\n marker_array.markers[i*3].color.r = 170/255\n marker_array.markers[i*3].color.g = 0.0\n marker_array.markers[i*3].color.b = 0.0\n\n start_point = agent_state.pose.position\n marker_array.markers[i*3].points.append(start_point)\n end_point = Point()\n fac = 3\n end_point.x = agent_state.pose.position.x + agent_state.twist.linear.x*fac\n end_point.y = agent_state.pose.position.y + agent_state.twist.linear.y*fac\n marker_array.markers[i*3].points.append(end_point)\n\n marker_array.markers.append(Marker())\n marker_array.markers[i*3+1].header.frame_id = \"/map\"\n marker_array.markers[i*3+1].id = i*3+1\n marker_array.markers[i*3+1].action = 0\n marker_array.markers[i*3+1].type = 2\n marker_array.markers[i*3+1].pose = agent_state.pose\n marker_array.markers[i*3+1].scale.x = 0.32\n marker_array.markers[i*3+1].scale.y = 0.32\n marker_array.markers[i*3+1].scale.z = 0.1\n marker_array.markers[i*3+1].color.a = 1.0\n marker_array.markers[i*3+1].color.r = 170/255\n marker_array.markers[i*3+1].color.g = 0.0\n marker_array.markers[i*3+1].color.b = 0.0\n\n lookahead = 100\n marker_array.markers.append(Marker())\n marker_array.markers[i*3+2].header.frame_id = \"/map\"\n marker_array.markers[i*3+2].id = i*3+2\n marker_array.markers[i*3+2].action = 0\n marker_array.markers[i*3+2].type = 4\n marker_array.markers[i*3+2].scale.x = 0.015\n marker_array.markers[i*3+2].color.a = 1.0\n marker_array.markers[i*3+2].color.r = 170/255\n marker_array.markers[i*3+2].color.g = 0.0\n marker_array.markers[i*3+2].color.b = 0.0\n for agent_states_lookahead in results[i_task][\"agent_states\"][i_pos:min(len(results[i_task][\"agent_states\"]), i_pos + lookahead)]:\n marker_array.markers[i*3+2].points.append(agent_states_lookahead[i].pose.position)\n # Publish Marker\n self.__agent_pub.publish(marker_array)\n\n def __rect_robot_collision(self, static_scan_msg, ped_scan_msg, robot_width, robot_height):\n \"\"\"\n Checks if the robot footprint robot_width x robot_height collided\n :param static_scan_msg: scan of the static objects\n :param ped_scan_msg: scan of the pedestrians\n :param robot_width: width of the robots footprint\n :param robot_height: height of the robots footprint\n \"\"\"\n robot_collision = False\n angle_min = ped_scan_msg.angle_min\n angle_increment = ped_scan_msg.angle_increment\n for i in range(len(ped_scan_msg.ranges)):\n angle = angle_min + i * angle_increment\n length = min(ped_scan_msg.ranges[i], static_scan_msg.ranges[i])\n x = math.cos(angle) * length\n y = math.sin(angle) * length\n if(x > -robot_height/2 and x < robot_height/2 and y > -robot_width/2 and y < robot_width/2):\n robot_collision = True\n\n return robot_collision\n\n def __odom_callback(self, data):\n self.__odom = data\n\n def __path_callback(self, data):\n self.__path = data\n\n def __done_callback(self, data):\n self.__done = data.data\n\n def __new_task_callback(self, data):\n self.__new_task_started = data.data\n\n def __flatland_topic_callback(self, data):\n self.__flatland_topics = data.topics\n\n def __trigger_callback(self, data):\n self.__timestep += 1\n\n def __clock_callback(self, data):\n self.__clock = data.clock\n\n def __ped_callback(self, data):\n self.__recent_agent_states = data.agent_states\n\n" }, { "path": "rl_agent/src/rl_agent/evaluation/__init__.py", "content": "" }, { "path": "rl_agent/src/tf_python.cpp", "content": "/**\n * \n * Ronja Gueldenring\n * This class is needed, because rospy and python3.5 is not compatible regarding \n * the tf-package.\n * The class provides all transformation needed by rl_agent in python\n * \n**/\n#include \n\n\nnamespace rl_agent {\n\n\tTFPython::TFPython(const ros::NodeHandle& node_handle): nh_(node_handle){\n goal_sub_ = nh_.subscribe(\"move_base_simple/goal\", 1, &TFPython::goal_callback, this);\n transformed_goal_pub_ = nh_.advertise(\"rl_agent/robot_to_goal\", 1, false);\n }\n\n\t/**\n\t * \n\t * @brief Publishes transform from robot to goal for rl-agent\n * \n\t **/ \n void TFPython::robot_to_goal_transform(){\n if ( goal_.header.frame_id == \"\"){\n return;\n }\n tf::StampedTransform rob_to_map;\n\t\ttry{\n\t\tlistener_.lookupTransform(\"base_footprint\", goal_.header.frame_id, \n\t\t\t\t\t\t\t\tros::Time(0), rob_to_map);\n\t\t}\n\t\tcatch (tf::TransformException ex){\n\t\t\tROS_ERROR(\"%s\",ex.what());\n\t\t\tros::Duration(1.0).sleep();\n }\n\n tf::Transform map_to_goal;\n tf::poseMsgToTF(goal_.pose, map_to_goal);\n\n tf::Transform rob_to_goal;\n rob_to_goal = rob_to_map * map_to_goal;\n\n geometry_msgs::PoseStamped msg;\n msg.header.stamp = ros::Time::now();\n msg.header.frame_id = \"base_footprint\";\n tf::poseTFToMsg(rob_to_goal, msg.pose);\n transformed_goal_pub_.publish(msg);\n return;\n }\n\n\n void TFPython::goal_callback(const geometry_msgs::PoseStamped& goal){\n goal_ = goal;\n }\n\n\n}; // namespace rl_agent\n\nint main(int argc, char** argv){\n ros::init(argc, argv, \"tf_python\");\n ros::NodeHandle node;\n rl_agent::TFPython tf(node);\n ros::WallRate r(25); \n while (ros::ok()) {\n tf.robot_to_goal_transform();\n ros::spinOnce();\n r.sleep();\n }\n return 0;\n};" }, { "path": "rl_bringup/CMakeLists.txt", "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(rl_bringup)\n\nset(PACKAGE_DEPS\n roscpp\n rospy\n std_msgs\n nav_msgs\n tf\n flatland_msgs\n)\n\nfind_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPS})\n\ncatkin_package(\n CATKIN_DEPENDS ${PACKAGE_DEPS}\n)\n\ninstall(DIRECTORY config/\n DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config/\n)\n\n###########\n## Build ##\n###########\n\ninclude_directories(${catkin_INCLUDE_DIRS})\nlink_directories(${catkin_LIBRARY_DIRS})\n\nset(TOGGLE_SETUP_INIT toggle_setup_init)\nadd_executable(${TOGGLE_SETUP_INIT} src/toggle_setup_init.cpp)\nadd_dependencies(${TOGGLE_SETUP_INIT} ${catkin_EXPORTED_TARGETS})\ntarget_link_libraries(${TOGGLE_SETUP_INIT} ${catkin_LIBRARIES})\n" }, { "path": "rl_bringup/config/costmap_common_params.yaml", "content": "obstacle_range: 3.0\nraytrace_range: 6.0\nrobot_radius: 0.46\ninflation_radius: 0.6\nstatic_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}\nback_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}\nfront_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}\nobservation_sources: front_scan back_scan static_laser\n\n\n" }, { "path": "rl_bringup/config/dwa_params.yaml", "content": "DWAPlannerROS:\n max_trans_vel: 0.8 \n min_trans_vel: 0.05 \n\n max_vel_x: 0.8 \n min_vel_x: 0.05 \n\n max_vel_y: 0 \n min_vel_y: 0 \n\n max_rot_vel: 1.2 \n min_rot_vel: 0.0 \n\n acc_lim_theta: 1000.0 # 3.2 \n acc_lim_x: 1000.0 # 2.5\n acc_lim_y: 0 # 2.5\n acc_lim_trans: 1000.0 # \n\n\n rot_stopped_vel: 0.01 \n trans_stopped_vel: 0.01\n\n# Goal Tolerance Parameters\n yaw_goal_tolerance: 0.5 # 0.05\n xy_goal_tolerance: 0.5 # 0.10\n\n\n# Forward Simulation Parameters\n sim_time: 1.7 \n vx_samples: 5 \n vy_samples: 0 \n vtheta_samples: 20 \n\n \n\n" }, { "path": "rl_bringup/config/global_costmap_params.yaml", "content": "global_costmap:\n global_frame: /map\n robot_base_frame: base_footprint\n update_frequency: 5\n static_map: true\n publish_frequency: 0.0\n" }, { "path": "rl_bringup/config/local_costmap_params.yaml", "content": "local_costmap:\n global_frame: odom\n robot_base_frame: base_footprint\n update_frequency: 5.0\n publish_frequency: 2.0\n static_map: false\n rolling_window: true\n width: 6.0\n height: 6.0\n resolution: 0.05" }, { "path": "rl_bringup/config/movebase_params.yaml", "content": "\nbase_local_planner: rl_local_planner/RLLocalPlanner #dwa_local_planner/DWAPlannerROS \nbase_global_planner: navfn/NavfnROS\nupdate_frequency: 10.0\n" }, { "path": "rl_bringup/config/path_config.ini", "content": "[PATHES]\npath_to_venv=/home/ronja/venv_p3\npath_to_train_data=/home/ronja/database\npath_to_eval_data_train=/home/ronja/database/evaluation_data/train\npath_to_eval_data_test=/home/ronja/database/evaluation_data/test\npath_to_eval_sets=/home/ronja/database/evaluation_data/evaluation_sets\npath_to_catkin_ws=/home/ronja/catkin_ws\npath_to_tensorboard_log=/home/ronja/database/tensorboard_log_ppo_10\npath_to_models=/home/ronja/database/agents\nros_version=kinetic" }, { "path": "rl_bringup/config/path_config_docker.ini", "content": "[PATHES]\npath_to_venv=/venv_p3\npath_to_train_data=/data\npath_to_eval_data_train=/data/evaluation_data/train\npath_to_eval_data_test=/data/evaluation_data/test\npath_to_eval_sets=/data/evaluation_data/evaluation_sets\npath_to_catkin_ws=/usr/catkin_ws\npath_to_tensorboard_log=/data/tensorboard_log_ppo_12\npath_to_models=/data/agents\nros_version=kinetic" }, { "path": "rl_bringup/config/rl_common.yaml", "content": "rl_agent:\n f_scan_topic: f_scan\n b_scan_topic: b_scan\n robot_frame: base_footprint\n \n train_mode: 1 # 0: Execution in Simulation, 1: Training, 2: Execution in Real World\n\n update_frequency: 10\n goal_theshold: 0.6\n num_of_wps: 8\n look_ahead_distance: 1.5\n wp_reached_dist: 0.2\n\n\n ped_type: 11 # 0: Pedestrians don't avoid robot\n # 10: Pedestrians always avoid robot\n # 11: Pedestrians avoid robot if it stands still and after reaction t\n\n " }, { "path": "rl_bringup/config/rl_params_img.yaml", "content": "rl_agent:\n state_mode: 0 # 0: image, 1: laserscan\n \n img_width_pos: 70\n img_width_neg: 20 \n img_height: 70 \n resolution: 0.15\n " }, { "path": "rl_bringup/config/rl_params_scan.yaml", "content": "rl_agent:\n state_mode: 1 # 0: image, 1: laserscan\n \n resolution: 0.05\n scan_size: 90 \n\n\n \n" }, { "path": "rl_bringup/launch/rviz.launch", "content": "\n\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n" }, { "path": "rl_bringup/launch/setup.launch", "content": "\n\n \n \n \n\n \n \n \n\t\n\n \n\n \n \n \n \n \n \n \n \n\n \n \n \n \n \n \n\n \n \n \n \n \t\n\n \n \n \n \t\n\n \n \n \n \t\n\n \n \n \n \t\n\n \n \n \n \n \n\n" }, { "path": "rl_bringup/launch/sub_launch/dummy_localization.launch", "content": "\n\t \n\t\n\t\n\t\t\n\t\t\n\t\n\n" }, { "path": "rl_bringup/launch/sub_launch/flatland.launch", "content": "\n\t \n \n \n\n\n \n \n\n \n \n \n \n\n \n \n \n \n \n\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n\n \n \n \n \n \n\n" }, { "path": "rl_bringup/launch/sub_launch/move_base.launch", "content": "\n\t\n\n \n \n \n \n \n \n \n \n \n \n\n\n" }, { "path": "rl_bringup/launch/sub_launch/navigation.launch", "content": "\n\n\t \n\t \n\n\t\n \n\t\t\n\t\t\n\t\t\n \n\n\t\n\t\n\t\t\n\t\n \t\n\t\n\t\n\t\t\n\t\n\n" }, { "path": "rl_bringup/launch/sub_launch/pedestrians_only.launch", "content": "\n \n\t \n \n \n \n \n \n \n \n \n \n \n \n \n\n \n \n \n \n \n \n \n\t\t\n \n\n\n \n \n \n \n\n" }, { "path": "rl_bringup/launch/sub_launch/pedsim_visualizer.launch", "content": "\n\t \n\n \n \n\t\t\n \n \n" }, { "path": "rl_bringup/launch/sub_launch/simulation.launch", "content": "\n\t\n \n \n\n \n\n \n \n \n \n \n \n\n \n \n \n \n \n" }, { "path": "rl_bringup/package.xml", "content": "\n\n rl_bringup\n 0.0.0\n The rl_bringup package\n\n \n \n \n ronja\n\n\n \n \n \n TODO\n\n\n \n \n \n \n\n\n \n \n \n \n\n\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n catkin\n roscpp\n rospy\n std_msgs\n nav_msgs\n tf\n flatland_msgs\n roscpp\n rospy\n std_msgs\n nav_msgs\n tf\n flatland_msgs\n roscpp\n rospy\n std_msgs\n nav_msgs\n tf\n flatland_msgs\n\n\n\n \n \n \n\n \n\n" }, { "path": "rl_bringup/rviz/robot_navigation.rviz", "content": "Panels:\n - Class: rviz/Displays\n Help Height: 0\n Name: Displays\n Property Tree Widget:\n Expanded:\n - /TF1/Tree1\n - /TrackedPersons1/History as line1\n - /static obstacles1/sta_obj_21\n - /static obstacles1/stat_obj_51\n - /static obstacles1/stat_obj_91\n - /static obstacles1/stat_obj_81\n - /static obstacles1/stat_obj_71\n - /static obstacles1/stat_obj_61\n - /static obstacles1/stat_obj_41\n - /static obstacles1/stat_obj_31\n - /static obstacles1/stat_obj_11\n Splitter Ratio: 0.690048933\n Tree Height: 644\n - Class: rviz/Selection\n Name: Selection\n - Class: rviz/Tool Properties\n Expanded:\n - /2D Pose Estimate1\n - /2D Nav Goal1\n Name: Tool Properties\n Splitter Ratio: 0.588679016\n - Class: rviz/Views\n Expanded:\n - /Current View1\n Name: Views\n Splitter Ratio: 0.5\n - Class: rviz/Time\n Experimental: false\n Name: Time\n SyncMode: 0\n SyncSource: static_laser\n - Class: rviz_plugin/Teleop\n Name: Teleop\n - Class: rviz_plugin/Teleop\n Name: Teleop\nToolbars:\n toolButtonStyle: 2\nVisualization Manager:\n Class: \"\"\n Displays:\n - Alpha: 0.5\n Cell Size: 1\n Class: rviz/Grid\n Color: 160; 160; 164\n Enabled: false\n Line Style:\n Line Width: 0.0299999993\n Value: Lines\n Name: Grid\n Normal Cell Count: 0\n Offset:\n X: 0\n Y: 0\n Z: 0\n Plane: XY\n Plane Cell Count: 10\n Reference Frame: \n Value: false\n - Class: rviz/TF\n Enabled: true\n Frame Timeout: 15\n Frames:\n All Enabled: false\n base_footprint:\n Value: true\n map:\n Value: true\n odom:\n Value: true\n ped_laser_link:\n Value: true\n static_laser_link:\n Value: true\n Marker Scale: 1\n Name: TF\n Show Arrows: false\n Show Axes: true\n Show Names: false\n Tree:\n map:\n odom:\n base_footprint:\n ped_laser_link:\n {}\n static_laser_link:\n {}\n Update Interval: 0\n Value: true\n - Class: rviz/Group\n Displays:\n - Class: rviz/MarkerArray\n Enabled: false\n Marker Topic: /flatland_server/debug/layer/2d\n Name: World\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n - Class: rviz/Marker\n Enabled: false\n Marker Topic: /pedsim_visualizer/walls\n Name: pedsim_walls\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n - Alpha: 0.899999976\n Class: rviz/Map\n Color Scheme: map\n Draw Behind: true\n Enabled: true\n Name: Map\n Topic: /map\n Unreliable: false\n Use Timestamp: false\n Value: true\n Enabled: true\n Name: world\n - Class: rviz/Group\n Displays:\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/LaserScan\n Color: 85; 85; 255\n Color Transformer: FlatColor\n Decay Time: 0\n Enabled: true\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 4096\n Min Color: 0; 0; 0\n Min Intensity: 0\n Name: static_laser\n Position Transformer: XYZ\n Queue Size: 1\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.100000001\n Style: Flat Squares\n Topic: /static_laser\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: true\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/LaserScan\n Color: 255; 170; 255\n Color Transformer: FlatColor\n Decay Time: 0\n Enabled: true\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 4096\n Min Color: 0; 0; 0\n Min Intensity: 0\n Name: ped_laser\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.100000001\n Style: Flat Squares\n Topic: /ped_laser\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: true\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/LaserScan\n Color: 85; 85; 255\n Color Transformer: FlatColor\n Decay Time: 0\n Enabled: true\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 4096\n Min Color: 0; 0; 0\n Min Intensity: 0\n Name: f_scan\n Position Transformer: XYZ\n Queue Size: 1\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.100000001\n Style: Flat Squares\n Topic: /f_scan\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: true\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/LaserScan\n Color: 85; 85; 255\n Color Transformer: FlatColor\n Decay Time: 0\n Enabled: true\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 4096\n Min Color: 0; 0; 0\n Min Intensity: 0\n Name: b_scan\n Position Transformer: XYZ\n Queue Size: 1\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.100000001\n Style: Flat Squares\n Topic: /b_scan\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: true\n Enabled: true\n Name: LaserScanner\n - Alpha: 1\n Class: spencer_tracking_rviz_plugin/TrackedPersons\n Color: 170; 0; 0\n Color map offset: 0\n Color transform: Constant color\n Delete after no. cycles: 20\n Enabled: true\n Excluded person IDs: \"\"\n Font color: 85; 85; 127\n Font color style: Constant color\n Font scale: 2\n History as line:\n Line width: 0.00999999978\n Value: true\n History size: 50\n Included person IDs: \"\"\n Min. history point distance: 0.400000006\n Missed alpha: 0.5\n Name: TrackedPersons\n Occlusion alpha: 0.300000012\n Render covariances:\n Line width: 0.100000001\n Value: true\n Render detection IDs: false\n Render history: true\n Render person visual: false\n Render track IDs: false\n Render track state: false\n Render velocities: true\n Show DELETED tracks: false\n Show MATCHED tracks: true\n Show MISSED tracks: true\n Show OCCLUDED tracks: true\n Style:\n Line width: 0.0500000007\n Scaling factor: 1\n Value: Simple\n Topic: /pedsim_visualizer/tracked_persons\n Unreliable: false\n Value: true\n Z offset:\n Use Z position from message: false\n Value: 0\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 0; 0; 255\n Enabled: true\n Head Diameter: 0.300000012\n Head Length: 0.200000003\n Length: 0.300000012\n Line Style: Billboards\n Line Width: 0.0599999987\n Name: global_path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.0299999993\n Shaft Diameter: 0.100000001\n Shaft Length: 0.100000001\n Topic: /move_base/NavfnROS/plan\n Unreliable: false\n Value: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /flatland_server/debug/model/robot_1\n Name: robot1\n Namespaces:\n \"\": true\n Queue Size: 100\n Value: true\n - Class: rviz/Group\n Displays:\n - Alpha: 1\n Class: rviz/PointStamped\n Color: 85; 85; 255\n Enabled: true\n History Length: 1\n Name: PointStamped\n Radius: 0.200000003\n Topic: /wp_vis1\n Unreliable: false\n Value: true\n - Alpha: 1\n Class: rviz/PointStamped\n Color: 85; 85; 255\n Enabled: true\n History Length: 1\n Name: PointStamped\n Radius: 0.200000003\n Topic: /wp_vis2\n Unreliable: false\n Value: true\n - Alpha: 1\n Class: rviz/PointStamped\n Color: 85; 85; 255\n Enabled: true\n History Length: 1\n Name: PointStamped\n Radius: 0.200000003\n Topic: /wp_vis3\n Unreliable: false\n Value: true\n - Alpha: 1\n Class: rviz/PointStamped\n Color: 85; 85; 255\n Enabled: true\n History Length: 1\n Name: PointStamped\n Radius: 0.200000003\n Topic: /wp_vis4\n Unreliable: false\n Value: true\n Enabled: true\n Name: waypoints\n - Class: rviz/Group\n Displays:\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: flatland_server/debug/model/stat_obj_0\n Name: stat_obj_0\n Namespaces:\n {}\n Queue Size: 1\n Value: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /flatland_server/debug/model/stat_obj_2\n Name: sta_obj_2\n Namespaces:\n {}\n Queue Size: 1\n Value: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /flatland_server/debug/model/stat_obj_5\n Name: stat_obj_5\n Namespaces:\n {}\n Queue Size: 1\n Value: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /flatland_server/debug/model/stat_obj_9\n Name: stat_obj_9\n Namespaces:\n {}\n Queue Size: 1\n Value: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /flatland_server/debug/model/stat_obj_8\n Name: stat_obj_8\n Namespaces:\n {}\n Queue Size: 1\n Value: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /flatland_server/debug/model/stat_obj_7\n Name: stat_obj_7\n Namespaces:\n {}\n Queue Size: 1\n Value: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /flatland_server/debug/model/stat_obj_6\n Name: stat_obj_6\n Namespaces:\n {}\n Queue Size: 1\n Value: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /flatland_server/debug/model/stat_obj_4\n Name: stat_obj_4\n Namespaces:\n {}\n Queue Size: 1\n Value: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /flatland_server/debug/model/stat_obj_3\n Name: stat_obj_3\n Namespaces:\n {}\n Queue Size: 1\n Value: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /flatland_server/debug/model/stat_obj_1\n Name: stat_obj_1\n Namespaces:\n {}\n Queue Size: 1\n Value: true\n Enabled: true\n Name: static obstacles\n - Class: rviz/Group\n Displays:\n - Alpha: 0.699999988\n Class: rviz/Map\n Color Scheme: map\n Draw Behind: false\n Enabled: true\n Name: Map\n Topic: /rl_map\n Unreliable: false\n Use Timestamp: false\n Value: true\n Enabled: true\n Name: input_state\n - Class: rviz/Group\n Displays:\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /flatland_server/debug/model/person_8\n Name: person8\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /flatland_server/debug/model/person_7\n Name: person7\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /flatland_server/debug/model/person_5\n Name: person5\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /flatland_server/debug/model/person_4\n Name: person4\n Namespaces:\n \"\": true\n Queue Size: 100\n Value: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /flatland_server/debug/model/person_3\n Name: person3\n Namespaces:\n \"\": true\n Queue Size: 100\n Value: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /flatland_server/debug/model/person_6\n Name: person6\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /flatland_server/debug/model/person_2\n Name: person2\n Namespaces:\n \"\": true\n Queue Size: 100\n Value: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /flatland_server/debug/model/person_0\n Name: person0\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /flatland_server/debug/model/person_1\n Name: person1\n Namespaces:\n \"\": true\n Queue Size: 100\n Value: true\n - Class: rviz/MarkerArray\n Enabled: true\n Marker Topic: /flatland_server/debug/model/person_9\n Name: person9\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n Enabled: true\n Name: persons\n Enabled: true\n Global Options:\n Background Color: 255; 255; 255\n Default Light: true\n Fixed Frame: map\n Frame Rate: 30\n Name: root\n Tools:\n - Class: rviz/MoveCamera\n - Class: rviz/Interact\n Hide Inactive Objects: true\n - Class: rviz/Select\n - Class: rviz/SetInitialPose\n Topic: /initialpose\n - Class: rviz/SetGoal\n Topic: /move_base_simple/goal\n - Class: rviz/Measure\n Value: true\n Views:\n Current:\n Angle: 0.0750005692\n Class: rviz/TopDownOrtho\n Enable Stereo Rendering:\n Stereo Eye Separation: 0.0599999987\n Stereo Focal Distance: 1\n Swap Stereo Eyes: false\n Value: false\n Invert Z Axis: false\n Name: Current View\n Near Clip Distance: 0.00999999978\n Scale: 35.4415321\n Target Frame: \n Value: TopDownOrtho (rviz)\n X: 15.649415\n Y: 13.8903675\n Saved: ~\nWindow Geometry:\n Displays:\n collapsed: false\n Height: 1029\n Hide Left Dock: false\n Hide Right Dock: true\n QMainWindow State: 000000ff00000000fd0000000400000000000002a2000003aafc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006f00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000039500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000041000002c5000000f300fffffffb0000000c00540065006c0065006f0070020000012c000003ef0000026a00000049fb0000000a0049006d006100670065000000021b000001370000000000000000fb0000000a0049006d00610067006500000002fa0000006a0000000000000000fb0000000c00540065006c0065006f0070010000030c000000df0000008100fffffffb0000000a0049006d0061006700650100000310000000c600000000000000000000000100000170000004d5fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000069000004d5000000bb00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000027600fffffffb0000000800540069006d00650100000000000004500000000000000000000004d4000003aa00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000\n Selection:\n collapsed: false\n Teleop:\n collapsed: false\n Time:\n collapsed: false\n Tool Properties:\n collapsed: false\n Views:\n collapsed: true\n Width: 1916\n X: 0\n Y: 49\n" }, { "path": "rl_bringup/src/toggle_setup_init.cpp", "content": " /*\n * @name\t \t toggle_setup_init.cpp\n * @brief\t \t Simulation will be triggered for n_sec for initialize setup.\n * @author \tRonja Gueldenring\n * @date \t\t 2019/04/05\n **/\n#include \n#include \n\nint main(int argc, char** argv){\n \n ros::init(argc, argv, \"map_echo\");\n ros::NodeHandle node;\n\n std::string train_mode_topic = ros::this_node::getNamespace() + \"/rl_agent/train_mode\";\n int rl_mode;\n node.getParam(train_mode_topic, rl_mode);\n\n bool keep_clock_running = false;\n if(rl_mode == 2){\n keep_clock_running = true;\n }\n \n float n_sec = 10.0;\n ros::ServiceClient step_simulation_ = node.serviceClient(\"step\");\n flatland_msgs::Step msg;\n msg.request.step_time.data = 0.1;\n ros::WallTime begin = ros::WallTime::now();\n ros::WallRate r(30);\n while ((ros::WallTime::now() - begin).toSec() < n_sec || keep_clock_running) {\n if(!step_simulation_.call(msg)){\n ROS_ERROR(\"Failed to call step_simulation_ service\");\n }\n r.sleep();\n }\n\n return 0;\n};\n" }, { "path": "rl_local_planner/CMakeLists.txt", "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(rl_local_planner)\n\nfind_package(catkin REQUIRED COMPONENTS\n roscpp\n rospy\n std_msgs\n base_local_planner\n tf\n sensor_msgs\n nav_msgs\n nav_core\n laser_geometry\n geometry_msgs\n message_generation\n geometry_msgs\n)\n\nfind_package(PCL REQUIRED)\nfind_package(Eigen3 REQUIRED)\nfind_package(Boost REQUIRED COMPONENTS system)\n\n###################################\n## catkin specific configuration ##\n###################################\n\ncatkin_package(\n INCLUDE_DIRS\n include\n ${EIGEN3_INCLUDE_DIRS}\n LIBRARIES\n rlLocalPlanner\n CATKIN_DEPENDS\n base_local_planner\n costmap_2d\n nav_msgs\n nav_core\n roscpp\n std_msgs\n tf\n laser_geometry\n geometry_msgs\n message_runtime\n nav_msgs\n DEPENDS\n EIGEN3\n)\n\n\n\n###########\n## Build ##\n###########\n\ninclude_directories(\n include\n include/${PROJECT_NAME}\n ${catkin_INCLUDE_DIRS}\n ${EIGEN3_INCLUDE_DIRS}\n)\n\nadd_definitions(${EIGEN3_DEFINITIONS})\n\nadd_library(rlLocalPlanner \t\n src/rl_local_planner.cpp\n)\n\nadd_dependencies(rlLocalPlanner\n ${${PROJECT_NAME}_EXPORTED_TARGETS}\n ${catkin_EXPORTED_TARGETS}\n)\n\ntarget_link_libraries(rlLocalPlanner\n ${catkin_LIBRARIES}\n ${boost_LIBRARIES}\n)\n\nadd_executable(wp_generator\n src/wp_generator.cpp\n)\n\nadd_dependencies(wp_generator\n ${${PROJECT_NAME}_EXPORTED_TARGETS}\n ${catkin_EXPORTED_TARGETS}\n)\n\ntarget_link_libraries(wp_generator\n ${catkin_LIBRARIES}\n)\n\nadd_executable(image_generator\n src/image_generator.cpp\n)\n\nadd_dependencies(image_generator\n ${${PROJECT_NAME}_EXPORTED_TARGETS}\n ${catkin_EXPORTED_TARGETS}\n)\n\ntarget_link_libraries(image_generator\n ${catkin_LIBRARIES}\n)\n\n\n\n#############\n## Install ##\n#############\n\ninstall(\n TARGETS \n rlLocalPlanner\n ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}\n LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}\n)\n\ninstall(\n FILES\n blp_plugin.xml\n DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}\n)\n\ninstall(DIRECTORY include/${PROJECT_NAME}/\n DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}\n)\n\ninstall(\n TARGETS\n wp_generator\n image_generator\n DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}\n)\n" }, { "path": "rl_local_planner/blp_plugin.xml", "content": "\n \n \n An implementation of a local planner communicating with an rl-agent.\n \n \n\n\n" }, { "path": "rl_local_planner/include/rl_local_planner/image_generator.h", "content": "/*\n * @name\t \t image_generator.h\n * @brief\t \t An image is generated from laserscan data and waypoints on the path\n * @author \tRonja Gueldenring\n * @date \t \t2019/04/05\n **/\n\n#ifndef IMAGE_GENERATOR_H\n#define IMAGE_GENERATOR_H\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n\n\n\nnamespace rl_image_generator {\n /**\n * This class generates an image from laserscan data and waypoints on the path.\n */\n class ImageGenerator {\n public:\n /**\n * @brief Constructor f\n */\n ImageGenerator(const ros::NodeHandle& node_handle);\n virtual ~ImageGenerator() = default;\n protected:\n ros::NodeHandle nh_;\n private:\n int img_width_pos_; //number of height cells in positive direction\n int img_width_neg_; //number of height cells in negative direction\n int img_height_; //number of height cells in negative direction\n float resolution_; //resolution in m/cell\n tf::TransformListener tf_; \n std::string robot_frame_; \n\n /**\n * set_path_service + path_callback_\n * @brief Service, that generates image from most recent scan and waypoint.\n */\n ros::ServiceServer get_image_service_;\n bool get_img_callback_(rl_msgs::StateImageGenerationSrv::Request& request, rl_msgs::StateImageGenerationSrv::Response& response);\n\n /**\n * @brief Image is generated by adding obstacles retrieved \n * from the laser scans and adding a line from waypoint\n * to waypoint.\n * @param scan laser scan that has to be added to the image.\n * @wp waypoint vector that has to be added to the image.\n **/\n nav_msgs::OccupancyGrid generate_image(sensor_msgs::LaserScan& scan, rl_msgs::Waypoint& wp);\n /**\n * @brief The index of the image for point (x, y) in [m] is computed.\n * @param x x-position\n * @param y y-position\n * @return image index\n */\n int point_to_index_(double x, double y);\n\n /**\n * @brief Adding scan and occlusions to the image\n * @param image Image, where to add the scan\n * @param scan Scan, that should be added to the image.\n */\n void add_scan_to_img_(std::vector& image, sensor_msgs::LaserScan& scan);\n\n /**\n * @brief Adding goal point as square\n * @param image Image, where to add the square\n * @param x x-position of goal.\n * @param y y-position of goal.\n */\n void add_goal_point(std::vector& image, float x_goal, float y_goal);\n\n /**\n * @brief Adding Line to the image (Bresenham)\n * @param image Image, where to add the line\n * @param x1 x-position of first line point.\n * @param y1 y-position of first line point.\n * @param x2 x-position of second line point.\n * @param y2 y-position of second line point.\n */\n void add_line_to_img_(std::vector& image, float x1, float y1, float x2, float y2, int value);\n\n /**\n * @brief Mean square distance.\n * @param x x-position\n * @param y x-position\n * @retur sqrt(x^2 + y^2)\n */\n double metric_dist(double x, double y);\n\n float get_res();\n int get_img_height();\n int get_img_pos_width();\n int get_img_neg_width();\n\n\n };\n};\n#endif /* IMAGE_GENERATOR_H */\n" }, { "path": "rl_local_planner/include/rl_local_planner/rl_local_planner.h", "content": "/*\n * @name\t \t rl_local_planner.h\n * @brief\t \t Connector of move_base and rl_agent using RL library. Forwards action of rl_agent\n * \t\t\t\t to move_base each time step.\n * @author \tRonja Gueldenring\n * @date \t \t2019/04/05\n **/\n\n\n#ifndef RL_LOCAL_PLANNER_H\n#define RL_LOCAL_PLANNER_H\n#include \n#include \n#include \n#include \n#include \n#include \n\n\nnamespace rl_local_planner {\n\n /**\n * This class serves as connector between move\\_base and the rl_agent, that uses \n * a RL-library. The rl_agent provides an action each timestep that is forwarded\n * to move_base over this class.\n */\n class RLLocalPlanner : public nav_core::BaseLocalPlanner {\n public:\n /**\n * @brief Constructor for RLLocalPlanner wrapper\n */\n RLLocalPlanner();\n\n \n virtual ~RLLocalPlanner() = default;\n\n\n /**\n * @brief Constructs the ros wrapper\n * @param name The name to give this instance of the trajectory planner\n * @param tf A pointer to a transform listener\n * @param costmap The cost map to use for assigning costs to trajectories\n */\n void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros);\n\n /**\n * @brief Check if the goal pose has been achieved\n * @return True if achieved, false otherwise\n */\n bool isGoalReached();\n\n /**\n * @brief Set the plan that the controller is following\n * @param orig_global_plan The plan to pass to the controller\n * @return True if the plan was updated successfully, false otherwise\n */\n bool setPlan(const std::vector& orig_plan);\n\n /**\n * @brief Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base\n * @param cmd_vel Will be filled with the velocity command to be passed to the robot base\n * @return >0 if a valid velocity command was found, otherwise a result <0, contains the fail_code\n */\n bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);\n protected:\n ros::NodeHandle nh_;\n private:\n // Class variables\n tf::TransformListener* tf_;\n tf::Vector3 original_goal_; // goal of global plan\n geometry_msgs::Twist action_; // most recent action\n std::string path_frame_; // name of path frame \n std::string robot_frame_; // name of robot frame\n double goal_threshold_; // threshold, when goal is reached\n bool is_action_new_; // True, if new action is available\n int rl_mode_; // mode of rl_agent\n bool done_; // True, if the rl_agent is done\n\n // Services\n ros::ServiceClient rl_agent_;\n ros::ServiceClient set_path_service_;\n \n // Publisher\n ros::Publisher trigger_agent_pub;\n\n // Subscriber\n ros::Subscriber agent_action_sub_;\n /**\n * @brief cmd_vel of rl-agent is published here.\n * @param cmd_vel velocity command\n */\n void agent_action_callback_(const geometry_msgs::Twist& cmd_vel);\n ros::Subscriber done_sub_;\n /**\n * @brief If episode is finished done=True is published here.\n * @param done True, if episode is done.\n */\n void done_callback_(const std_msgs::Bool& done);\n\n // functions\n /**\n * @brief Mean square distance.\n * @param x x-position\n * @param y x-position\n * @retur sqrt(x^2 + y^2)\n */\n double metric_dist(double x, double y);\n };\n};\n#endif /* RL_LOCAL_PLANNER_H */\n" }, { "path": "rl_local_planner/include/rl_local_planner/wp_generator.h", "content": "/*\n * @name\t \twp_generator.h\n * @brief\t \tGeneration of waypoints on the global path and \n * \t\t\t\t \tdetermination of the closest waypoints to the robot.\n * @author \tRonja Gueldenring\n * @date \t\t2019/04/05\n **/\n\n#ifndef WP_GENERATOR_H\n#define WP_GENERATOR_H\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n\nnamespace rl_local_planner {\n /**\n * This class downsamples the global path to waypoints and \n * determines the next closest waypoints to the robot.\n */\n class WpGenerator {\n public:\n /**\n * @brief Constructor for WpGenerator wrapper\n */\n WpGenerator(const ros::NodeHandle& node_handle);\n\n \n virtual ~WpGenerator() = default;\n\n /**\n * @brief Determines the next closest waypoints to the robot\n */\n void get_closest_wps();\n protected:\n ros::NodeHandle nh_;\n private:\n std::vector waypoints_; //List of all remaining waypoints on path\n double look_ahead_distance_; //distance between waypoints\n double wp_reached_dist_; //distance, when threshold is reaches\n int num_of_wps_; //number of closest waypoints, that are published\n std::string path_frame_; //frame name of path\n std::string robot_frame_; //frame name of the robot\n tf::Vector3 goal_; //most recent goal\n\n ros::Publisher wp_pub_; //Publisher for next closest waypoints \n ros::Publisher wp_reached_pub_; //Publisher for reaching waypoint within a wp_reached_dist_\n tf::TransformListener tf_;\n\n /**\n * @brief global plan is downsampled to a number of waypoints with distance look_ahead_distance_.\n * @param global_plan The most recent global plan.\n */\n void precalculate_waypoints_(std::vector global_plan);\n \n /**\n * @brief Mean square distance.\n * @param x x-position\n * @param y x-position\n * @retur sqrt(x^2 + y^2)\n */\n double metric_dist(double x, double y);\n tf::Vector3 get_transformed_wp_(geometry_msgs::PoseStamped wp, tf::StampedTransform transform);\n\n /**\n * set_path_service + path_callback_\n * @brief If new path available, it can be set here.\n * Wp generator only considers most recent path in further computations.\n */\n ros::ServiceServer set_path_service;\n\t bool path_callback_(rl_msgs::SetPath::Request& request, rl_msgs::SetPath::Response& response);\n \n\n };\n};\n#endif /* WP_GENERATOR_H */\n" }, { "path": "rl_local_planner/package.xml", "content": "\n\n rl_local_planner\n 0.0.0\n The rl_local_planner package\n\n \n \n \n ronja\n\n\n \n \n \n TODO\n\n\n catkin\n roscpp\n rospy\n std_msgs\n tf\n sensor_msgs\n nav_msgs\n base_local_planner\n costmap_2d\n nav_core\n laser_geometry\n geometry_msgs\n message_generation\n rl_msgs\n roscpp\n rospy\n std_msgs\n tf\n sensor_msgs\n nav_msgs\n base_local_planner\n costmap_2d\n nav_core\n laser_geometry\n geometry_msgs\n roscpp\n rospy\n std_msgs\n tf\n sensor_msgs\n nav_msgs\n base_local_planner\n costmap_2d\n nav_core\n laser_geometry\n geometry_msgs\n message_runtime\n\n\n\n \n \n \n \n \n\n" }, { "path": "rl_local_planner/src/image_generator.cpp", "content": "/*\n * @name\t \timage_generator.cpp\n * @brief\t \tAn image is generated from laserscan data and waypoints on the path\n * @author \tRonja Gueldenring\n * @date \t\t2019/04/05\n **/\n#include \n\n\nnamespace rl_image_generator {\n\n\tImageGenerator::ImageGenerator(const ros::NodeHandle& node_handle)\n\t\t\t\t\t\t\t: nh_{node_handle}{\n\n // getting params from param server\n nh_.getParam(\"rl_agent/img_width_pos\", img_width_pos_);\n nh_.getParam(\"rl_agent/img_width_neg\", img_width_neg_);\n nh_.getParam(\"rl_agent/img_height\", img_height_);\n nh_.getParam(\"rl_agent/resolution\", resolution_);\n nh_.getParam(\"rl_agent/robot_frame\", robot_frame_);\n\n //Services\n std::string img_service_name_ = ros::this_node::getName() + \"/get_image\";\n get_image_service_ = nh_.advertiseService(img_service_name_, &ImageGenerator::get_img_callback_, this);\n\t}\n\n bool ImageGenerator::get_img_callback_(rl_msgs::StateImageGenerationSrv::Request& request, rl_msgs::StateImageGenerationSrv::Response& response){\n nav_msgs::OccupancyGrid img = generate_image(request.scan, request.wps);\n response.img = img;\n return true;\n }//get_img_callback_\n\n nav_msgs::OccupancyGrid ImageGenerator::generate_image(sensor_msgs::LaserScan& scan, rl_msgs::Waypoint& wp){\n //Initializing image\n nav_msgs::OccupancyGrid img;\n img.header.stamp = ros::Time::now();\n img.header.frame_id = robot_frame_;\n img.info.resolution = resolution_;\n img.info.width = (img_width_pos_ + img_width_neg_);\n img.info.height = img_height_;\n img.info.origin.position.x = -img_width_neg_*resolution_;\n img.info.origin.position.y = -img_height_*resolution_/2.0;\n std::vector image;\n image.resize((img_height_*(img_width_pos_ + img_width_neg_)), 50);\n img.header.stamp = scan.header.stamp;\n\n //Adding laserscan infromation to the image\n add_scan_to_img_(image, scan);\n\n //Adding path infromation to the image\n if (wp.points.size() > 0){\n if(img.header.stamp > wp.header.stamp){\n img.header.stamp = wp.header.stamp;\n }\n geometry_msgs::Point zero_point;\n zero_point.x = 0.0;\n zero_point.y = 0.0;\n add_line_to_img_(image, zero_point.x, zero_point.y, wp.points[0].x, wp.points[0].y, 100);\n int num_wps = wp.points.size();\n for(int i = 1; i < num_wps; i++){\n add_line_to_img_(image, wp.points[i-1].x, wp.points[i-1].y, wp.points[i].x, wp.points[i].y, 100);\n }\n }\n img.data = image;\n return img; \n }//generate_image\n\n void ImageGenerator::add_goal_point(std::vector& image, float x_goal, float y_goal){\n float size = 0.5;\n for (int iy = 0; iy < (int)(size/resolution_); iy++){\n for (int ix = 0; ix < (int)(size/resolution_); ix++){\n float y = y_goal - size/2 + iy*resolution_;\n float x = x_goal - size/2 + ix*resolution_;\n int index = point_to_index_(x,y);\n if(index >= 0 && index < (img_height_*(img_width_pos_ + img_width_neg_))){\n image[index] = 100;\n }\n }\n }\n\n }//add_goal_point\n\n void ImageGenerator::add_scan_to_img_(std::vector& image, sensor_msgs::LaserScan& scan){\n if(scan.ranges.size() == 0){\n return;\n }\n tf::StampedTransform scan_transform_;\n try{\n tf_.lookupTransform(robot_frame_, scan.header.frame_id, \n ros::Time(0), scan_transform_);\n\n }\n catch (tf::TransformException ex){\n ROS_ERROR(\"%s\",ex.what());\n ros::WallDuration(1.0).sleep();\n }\n\n double max_dist = sqrt(pow(((img_width_pos_ + img_width_neg_)*resolution_),2) + pow((img_height_*resolution_/2.0),2));\n for (int i=0; i < scan.ranges.size(); i+=1){\n // std::vector angles = {scan.angle_min + i * scan.angle_increment - 0.333*scan.angle_increment,\n // scan.angle_min + i * scan.angle_increment - 0.0*scan.angle_increment,\n // scan.angle_min + i * scan.angle_increment - 0.333*scan.angle_increment\n // };\n std::vector angles;\n angles.push_back(scan.angle_min + i * scan.angle_increment - 0.4444*scan.angle_increment);\n angles.push_back(scan.angle_min + i * scan.angle_increment - 0.3333*scan.angle_increment);\n angles.push_back(scan.angle_min + i * scan.angle_increment - 0.2222*scan.angle_increment);\n angles.push_back(scan.angle_min + i * scan.angle_increment - 0.1111*scan.angle_increment);\n angles.push_back(scan.angle_min + i * scan.angle_increment - 0.0*scan.angle_increment);\n angles.push_back(scan.angle_min + i * scan.angle_increment + 0.1111*scan.angle_increment);\n angles.push_back(scan.angle_min + i * scan.angle_increment + 0.2222*scan.angle_increment);\n angles.push_back(scan.angle_min + i * scan.angle_increment + 0.3333*scan.angle_increment);\n angles.push_back(scan.angle_min + i * scan.angle_increment + 0.4444*scan.angle_increment);\n for (int k=0; k < angles.size(); k++){\n double angle = angles[k]; //scan.angle_min + i * scan.angle_increment;\n double length;\n double length2;\n if (isnan(scan.ranges[i]) || scan.ranges[i] == 0.0){\n continue;\n }else{\n length = scan.ranges[i];\n length2 = max_dist;\n }\n\n // Transform laserpoint to robot frame\n tf::Vector3 laser_point(cos(angle)*length, sin(angle)*length, 0.);\n tf::Vector3 laser_point_transformed = scan_transform_* laser_point;\n float x1 = laser_point_transformed.getX();\n float y1 = laser_point_transformed.getY();\n\n tf::Vector3 laser_point2(cos(angle)*length2, sin(angle)*length2, 0.);\n tf::Vector3 laser_point2_transformed = scan_transform_* laser_point2;\n float x2 = laser_point2_transformed.getX();\n float y2 = laser_point2_transformed.getY();\n add_line_to_img_(image, x1, y1, x2, y2, 0);\n }\n }\n return;\n }//add_scan_to_img\n\n void ImageGenerator::add_line_to_img_(std::vector& image, float x1, float y1, float x2, float y2, int value){\n const bool steep = (fabs(y2 - y1) > fabs(x2 - x1));\n if(steep)\n {\n std::swap(x1, y1);\n std::swap(x2, y2);\n }\n \n if(x1 > x2)\n {\n std::swap(x1, x2);\n std::swap(y1, y2);\n }\n \n const float dx = x2 - x1;\n const float dy = fabs(y2 - y1);\n \n float error = dx / 2.0f;\n const float ystep = (y1 < y2) ? resolution_*0.7 : -resolution_*0.7;\n float y = y1;\n \n for(float x=x1; x= 0 && index < (img_height_*(img_width_pos_ + img_width_neg_))){\n image[index] = value;\n }\n \n error -= dy;\n if(error < 0)\n {\n y += ystep;\n error += dx;\n }\n }\n return;\n }//add_line_to_img_\n\n int ImageGenerator::point_to_index_(double x, double y){\n int y_index = (int) ceil((y + (((float)img_height_* resolution_))/2.0)/resolution_);\n int x_index = (int) ceil((x + (((float)img_width_neg_* resolution_)))/resolution_);\n\n int index = (y_index) * (img_width_pos_ + img_width_neg_) + (x_index);\n if (x_index < 0 || x_index >= (img_width_pos_ + img_width_neg_) || y_index < 0 || y_index > img_height_){\n index = (img_width_pos_ + img_width_neg_) * img_height_ + 1;\n }\n return index;\n }//point_to_index_\n\n\tdouble ImageGenerator::metric_dist(double x, double y){\n\t\tdouble dist = sqrt(pow(x , 2) + pow(y , 2));\n\t\treturn dist; \n\t} //metric_dist\n\n int ImageGenerator::get_img_neg_width(){\n return img_width_neg_;\n }\n\n int ImageGenerator::get_img_pos_width(){\n return img_width_pos_;\n }\n\n int ImageGenerator::get_img_height(){\n return img_height_;\n }\n\n float ImageGenerator::get_res(){\n return resolution_;\n }\n\n}; // namespace rl_image_generator\n\n\nint main(int argc, char** argv){\n ros::init(argc, argv, \"image_generator\");\n ros::NodeHandle node;\n rl_image_generator::ImageGenerator ig(node);\n ros::WallRate r(100);\n while (ros::ok()) {\n ros::spinOnce();\n r.sleep();\n }\n return 0;\n};" }, { "path": "rl_local_planner/src/rl_local_planner.cpp", "content": "/*\n * @name\t \trl_local_planner.cpp\n * @brief\t \tConnector of move_base and rl_agent using RL library. Forwards action of rl_agent\n * \t\t\t\tto move_base each time step.\n * @author \tRonja Gueldenring\n * @date \t \t2019/04/05\n **/\n#include \n#include \n\n\nPLUGINLIB_EXPORT_CLASS(rl_local_planner::RLLocalPlanner, nav_core::BaseLocalPlanner)\nnamespace rl_local_planner {\n\n\tRLLocalPlanner::RLLocalPlanner(): nh_(){\n\n\t}//RLLocalPlanner\n\n\tvoid RLLocalPlanner::initialize(std::string name, tf::TransformListener* tf,\n\t\tcostmap_2d::Costmap2DROS* costmap_ros){\n\n\t\t// params\n\t\ttf_ = tf;\n\n\t\t// getting params from param server\n\t\tnh_.getParam(\"rl_agent/robot_frame\", robot_frame_);\n\t\tnh_.getParam(\"rl_agent/train_mode\", rl_mode_);\n\t\tnh_.getParam(\"rl_agent/goal_theshold\", goal_threshold_);\n\t\t\n\t\t// initializing class variables\n\t\tif (rl_mode_ == 1)\n\t\t\tgoal_threshold_ = 0.0;\n\n\t\tis_action_new_ = false;\n\t\tdone_ = false;\n\n\t\t// Subscriber\n\t\tagent_action_sub_ = nh_.subscribe(\"rl_agent/action\", 1, &RLLocalPlanner::agent_action_callback_, this);\n\t\tdone_sub_ = nh_.subscribe(\"rl_agent/done\", 1, &RLLocalPlanner::done_callback_, this);\n\t\t\n\t\t//Publisher\n\t\ttrigger_agent_pub = nh_.advertise(\"trigger_agent\", 1, false);\n\t\t\n\t\t// Services\n\t\tstd::string set_path_service_name = ros::this_node::getNamespace() + \"/wp_generator/set_gobal_plan\";\n\t\tset_path_service_ = nh_.serviceClient(set_path_service_name);\n\t} //initialize\n\n \tbool RLLocalPlanner::isGoalReached(){\n\n\t\ttf::StampedTransform transform;\n\t\ttry{\n\t\ttf_->lookupTransform(robot_frame_, path_frame_, \n\t\t\t\t\t\t\t\tros::Time(0), transform);\n\t\t}\n\t\tcatch (tf::TransformException ex){\n\t\t\tROS_ERROR(\"%s\",ex.what());\n\t\t\tros::Duration(1.0).sleep();\n\t\t}\n\n\t\ttf::Vector3 original_goal_transformed = transform * original_goal_;\n\n\t\tif(metric_dist(original_goal_transformed.getX(), original_goal_transformed.getY()) < goal_threshold_)\n\t\t\treturn true;\n\n\t\tif(done_){\n\t\t\tdone_ = false;\n\t\t\treturn true;\n\t\t}\n\t\t\n\t\treturn false;\n\n \t} //isGoalReached\n\n \tbool RLLocalPlanner::setPlan(const std::vector& orig_plan)\n \t{\n\t\t// goal_threshold_ = goal_threshold;\n\t\tif ( orig_plan.size() < 1 ){\n\t\t\tROS_ERROR_STREAM(\"RLLocalPlanner: Got an empty plan\");\n\t\t\treturn false;\n\t\t}\n\t\tgeometry_msgs::PoseStamped original_goal = orig_plan.back();\n\t\toriginal_goal_ = tf::Vector3(original_goal.pose.position.x,\n\t\t\t\t\t\t\toriginal_goal.pose.position.y,\n\t\t\t\t\t\t\t0.);\n\t\tpath_frame_ = original_goal.header.frame_id;\n\t\tdone_ = false;\n\t\trl_msgs::SetPath srv;\n\t\tsrv.request.path.header.stamp = ros::Time::now();\n\t\tsrv.request.path.header.frame_id = path_frame_;\n\t\tsrv.request.path.poses = orig_plan;\n\t\tif (!set_path_service_.call(srv)){\n\t\t\tROS_ERROR(\"Failed set path on waypoint generator.\");\n \t\t\treturn false;\n\t\t}\n\t\treturn true;\n \t} //setPlan\n\n\tbool RLLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel){\n\t\t// ROS_WARN(\"Velocity command\");\n\t\t// Trigger agent to compute next action\n\t\tstd_msgs::Bool msg;\n\t\tmsg.data = true;\n\t\ttrigger_agent_pub.publish(msg);\n\n\t\t//Waiting for agents action\n\t\tros::WallRate r(1000);\n\t\twhile(!is_action_new_ && !done_){\n\t\t\tr.sleep();\n\t\t}\n\n\t\tis_action_new_ = false;\n\n\t\t//assigning new action\n\t\tif(done_){\n\t\t\taction_.linear.x = 0.0;\n\t\t\taction_.angular.z = 0.0;\n\t\t}\n\t\tcmd_vel = action_;\n\t\treturn true;\n\n\t}//computeVelocityCommands\n\t\n\n\tdouble RLLocalPlanner::metric_dist(double x, double y){\n\t\tdouble dist = sqrt(pow(x , 2) + pow(y , 2));\n\t\treturn dist; \n\t}//metric_dist\n\n\tvoid RLLocalPlanner::agent_action_callback_(const geometry_msgs::Twist& cmd_vel){\n\t\taction_ = cmd_vel;\n\t\tis_action_new_ = true;\n\t}//agent_action_callback_\n\n\tvoid RLLocalPlanner::done_callback_(const std_msgs::Bool& done){\n\t\tdone_ = done.data; \n\t}//done_callback_\n\n}; // namespace rl_local_planner\n" }, { "path": "rl_local_planner/src/wp_generator.cpp", "content": "/*\n * @name\t \twp_generator.cpp\n * @brief\t \tGeneration of waypoints on the global path and \n * \t\t\t\tdetermination of the closest waypoints to the robot.\n * @author \t\tRonja Gueldenring\n * @date \t\t2019/04/05\n **/\n#include \n\n\nnamespace rl_local_planner {\n\n\tWpGenerator::WpGenerator(const ros::NodeHandle& node_handle)\n\t\t\t\t\t\t\t: nh_{node_handle}{\n \n\t\t// Get params of parameter server\n\t\tnh_.getParam(\"rl_agent/look_ahead_distance\", look_ahead_distance_);\n\t\tnh_.getParam(\"rl_agent/wp_reached_dist\", wp_reached_dist_);\n\t\tnh_.getParam(\"rl_agent/num_of_wps\", num_of_wps_);\n\t\tnh_.getParam(\"rl_agent/robot_frame\", robot_frame_);\n\n\t\t// Services\n\t\tstd::string global_plan_service_name = ros::this_node::getName() + \"/set_gobal_plan\";\n\t\tset_path_service = nh_.advertiseService(global_plan_service_name, &WpGenerator::path_callback_, this);\n \n\t\t//Publisher\n\t\twp_pub_ = nh_.advertise(\"wp\", 1, true);\n\t\twp_reached_pub_ = nh_.advertise(\"wp_reached\", 1, true);\n\t}\n\n\tbool WpGenerator::path_callback_(rl_msgs::SetPath::Request& request, rl_msgs::SetPath::Response& response){\n\t\tif(request.path.poses.size() == 0){\n\t\t\t\treturn false;\n\t\t}\n\t\tpath_frame_ = request.path.header.frame_id;\n geometry_msgs::PoseStamped goal_temp = request.path.poses.back();\n\t\tgoal_ = tf::Vector3(goal_temp.pose.position.x,\n\t\t\t\t\t\t\tgoal_temp.pose.position.y,\n\t\t\t\t\t\t\t0.);\n precalculate_waypoints_(request.path.poses);\n get_closest_wps();\n return true;\n }\n\n\n\tvoid WpGenerator::precalculate_waypoints_(std::vector global_plan){\n\t\twaypoints_.clear();\n\t\tgeometry_msgs::PoseStamped prev = global_plan[0];\n\t\tdouble dist = 0.0;\n\t\tfor(int i = 1; i < global_plan.size(); i++){\n\t\t\tdist += metric_dist((prev.pose.position.x - global_plan[i].pose.position.x),\n\t\t\t\t\t\t\t(prev.pose.position.y - global_plan[i].pose.position.y));\n\t\t\tif(dist > look_ahead_distance_){\n\t\t\t\tdist = 0.0;\n\t\t\t\twaypoints_.push_back(global_plan[i]);\n\t\t\t}\n\t\t\tprev = global_plan[i];\n\t\t}\n\t\tfor(int i = 0; i < num_of_wps_ + 1; i++){\n \twaypoints_.push_back(global_plan.back());\n\t\t}\n\t\treturn;\n\t}\n\n\tvoid WpGenerator::get_closest_wps(){\n\t\tif(waypoints_.size() < num_of_wps_){\n return;\n }\n\n\t\t//Prepare Waypoint-message that will be published\n\t\trl_msgs::Waypoint msg;\n\t\tmsg.header.frame_id = robot_frame_;\n\t\tmsg.header.stamp = ros::Time::now();\n msg.is_new.data = false;\n\n\t\t// Get Transform from path_frame_ to robot_frame\n\t\ttf::StampedTransform transform;\n\t\ttry{\n\t\t tf_.lookupTransform(robot_frame_, path_frame_, \n\t\t\t\t\t\t\t\tros::Time(0), transform);\n\n\t\t}\n\t\tcatch (tf::TransformException ex){\n\t\t\tROS_ERROR(\"%s\",ex.what());\n\t\t\tros::WallDuration(1.0).sleep();\n\t\t}\n\n\t\t//Find closest point on path\n\t\ttf::Vector3 closest_wp = get_transformed_wp_(waypoints_[0], transform);\n\t\tdouble dist = metric_dist(closest_wp.getX(), closest_wp.getY());\n\t\tfor(int i = 1; i < waypoints_.size(); i++){\n\t\t\ttf::Vector3 temp_wp = get_transformed_wp_(waypoints_[i], transform);\n\t\t\tdouble temp_dist = metric_dist(temp_wp.getX(), temp_wp.getY());\n\t\t\tif (temp_dist < dist){\n\t\t\t\tdist = temp_dist;\n\t\t\t\tclosest_wp = temp_wp;\n\t\t\t}else{\n\t\t\t\t//Closest point on path found!\n\t\t\t\tint start_index = i - 1;\n\n\t\t\t\t//Is closest wp reached in a certain radius?\n\t\t\t\tif (dist < wp_reached_dist_){\n\t\t\t\t\tmsg.is_new.data = true;\n\t\t\t\t\tfor (int k = 0; k < i; k++){\n\t\t\t\t\t\twaypoints_.erase(waypoints_.begin());\n\t\t\t\t\t}\n\t\t\t\t\tstart_index = 0;\t\n\t\t\t\t}\n\n\t\t\t\t// Is closest waypoint already overtaken? Then take next one.\n\t\t\t\ttf::Vector3 second_wp = get_transformed_wp_(waypoints_[start_index + 1], transform);\n\t\t\t\tdouble dist_second_wp = metric_dist(temp_wp.getX(), temp_wp.getY());\n\t\t\t\tif (dist + look_ahead_distance_ > dist_second_wp){\n\t\t\t\t\tstart_index +=1;\n\t\t\t\t}\n\n\t\t\t\t//Getting all sequencing waypoints.\n\t\t\t\tstd::vector points;\n\t\t\t\tfor(int j = start_index; j < start_index + num_of_wps_; j++){\n\t\t\t\t\tgeometry_msgs::Point p;\n\t\t\t\t\tif (j < waypoints_.size()){\n\t\t\t\t\t\ttf::Vector3 wp = get_transformed_wp_(waypoints_[j], transform);\n\t\t\t\t\t\tp.x = wp.getX();\n\t\t\t\t\t\tp.y = wp.getY();\n\t\t\t\t\t\tpoints.push_back(p);\n\t\t\t\t\t}\n\t\t\t\t}\n\t\t\t\tmsg.points = points;\n\t\t\t\tif (msg.is_new.data == true){\n\t\t\t\t\twp_reached_pub_.publish(msg);\n\t\t\t\t}else{\n\t\t\t\t\twp_pub_.publish(msg);\n\t\t\t\t}\n\t\t\t\t\n\t\t\t\tbreak;\n\t\t\t}\n\t\t}\n\t\treturn;\t\t\n\t}\n\n\ttf::Vector3 WpGenerator::get_transformed_wp_(geometry_msgs::PoseStamped wp, tf::StampedTransform transform){\n\t\ttf::Vector3 wp_vec(wp.pose.position.x,\n\t\t\t\twp.pose.position.y,\n\t\t\t\t0.);\n\t\ttf::Vector3 wp_vec_transformed = transform*wp_vec;\n\t\treturn wp_vec_transformed;\n\t}\n\n double WpGenerator::metric_dist(double x, double y){\n\t\tdouble dist = sqrt(pow(x , 2) + pow(y , 2));\n\t\treturn dist; \n\t}//metric_dist\n\n}; // namespace rl_local_planner\n\nint main(int argc, char** argv){\n ros::init(argc, argv, \"wp_generator\");\n ros::NodeHandle node;\n int rl_mode;\n std::string train_mode_topic = ros::this_node::getNamespace() + \"/rl_agent/train_mode\";\n node.param(train_mode_topic, rl_mode, 1);\n rl_local_planner::WpGenerator wg(node);\n\tros::WallRate r(100);\n\twhile (ros::ok()) {\n\t\t\twg.get_closest_wps();\n\t\t\tros::spinOnce();\n\t\t\tr.sleep();\n\t}\n\treturn 0;\n \n};" }, { "path": "rl_msgs/CMakeLists.txt", "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(rl_msgs)\n\n# Ensure we're using c++11\nset(CMAKE_CXX_FLAGS \"${CMAKE_CXX_FLAGS} -std=c++11\")\n\n## Find catkin macros and libraries\n## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)\n## is used, also find other catkin packages\nfind_package(catkin REQUIRED COMPONENTS\n std_msgs\n geometry_msgs\n nav_msgs\n sensor_msgs\n message_generation\n)\n\n## System dependencies are found with CMake's conventions\nfind_package(PkgConfig REQUIRED)\n\n################\n## Build MSGS ##\n################\nadd_message_files(FILES\n Waypoint.msg\n)\n\nadd_service_files(FILES\n MergeScans.srv\n SetPath.srv\n StateImageGenerationSrv.srv\n)\n\ngenerate_messages(\n DEPENDENCIES\n std_msgs\n geometry_msgs\n nav_msgs \n sensor_msgs\n rl_msgs\n)\n\n###################################\n## catkin specific configuration ##\n###################################\ncatkin_package(\n INCLUDE_DIRS\n CATKIN_DEPENDS \n std_msgs \n geometry_msgs \n nav_msgs \n sensor_msgs \n message_runtime\n)\n\n###########\n## Build ##\n###########\n\ninclude_directories(\n ${catkin_INCLUDE_DIRS}\n)\n" }, { "path": "rl_msgs/msg/Waypoint.msg", "content": "std_msgs/Header header\ngeometry_msgs/Point[] points\nstd_msgs/Bool is_new" }, { "path": "rl_msgs/package.xml", "content": "\n\n rl_msgs\n 1.1.1\n The rl_msgs package\n BSD\n ronja\n\n Ronja Gueldenring\n\n catkin\n\n cmake_modules\n message_generation\n\n std_msgs\n geometry_msgs\n sensor_msgs\n nav_msgs\n\n message_runtime\n\n \n \n \n\n" }, { "path": "rl_msgs/srv/MergeScans.srv", "content": "sensor_msgs/LaserScan[] scans\n---\nsensor_msgs/LaserScan merged_scan\n" }, { "path": "rl_msgs/srv/SetPath.srv", "content": "nav_msgs/Path path\n" }, { "path": "rl_msgs/srv/StateImageGenerationSrv.srv", "content": "sensor_msgs/LaserScan scan\nrl_msgs/Waypoint wps\n\n---\nnav_msgs/OccupancyGrid img\n" }, { "path": "start_scripts/entrypoint_ppo2.sh", "content": "#! /bin/bash\n#get config-params\npath_to_venv=$(awk -F \"=\" '/path_to_venv/ {print $2}' ../rl_bringup/config/path_config.ini)\npath_to_catkin_ws=$(awk -F \"=\" '/path_to_catkin_ws/ {print $2}' ../rl_bringup/config/path_config.ini)\nros_ver=$(awk -F \"=\" '/ros_version/ {print $2}' ../rl_bringup/config/path_config.ini)\n\n# source ros stuff\nsource /opt/ros/$ros_ver/setup.bash\nsource $path_to_catkin_ws/devel/setup.bash\nsource $path_to_venv/bin/activate\n\nagent_id=$1\nnum_sims=$2\n\nINPUT=./training_params/ppo2_params.csv\nOLDIFS=$IFS\nIFS=,\n[ ! -f $INPUT ] && { echo \"$INPUT file not found\"; }\ninit=false\n\n# Checking if roscore is running. If not roscore is started\nif ! rostopic list ; \nthen\n screen -dmS roscore bash -c \"source ./start_roscore.sh\"\n screen -S roscore -X logfile screenlog_roscore.log\n screen -S roscore -X log\n sleep 4\nfi\n\n# Loading training maps\nMAPS=./training_params/training_maps.csv\nmaps=()\nwhile read map\ndo\n if [ -z \"$map\" ];\n then\n break\n fi\n maps+=($map)\ndone < $MAPS\n\nwhile 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\ndo\n if [ \"$init\" = false ] ;\n then\n init=true\n continue;\n fi\n\n if [ \"$agent_name\" = \"$agent_id\" ] || [ \"$agent_id\" = \"all\" ];\n then\n echo \"$agent_name\"\n echo \"$policy\"\n\n # Starting num_sims simulations with different maps.\n i_map=0\n for ((i=1;i<=$num_sims;i++));\n do\n echo \"${maps[i_map]}\"\n screen -dmS ros_sim$i bash -c \"source ./ros.sh sim$i $policy ${maps[i_map]}\"\n screen -S ros_sim$i -X logfile screenlog_\"$agent_name\"_ros_sim$i.log\n screen -S ros_sim$i -X log\n i_map=$((i_map+1))\n if [ \"$i_map\" -eq \"${#maps[@]}\" ] ;\n then\n i_map=0\n fi\n sleep 5\n done\n\n # Starting Reinforcement Learning training (PPO2)\n 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\"\n screen -S python -X logfile screenlog_\"$agent_name\"_py.log\n screen -S python -X log\n\n # Wait until training is done\n while (screen -list | grep -q \"python\");\n do\n echo \"sleep\"\n sleep 5\n done\n\n # Old training setup is closed, so that new one can be started.\n for ((i=1;i<=$num_sims;i++));\n do\n screen -X -S ros_sim$i quit\n done\n fi\ndone < $INPUT\n#bash\n" }, { "path": "start_scripts/ppo2_training.sh", "content": "#! /bin/sh\n\n#get config-params\npath_to_venv=$(awk -F \"=\" '/path_to_venv/ {print $2}' ../rl_bringup/config/path_config.ini)\npath_to_catkin_ws=$(awk -F \"=\" '/path_to_catkin_ws/ {print $2}' ../rl_bringup/config/path_config.ini)\nros_ver=$(awk -F \"=\" '/ros_version/ {print $2}' ../rl_bringup/config/path_config.ini)\n\n# source ros stuff\nsource /opt/ros/$ros_ver/setup.bash\nsource $path_to_catkin_ws/devel/setup.bash\nsource $path_to_venv/bin/activate\n\n# get params\nagent_name=$1\ntotal_timesteps=$2\npolicy=$3\ngamma=$4\nn_steps=$5\nent_coef=$6\nlearning_rate=$7\nvf_coef=$8\nmax_grad_norm=$9\nlam=${10}\nnminibatches=${11}\nnoptepochs=${12}\ncliprange=${13}\nrobot_radius=${14}\nrew_fnc=${15}\nnum_stacks=${16}\nstack_offset=${17}\ndisc_action_space=${18}\nnormalize=${19}\nstage=${20}\npretrained_model_path=${21}\ntask_mode=${22}\nnum_sim=${23}\n\n# start rl_agent\npython ../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\n\ndeactivate\nIFS=$OLDIFS\n\n\n\n" }, { "path": "start_scripts/ros.sh", "content": "#! /bin/sh\nsim_name=$1\npolicy=$2\nmap=$3\n\n#get config-params\npath_to_catkin_ws=$(awk -F \"=\" '/path_to_catkin_ws/ {print $2}' ../rl_bringup/config/path_config.ini)\nros_ver=$(awk -F \"=\" '/ros_version/ {print $2}' ../rl_bringup/config/path_config.ini)\n\n# source ros stuff\nsource /opt/ros/$ros_ver/setup.bash\nsource $path_to_catkin_ws/devel/setup.bash\n\nmap_path=$path_to_catkin_ws/src/drl_local_planner_ros_stable_baselines/flatland_setup/maps/$map\necho \"map_path\"\necho \"$map_path\"\necho \"$sim_name\"\n\nif [ \"$policy\" = \"CnnPolicy_multi_input_vel\" ] || [ \"$policy\" = \"CnnPolicy_multi_input_vel2\" ] || [ \"$policy\" = \"CnnPolicy\" ];\nthen\n echo \"$policy\"\n roslaunch rl_bringup setup.launch ns:=\"$sim_name\" rl_params:=\"rl_params_img\" map_path:=\"$map_path\"\nfi\n\nif [ \"$policy\" = \"CNN1DPolicy\" ] || [ \"$policy\" = \"CNN1DPolicy_multi_input\" ];\nthen\n echo \"$policy\"\n roslaunch rl_bringup setup.launch ns:=\"$sim_name\" rl_params:=\"rl_params_scan\" map_path:=\"$map_path\"\nfi\n\n" }, { "path": "start_scripts/start_roscore.sh", "content": "#! /bin/sh\n\n#get config-params\npath_to_catkin_ws=$(awk -F \"=\" '/path_to_catkin_ws/ {print $2}' ../rl_bringup/config/path_config.ini)\nros_ver=$(awk -F \"=\" '/ros_version/ {print $2}' ../rl_bringup/config/path_config.ini)\n\n# source ros stuff\nsource /opt/ros/$ros_ver/setup.bash\nsource $path_to_catkin_ws/devel/setup.bash\n\nroscore" }, { "path": "start_scripts/training_params/ppo2_params.csv", "content": "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\nppo2_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\nppo2_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\nppo2_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\nppo2_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\nppo2_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\nppo2_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\nppo2_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\nppo2_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\nppo2_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\n\n\n\n" }, { "path": "start_scripts/training_params/training_maps.csv", "content": "open_field_1\ncorridor_1\ncorridor_3\ncomplex_map_1\n\n" } ]