├── .gitignore ├── README.md ├── dependencies.rosinstall ├── external ├── asl_pepper │ ├── asl_pepper_2d_simulator │ │ ├── CMakeLists.txt │ │ ├── launch │ │ │ ├── pepper_2d_simulator.launch │ │ │ └── static_pepper_upload.launch │ │ ├── meshes │ │ │ └── pepper_frozen_standing.stl │ │ ├── package.xml │ │ ├── python │ │ │ ├── gymified_pepper_envs.py │ │ │ ├── pepper_2d_iarlenv.py │ │ │ ├── pepper_2d_simulator.py │ │ │ └── setup.py │ │ ├── scripts │ │ │ └── pepper_2d_simulator │ │ └── urdf │ │ │ └── pepper_frozen_standing.urdf │ ├── asl_pepper_gmapping │ │ ├── CMakeLists.txt │ │ ├── launch │ │ │ ├── gmapping.launch │ │ │ └── save_map.launch │ │ ├── maps │ │ │ ├── office_full.pgm │ │ │ └── office_full.yaml │ │ └── package.xml │ ├── asl_pepper_motion_planning │ │ ├── CMakeLists.txt │ │ ├── launch │ │ │ ├── amcl.launch │ │ │ ├── global_planner.launch │ │ │ ├── motion_planner.launch │ │ │ ├── rvo_planner.launch │ │ │ ├── rvo_planner_sim.launch │ │ │ └── test_rl.launch │ │ ├── package.xml │ │ ├── scripts │ │ │ ├── PPO.py │ │ │ ├── VAE.py │ │ │ ├── big_empty.yaml │ │ │ ├── branch_and_bound.py │ │ │ ├── cnn_policy.py │ │ │ ├── cuda.py │ │ │ ├── debug_virtual_pepper.ipynb │ │ │ ├── empty.pgm │ │ │ ├── empty.yaml │ │ │ ├── global_planner │ │ │ ├── image_loading_performance.py │ │ │ ├── leonhard_train_PPO.sh │ │ │ ├── motion_planner │ │ │ ├── motion_planner.py │ │ │ ├── office_full.pgm │ │ │ ├── office_full.yaml │ │ │ ├── path_smoothing.py │ │ │ ├── pposgd_simple.py │ │ │ ├── publish_sim_map.sh │ │ │ ├── rooms.pgm │ │ │ ├── rooms.yaml │ │ │ ├── run_rl.py │ │ │ ├── run_sb_RVO_load.py │ │ │ ├── run_sb_RVO_train.py │ │ │ ├── run_sb_load.py │ │ │ ├── run_sb_render_RVO.py │ │ │ ├── run_sb_train.py │ │ │ ├── rvo_planner │ │ │ ├── smallMlp.py │ │ │ ├── test_rl │ │ │ ├── trainPPO.py │ │ │ ├── trainPathRL.py │ │ │ ├── update_rospyenv.sh │ │ │ ├── vae_train.py │ │ │ └── watch_plot.py │ │ └── src │ │ │ ├── asl_pepper_collision_check.cpp │ │ │ └── asl_pepper_motion_planning_node.cpp │ ├── asl_pepper_move_base │ │ ├── CMakeLists.txt │ │ ├── launch │ │ │ ├── auto_move_base.launch │ │ │ ├── live_move_base.launch │ │ │ └── move_base.launch │ │ ├── package.xml │ │ ├── params │ │ │ ├── base_local_planner_params.yaml │ │ │ ├── costmap_common_params.yaml │ │ │ ├── global_costmap_params.yaml │ │ │ ├── global_costmap_params_sim.yaml │ │ │ └── local_costmap_params.yaml │ │ └── scripts │ │ │ └── obstacles_converter │ ├── asl_pepper_rosbags │ │ ├── CMakeLists.txt │ │ ├── launch │ │ │ ├── asl_lap.launch │ │ │ ├── cla1.launch │ │ │ ├── cla2.launch │ │ │ ├── cla_jerky.launch │ │ │ ├── corridor_koze_kids.launch │ │ │ ├── demo2.launch │ │ │ ├── fadri_nudge.launch │ │ │ ├── full_sensors.launch │ │ │ ├── full_sensors2.launch │ │ │ ├── hg1.launch │ │ │ ├── hg2.launch │ │ │ ├── jenjen_fadri_and_the_boys.launch │ │ │ ├── juan_and_florian.launch │ │ │ ├── koze.launch │ │ │ ├── koze_circuit.launch │ │ │ ├── koze_circuit_filtered.launch │ │ │ ├── launch_rosbag_v2.launch │ │ │ ├── manip_corner.launch │ │ │ ├── merged_demo2.launch │ │ │ ├── multiagent_sim_koze.launch │ │ │ ├── multiagent_sim_unity.launch │ │ │ ├── realsense_crowd.launch │ │ │ ├── realsense_test.launch │ │ │ ├── rik_bananas.launch │ │ │ ├── simulator.launch │ │ │ └── temp.launch │ │ ├── misc │ │ │ ├── all_realsense.txt │ │ │ ├── open_lab.txt │ │ │ └── topics_pepper_all_sensors.txt │ │ └── package.xml │ ├── asl_pepper_sensor_preprocessing │ │ ├── CMakeLists.txt │ │ ├── README.md │ │ ├── launch │ │ │ ├── combine_laser_scans.launch │ │ │ ├── crop_laser_scans.launch │ │ │ ├── depth_to_pc2.launch │ │ │ ├── filter_laser_scans.launch │ │ │ ├── front_laser_filters.yaml │ │ │ └── rear_laser_filters.yaml │ │ ├── package.xml │ │ └── src │ │ │ ├── combine_laser_scans.cpp │ │ │ └── crop_laser_scan.cpp │ ├── dependencies.rosinstall │ ├── maps │ │ ├── asl.pgm │ │ ├── asl.yaml │ │ ├── asl_office_j.pgm │ │ ├── asl_office_j.yaml │ │ ├── cla_glass_hall.pgm │ │ ├── cla_glass_hall.yaml │ │ ├── empty.pgm │ │ ├── empty.yaml │ │ ├── hg_main_hall.pgm │ │ ├── hg_main_hall.yaml │ │ ├── markclutter.pgm │ │ ├── markclutter.yaml │ │ ├── markcomplex.pgm │ │ ├── markcomplex.yaml │ │ ├── markmaze.pgm │ │ ├── markmaze.yaml │ │ ├── marksimple.pgm │ │ ├── marksimple.yaml │ │ ├── marktm1.pgm │ │ ├── marktm1.yaml │ │ ├── marktm2.pgm │ │ ├── marktm2.yaml │ │ ├── marktm3.pgm │ │ ├── marktm3.yaml │ │ ├── marktrain.pgm │ │ ├── marktrain.yaml │ │ ├── openlab_office_j.pgm │ │ ├── openlab_office_j.yaml │ │ ├── original_ckk.pgm │ │ ├── original_ckk.yaml │ │ ├── scrubbed_ckk.pgm │ │ ├── scrubbed_ckk.yaml │ │ ├── unity_scene_map.pgm │ │ └── unity_scene_map.yaml │ └── rviz │ │ ├── default.rviz │ │ ├── downloaded.rviz │ │ ├── empty.rviz │ │ ├── framesoft.rviz │ │ ├── ia.rviz │ │ ├── ia_sim.rviz │ │ ├── iros.rviz │ │ ├── iros_ckk.rviz │ │ ├── iros_navrep.rviz │ │ ├── lasers.rviz │ │ ├── localization.rviz │ │ ├── localization_scrub.rviz │ │ ├── maplab.rviz │ │ ├── move_base.rviz │ │ ├── pedsim_simulator.rviz │ │ ├── pepper.rviz │ │ ├── pepper_cartographer.rviz │ │ ├── pepper_rl.rviz │ │ ├── pepper_sim.rviz │ │ ├── persistent_settings │ │ ├── rds.rviz │ │ ├── responsive.rviz │ │ ├── ros_planner_example.rviz │ │ ├── rvo.rviz │ │ ├── unity.rviz │ │ └── vae.rviz └── frame_msgs │ ├── CMakeLists.txt │ ├── msg │ ├── DetectedPerson.msg │ ├── DetectedPersons.msg │ ├── PersonTrajectories.msg │ ├── PersonTrajectory.msg │ ├── PersonTrajectoryEntry.msg │ ├── TrackedPerson.msg │ ├── TrackedPerson2d.msg │ ├── TrackedPersons.msg │ └── TrackedPersons2d.msg │ └── package.xml ├── ia_ros ├── CMakeLists.txt ├── launch │ ├── auto_ros_ia_node.launch │ ├── example_move_base.launch │ ├── example_move_base_no_slam.launch │ ├── example_real_map_sim.launch │ ├── example_sim_and_planner.launch │ ├── live_ros_ia_node.launch │ ├── ros_ia_node.launch │ └── ros_sim_node.launch ├── package.xml └── scripts │ ├── TODO.txt │ ├── ia_dashboard │ ├── ros_ia_node │ ├── ros_ia_node.py │ └── ros_sim_node ├── install_ros.sh ├── media ├── ian_iros_thumbnail.png └── ian_toy_example.png └── python ├── cIA ├── cia.pyx └── setup.py ├── examples ├── test_ia.py └── test_simulator.py ├── maps ├── asl.pgm ├── asl.yaml ├── asl_graph.pickle ├── corridors.pgm ├── corridors.yaml ├── default.pgm └── default.yaml ├── pyIA ├── __init__.py ├── actions.py ├── agents.py ├── arguments.py ├── ia_planning.py ├── mdptree.py ├── paths.py ├── profile_ia.sh ├── simulator.py └── state_estimation.py ├── scenarios ├── aslbusy.pickle ├── aslgates.pickle ├── aslgroups.pickle ├── aslguards.pickle ├── aslquiet.pickle ├── asltrap.pickle ├── irosasl1.pickle ├── irosasl2.pickle ├── irosasl3.pickle ├── irosasl4.pickle ├── irosasl5.pickle ├── irosasl6.pickle ├── irosasl_office_j1.pickle ├── irosasl_office_j2.pickle ├── irosasl_office_j3.pickle ├── irosasl_office_j4.pickle ├── irosasl_office_j5.pickle ├── irosasl_office_j6.pickle ├── irosunity_scene_map1.pickle ├── irosunity_scene_map2.pickle ├── irosunity_scene_map3.pickle ├── irosunity_scene_map4.pickle ├── irosunity_scene_map5.pickle ├── irosunity_scene_map6.pickle ├── none.pickle ├── officegroups.pickle ├── rlasl1.pickle ├── rlasl2.pickle ├── rlasl3.pickle ├── rlasl_office_j1.pickle ├── rlasl_office_j2.pickle ├── rlasl_office_j3.pickle ├── rlunity_scene_map1.pickle ├── rlunity_scene_map2.pickle ├── rlunity_scene_map3.pickle ├── scenarios.txt ├── toyempty1.pickle └── unitytest.pickle ├── setup.py └── tools ├── check_agent0_is_robot.py ├── draw_maps.py ├── draw_rl_scenes.py ├── draw_scenes.py ├── get_distance_to_goal.py ├── gui_graph_editor.py └── gui_world_editor.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Specific paths # 2 | ################## 3 | asl_pepper_calibration/calibration_data 4 | 5 | # File patterns # 6 | ################# 7 | *~ 8 | .ipynb_checkpoints 9 | *.pyc 10 | *.bag 11 | .ycm_extra_conf.py 12 | compile_commands.json 13 | ORBvoc.txt 14 | __pycache__/ 15 | *.bin 16 | *.out 17 | build/ 18 | 19 | *.egg-info/ 20 | -------------------------------------------------------------------------------- /dependencies.rosinstall: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: eigen_catkin 3 | uri: https://github.com/ethz-asl/eigen_catkin.git 4 | version: master 5 | - git: 6 | local-name: glog_catkin 7 | uri: https://github.com/ethz-asl/glog_catkin.git 8 | version: master 9 | - git: 10 | local-name: catkin_simple 11 | uri: https://github.com/catkin/catkin_simple.git 12 | version: master 13 | - git: 14 | local-name: shlopenslam_gmapping 15 | uri: https://github.com/danieldugas/openslam_gmapping.git 16 | version: shlopenslam 17 | - git: 18 | local-name: slam_gmapping 19 | uri: https://github.com/danieldugas/slam_gmapping.git 20 | version: shlopenslam 21 | - git: 22 | local-name: opencv3_catkin 23 | uri: https://github.com/ethz-asl/opencv3_catkin.git 24 | version: master 25 | - git: 26 | local-name: naoqi_driver 27 | uri: https://github.com/danieldugas/naoqi_driver.git 28 | version: devel 29 | - git: 30 | local-name: naoqi_bridge_msgs 31 | uri: https://github.com/ros-naoqi/naoqi_bridge_msgs.git 32 | version: master 33 | - git: 34 | local-name: sick_scan 35 | uri: https://github.com/SICKAG/sick_scan.git 36 | version: master 37 | - git: 38 | local-name: map_matcher 39 | uri: https://github.com/danieldugas/map_matcher.git 40 | version: master 41 | - git: 42 | local-name: ros_run_command 43 | uri: https://github.com/danieldugas/ros_run_command.git 44 | version: master 45 | - git: 46 | local-name: responsive 47 | uri: https://github.com/ethz-asl/pepper_local_planning.git 48 | version: asldemo 49 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_2d_simulator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(asl_pepper_2d_simulator) 3 | find_package(catkin_simple REQUIRED) 4 | catkin_simple() 5 | cs_install() 6 | cs_install_scripts(scripts/pepper_2d_simulator) 7 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_2d_simulator/launch/pepper_2d_simulator.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_2d_simulator/launch/static_pepper_upload.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_2d_simulator/meshes/pepper_frozen_standing.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/interaction_actions_for_navigation/e38ca94127ada5a72389c661829cb2fa85db782e/external/asl_pepper/asl_pepper_2d_simulator/meshes/pepper_frozen_standing.stl -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_2d_simulator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | asl_pepper_2d_simulator 4 | 0.0.1 5 | simple python based 2d simulator for pepper 6 | Daniel Dugas 7 | Daniel Dugas 8 | MIT 9 | 10 | catkin 11 | catkin_simple 12 | 13 | 14 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_2d_simulator/python/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup, find_packages 2 | import os 3 | 4 | setup( 5 | name="pepper_2d_simulator", 6 | description='Python library for Pepper 2d simulation', 7 | author='Daniel Dugas', 8 | version='0.0', 9 | # packages=find_packages(), 10 | py_modules=[ 11 | 'gymified_pepper_envs', 12 | 'pepper_2d_simulator', 13 | 'pepper_2d_iarlenv', 14 | ], 15 | ) 16 | 17 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_2d_simulator/scripts/pepper_2d_simulator: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from pepper_2d_simulator import * 4 | 5 | if __name__ == "__main__": 6 | # Source the current dir to be able to import map2d.py and others 7 | import os, sys 8 | scriptdir = os.path.dirname(os.path.realpath(__file__)) 9 | print("Script directory: {}".format(scriptdir)) 10 | sys.path.insert(0,scriptdir) 11 | # Load the map 12 | map_folder = "." 13 | map_name = "office_full" 14 | # raise IOError(sys.argv) 15 | if len(sys.argv) >= 3: 16 | map_folder = sys.argv[1] 17 | map_name = sys.argv[2] 18 | map_ = CMap2D(map_folder, map_name) 19 | print("Map '{}' loaded.".format(map_name)) 20 | # Standard single pepper ros simulator 21 | virtual_pepper = Virtual2DPepper(map_) 22 | virtual_pepper.run() 23 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_2d_simulator/urdf/pepper_frozen_standing.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_gmapping/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(asl_pepper_gmapping) 3 | 4 | add_definitions(-std=c++11) 5 | 6 | find_package(catkin_simple 0.1.0 REQUIRED) 7 | catkin_simple(ALL_DEPS_REQUIRED) 8 | 9 | #add_doxygen(REQUIRED) 10 | 11 | cs_install() 12 | cs_export() 13 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_gmapping/launch/gmapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_gmapping/launch/save_map.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_gmapping/maps/office_full.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/interaction_actions_for_navigation/e38ca94127ada5a72389c661829cb2fa85db782e/external/asl_pepper/asl_pepper_gmapping/maps/office_full.pgm -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_gmapping/maps/office_full.yaml: -------------------------------------------------------------------------------- 1 | image: office_full.pgm 2 | resolution: 0.020000 3 | origin: [-22.160000, -21.520000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_gmapping/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | asl_pepper_gmapping 5 | 0.0.1 6 | WIP 7 | 8 | Daniel Dugas 9 | 10 | Daniel Dugas 11 | 12 | BSD 13 | 14 | catkin 15 | catkin_simple 16 | 17 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_motion_planning/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(asl_pepper_motion_planning) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin_simple REQUIRED) 8 | # find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs sensor_msgs geometry_msgs naoqi_bridge_msgs) 9 | 10 | catkin_simple() 11 | 12 | ################################### 13 | ## catkin specific configuration ## 14 | ################################### 15 | ## The catkin_package macro generates cmake config files for your package 16 | ## Declare things to be passed to dependent projects 17 | ## INCLUDE_DIRS: uncomment this if you package contains header files 18 | ## LIBRARIES: libraries you create in this project that dependent projects also need 19 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 20 | ## DEPENDS: system dependencies of this project that dependent projects also need 21 | # catkin_package( 22 | # INCLUDE_DIRS include 23 | # LIBRARIES asl_pepper_motion_planning 24 | # CATKIN_DEPENDS joy 25 | # DEPENDS system_lib 26 | # ) 27 | 28 | include_directories(include ${catkin_INCLUDE_DIRS}) 29 | 30 | add_compile_options(-std=c++11) 31 | 32 | ########### 33 | ## Build ## 34 | ########### 35 | 36 | ## Specify additional locations of header files 37 | ## Your package locations should be listed before other locations 38 | # include_directories(include) 39 | 40 | ## Declare a C++ library 41 | # add_library(asl_pepper_motion_planning 42 | # src/${PROJECT_NAME}/asl_pepper_motion_planning.cpp 43 | # ) 44 | 45 | ## Declare a C++ executable 46 | cs_add_executable(asl_pepper_motion_planning_node src/asl_pepper_motion_planning_node.cpp) 47 | 48 | ## Specify libraries to link a library or executable target against 49 | target_link_libraries(asl_pepper_motion_planning_node ${catkin_LIBRARIES}) 50 | 51 | cs_install() 52 | cs_install_scripts(scripts/global_planner scripts/pepper_2d_simulator scripts/test_rl scripts/motion_planner scripts/rvo_planner) 53 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_motion_planning/launch/amcl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_motion_planning/launch/global_planner.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_motion_planning/launch/motion_planner.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_motion_planning/launch/rvo_planner.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 34 | 35 | 36 | 37 | 38 | 39 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 49 | 50 | 51 | 52 | 53 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_motion_planning/launch/rvo_planner_sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 36 | 37 | 38 | 39 | 40 | 41 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_motion_planning/launch/test_rl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_motion_planning/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | asl_pepper_motion_planning 5 | 0.0.1 6 | WIP 7 | Daniel Dugas 8 | Daniel Dugas 9 | BSD 10 | 11 | catkin 12 | catkin_simple 13 | joy 14 | roscpp 15 | 16 | spencer_tracking_msgs 17 | 18 | 19 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_motion_planning/scripts/big_empty.yaml: -------------------------------------------------------------------------------- 1 | image: empty.pgm 2 | resolution: 0.0200000 3 | origin: [-10.480000, -10.480000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.15 #0.196 7 | 8 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_motion_planning/scripts/cnn_policy.py: -------------------------------------------------------------------------------- 1 | import baselines.common.tf_util as U 2 | import tensorflow as tf 3 | import gym 4 | from baselines.common.distributions import make_pdtype 5 | 6 | class CnnPolicy(object): 7 | recurrent = False 8 | def __init__(self, name, ob_space, ac_space, kind='large'): 9 | with tf.variable_scope(name): 10 | self._init(ob_space, ac_space, kind) 11 | self.scope = tf.get_variable_scope().name 12 | 13 | def _init(self, ob_space, ac_space, kind): 14 | assert isinstance(ob_space, gym.spaces.Box) 15 | 16 | self.pdtype = pdtype = make_pdtype(ac_space) 17 | sequence_length = None 18 | 19 | ob = U.get_placeholder(name="ob", dtype=tf.float32, shape=[sequence_length] + list(ob_space.shape)) 20 | ob_2 = U.get_placeholder(name="ob_2", dtype=tf.float32, shape=[sequence_length] + [5]) # observations to feed in after convolutions 21 | ob_2_fc = tf.nn.relu(tf.layers.dense(ob_2, 64, name='s2_preproc', kernel_initializer=U.normc_initializer(1.0))) 22 | 23 | x = ob / 25. 24 | if kind == 'small': # from A3C paper 25 | x = tf.nn.relu(U.conv2d(x, 16, "l1", [2, 8], [1, 4], pad="VALID")) 26 | x = tf.nn.relu(U.conv2d(x, 32, "l2", [2, 4], [1, 2], pad="VALID")) 27 | x = U.flattenallbut0(x) 28 | x = tf.nn.relu(tf.layers.dense(x, 256, name='lin', kernel_initializer=U.normc_initializer(1.0))) 29 | x = tf.concat([x, ob_2_fc], axis=-1) 30 | elif kind == 'large': # Nature DQN 31 | x = tf.nn.relu(U.conv2d(x, 32, "l1", [2, 8], [1, 4], pad="VALID")) 32 | x = tf.nn.relu(U.conv2d(x, 64, "l2", [2, 4], [1, 2], pad="VALID")) 33 | x = tf.nn.relu(U.conv2d(x, 64, "l3", [2, 3], [1, 1], pad="VALID")) 34 | x = U.flattenallbut0(x) 35 | x = tf.nn.relu(tf.layers.dense(x, 512, name='lin', kernel_initializer=U.normc_initializer(1.0))) 36 | x = tf.concat([x, ob_2_fc], axis=-1) 37 | else: 38 | raise NotImplementedError 39 | 40 | x = tf.nn.relu(tf.layers.dense(x, 256, name='merged_lin', kernel_initializer=U.normc_initializer(1.0))) 41 | logits = tf.layers.dense(x, pdtype.param_shape()[0], name='logits', kernel_initializer=U.normc_initializer(0.01)) 42 | self.pd = pdtype.pdfromflat(logits) 43 | self.vpred = tf.layers.dense(x, 1, name='value', kernel_initializer=U.normc_initializer(1.0))[:,0] 44 | 45 | self.state_in = [] 46 | self.state_out = [] 47 | 48 | stochastic = tf.placeholder(name="stochastic", dtype=tf.bool, shape=()) 49 | ac = self.pd.sample() # XXX 50 | self._act = U.function([stochastic, ob, ob_2], [ac, self.vpred]) 51 | 52 | def act(self, stochastic, ob): 53 | ob_1 = ob[0] # laser state 54 | ob_2 = ob[1] # goal position and robot vel state 55 | ac1, vpred1 = self._act(stochastic, ob_1, ob_2) 56 | return ac1, vpred1 57 | def get_variables(self): 58 | return tf.get_collection(tf.GraphKeys.GLOBAL_VARIABLES, self.scope) 59 | def get_trainable_variables(self): 60 | return tf.get_collection(tf.GraphKeys.TRAINABLE_VARIABLES, self.scope) 61 | def get_initial_state(self): 62 | return [] 63 | def load_variables(self, path): 64 | saver = tf.train.Saver() 65 | sess = U.get_session() 66 | saver.restore(sess, path) 67 | def save_variables(self, path): 68 | saver = tf.train.Saver() 69 | sess = U.get_session() 70 | # variables = self.get_variables() 71 | import os 72 | dirname = os.path.dirname(path) 73 | if any(dirname): 74 | os.makedirs(dirname, exist_ok=True) 75 | saver.save(sess, path) 76 | 77 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_motion_planning/scripts/empty.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/interaction_actions_for_navigation/e38ca94127ada5a72389c661829cb2fa85db782e/external/asl_pepper/asl_pepper_motion_planning/scripts/empty.pgm -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_motion_planning/scripts/empty.yaml: -------------------------------------------------------------------------------- 1 | image: empty.pgm 2 | resolution: 0.0100000 3 | origin: [-5.480000, -5.480000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.15 #0.196 7 | 8 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_motion_planning/scripts/image_loading_performance.py: -------------------------------------------------------------------------------- 1 | import os 2 | import skimage 3 | from timeit import default_timer as timer 4 | 5 | def chunks(l, n): 6 | for i in range(0, len(l), n): 7 | yield l[i:i+n] 8 | 9 | from pympler import asizeof 10 | 11 | MAX_BATCH_SIZE = 1000 12 | IMAGE_DIR = os.path.expanduser('/media/pithos/nyc_frames') 13 | MASK_DIR = os.path.expanduser('/media/pithos/nyc_masks') 14 | filenames = sorted(os.listdir(IMAGE_DIR)) 15 | 16 | def get_memusage(): 17 | with open('/proc/self/status') as f: 18 | memusage = f.read().split('VmRSS:')[1].split('\n')[0] 19 | memsize = int(memusage.split(' ')[-2]) 20 | memunit = memusage.split(' ')[-1] 21 | if not memunit == "kB": 22 | raise ValueError(memunit + " unknown") 23 | return memsize 24 | 25 | start_memusage = get_memusage() 26 | memunit = "kB" 27 | print("Initial memory: {} {}".format(start_memusage, memunit)) 28 | printed_lines = 1 29 | 30 | for batch_size in range(1, MAX_BATCH_SIZE): 31 | if batch_size > 100 and not batch_size % 10 == 0: 32 | continue 33 | if batch_size > 1000 and not batch_size % 100 == 0: 34 | continue 35 | try: 36 | filename_batch = filenames[:batch_size] 37 | tic = timer() 38 | image_batch = [skimage.io.imread(os.path.join(IMAGE_DIR, filename)) for filename in filename_batch] 39 | toc = timer() 40 | load_time = toc-tic 41 | size = get_memusage() 42 | if printed_lines == 1 or printed_lines % 32 == 0: 43 | print("# images loaded | loading time [s] | per image [s] | size [{}] | diff [{}] | per image [{}]".format(memunit, memunit, memunit)) 44 | print("-------------------------------------------------------------------------------------------") 45 | print(" {:>14} | {:>16.2f} | {:>13.3f} | {:>9} | {:>9} | {:>10} ".format( 46 | len(image_batch), load_time, load_time/len(image_batch), size, size-start_memusage, (size-start_memusage)//len(image_batch))) 47 | printed_lines += 1 48 | del image_batch 49 | except KeyboardInterrupt: 50 | break 51 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_motion_planning/scripts/leonhard_train_PPO.sh: -------------------------------------------------------------------------------- 1 | # bsub -W 120:00 -R "rusage[ngpus_excl_p=1, mem=8192]" < 4.sh 2 | export MACHINE_NAME="leonhard" 3 | cd ~ 4 | module load eth_proxy python_gpu/3.6.4 5 | pip install --user -e ~/Documents/baselines 6 | pip install --user numba 7 | # install range_libc 8 | pip install --user cython 9 | cd ~/Documents/range_libc/pywrapper 10 | WITH_CUDA=ON python setup.py install --user 11 | python setupcmap2d.py install --user 12 | # pip install --user pycuda 13 | cd ~/Documents/pepper_ws/src/asl_pepper/asl_pepper_motion_planning/scripts 14 | for i in {1..100} 15 | do 16 | python trainPPO.py --mode BOUNCE --map-name office_full --reward-collision -5 17 | if [[ $? -eq 139 ]]; then 18 | echo "oops, sigsegv"; 19 | else 20 | break 21 | fi 22 | done 23 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_motion_planning/scripts/motion_planner: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from motion_planner import * 3 | 4 | def parse_args(): 5 | import argparse 6 | ## Arguments 7 | parser = argparse.ArgumentParser(description='Test node for the pepper RL path planning algorithm.') 8 | parser.add_argument('--map-folder', 9 | type=str, 10 | default=".", 11 | ) 12 | parser.add_argument('--map-name', 13 | type=str, 14 | default="office_full", 15 | ) 16 | parser.add_argument('--hot-start', 17 | action='store_true', 18 | help="if enabled, does not wait for a control handover", 19 | ) 20 | ARGS, unknown_args = parser.parse_known_args() 21 | 22 | # deal with unknown arguments 23 | # ROS appends some weird args, ignore those, but not the rest 24 | if unknown_args: 25 | non_ros_unknown_args = rospy.myargv(unknown_args) 26 | if non_ros_unknown_args: 27 | print("unknown arguments:") 28 | print(non_ros_unknown_args) 29 | parser.parse_args(args=["--help"]) 30 | raise ValueError 31 | return ARGS 32 | 33 | if __name__ == "__main__": 34 | args = parse_args() 35 | map_ = Map2D(args.map_folder, args.map_name) 36 | motion_planner = MotionPlanner(map_, start_disabled= not args.hot_start ) 37 | motion_planner.run() 38 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_motion_planning/scripts/office_full.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/interaction_actions_for_navigation/e38ca94127ada5a72389c661829cb2fa85db782e/external/asl_pepper/asl_pepper_motion_planning/scripts/office_full.pgm -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_motion_planning/scripts/office_full.yaml: -------------------------------------------------------------------------------- 1 | image: office_full.pgm 2 | resolution: 0.020000 3 | origin: [-32.160000, -11.520000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.15 #0.196 7 | 8 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_motion_planning/scripts/publish_sim_map.sh: -------------------------------------------------------------------------------- 1 | rosrun map_server map_server office_full.yaml _frame_id:="odom" 2 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_motion_planning/scripts/rooms.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/interaction_actions_for_navigation/e38ca94127ada5a72389c661829cb2fa85db782e/external/asl_pepper/asl_pepper_motion_planning/scripts/rooms.pgm -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_motion_planning/scripts/rooms.yaml: -------------------------------------------------------------------------------- 1 | image: rooms.pgm 2 | resolution: 0.0100000 3 | origin: [-5.480000, -5.480000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.15 #0.196 7 | 8 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_motion_planning/scripts/run_rl.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | from mpi4py import MPI 4 | from baselines.common import set_global_seeds 5 | from baselines import bench 6 | import os.path as osp 7 | from baselines import logger 8 | from baselines.common.atari_wrappers import make_atari, wrap_deepmind 9 | from pepper_2d_simulator import PepperRLEnv 10 | from map2d import Map2D 11 | 12 | import numpy as np 13 | from gym.spaces.box import Box 14 | 15 | 16 | def train(num_timesteps, seed, resume): 17 | map_folder = "." 18 | map_name = "empty" 19 | map_ = Map2D(map_folder, map_name) 20 | print("Map '{}' loaded.".format(map_name)) 21 | # RL multi-agent simulator 22 | import pposgd_simple 23 | import cnn_policy 24 | import baselines.common.tf_util as U 25 | rank = MPI.COMM_WORLD.Get_rank() 26 | sess = U.single_threaded_session() 27 | sess.__enter__() 28 | if rank == 0: 29 | logger.configure() 30 | else: 31 | logger.configure(format_strs=[]) 32 | env = PepperRLEnv(args) 33 | workerseed = seed + 10000 * MPI.COMM_WORLD.Get_rank() if seed is not None else None 34 | set_global_seeds(workerseed) 35 | def policy_fn(name, ob_space, ac_space): #pylint: disable=W0613 36 | return cnn_policy.CnnPolicy(name=name, ob_space=ob_space, ac_space=ac_space) 37 | pposgd_simple.learn(env, policy_fn, 38 | max_timesteps=int(num_timesteps * 1.1), 39 | timesteps_per_actorbatch=256, 40 | clip_param=0.2, entcoeff=0.01, 41 | optim_epochs=4, 42 | optim_stepsize=1e-3, # original 1e-3 43 | optim_batchsize=64, 44 | gamma=0.99, lam=0.95, 45 | schedule='linear', 46 | resume_training=resume, 47 | ) 48 | env.close() 49 | 50 | def parse_args(): 51 | import argparse 52 | ## Arguments 53 | parser = argparse.ArgumentParser(description='Keeps track of git repositories.') 54 | parser.add_argument( 55 | '--resume', 56 | dest='resume', 57 | action='store_true', 58 | help='loads saved models and continues training from there.', 59 | ) 60 | parser.add_argument( 61 | '--seed', 62 | dest="seed", 63 | type=int, 64 | default=None, 65 | help='seed value', 66 | ) 67 | parser.add_argument( 68 | '--timesteps', 69 | dest="num_timesteps", 70 | type=int, 71 | default=1000000, 72 | help='timesteps to train for', 73 | ) 74 | 75 | ARGS = parser.parse_args() 76 | return ARGS 77 | 78 | 79 | if __name__ == '__main__': 80 | args = parse_args() 81 | train(num_timesteps=args.num_timesteps, seed=args.seed, resume=args.resume) 82 | 83 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_motion_planning/scripts/run_sb_RVO_load.py: -------------------------------------------------------------------------------- 1 | import gym 2 | import numpy as np 3 | 4 | from stable_baselines.common.policies import MlpPolicy 5 | from stable_baselines.common.vec_env import DummyVecEnv 6 | from stable_baselines import PPO2, A2C 7 | 8 | # from pepper_2d_iarlenv import MultiIARLEnv, parse_iaenv_args, check_iaenv_args 9 | from pepper_2d_simulator import populate_PepperRLEnv_args, check_PepperRLEnv_args 10 | from pepper_2d_iarlenv import parse_training_args 11 | from gymified_pepper_envs import PepperRLVecEnvRVO, evaluate_model 12 | 13 | # Env 14 | args, _ = parse_training_args( 15 | ignore_unknown=False, 16 | env_populate_args_func=populate_PepperRLEnv_args, 17 | env_name="PepperRLVecEnvRVO" 18 | ) 19 | args.no_ros = True 20 | args.continuous = True 21 | args.n_envs = 8 22 | check_PepperRLEnv_args(args) 23 | 24 | env = PepperRLVecEnvRVO(args) 25 | 26 | model = PPO2.load(args.resume_from) 27 | 28 | obs = env.reset() 29 | for i in range(1000): 30 | action, _states = model.predict(obs) 31 | obs, rewards, dones, info = env.step(action) 32 | env.render(RENDER_LIDAR=True) 33 | 34 | print("Rendering done") 35 | 36 | mean_reward = evaluate_model(model, env, num_steps=10000) 37 | 38 | env.close() 39 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_motion_planning/scripts/run_sb_RVO_train.py: -------------------------------------------------------------------------------- 1 | import gym 2 | import os 3 | 4 | from stable_baselines.common.policies import MlpPolicy 5 | from stable_baselines.common.vec_env import DummyVecEnv 6 | from stable_baselines import PPO2, A2C 7 | 8 | # from pepper_2d_iarlenv import MultiIARLEnv, parse_iaenv_args, check_iaenv_args 9 | from pepper_2d_simulator import populate_PepperRLEnv_args, check_PepperRLEnv_args 10 | from pepper_2d_iarlenv import parse_training_args 11 | from gymified_pepper_envs import PepperRLVecEnvRVO, evaluate_model 12 | 13 | # args 14 | 15 | args, _ = parse_training_args( 16 | ignore_unknown=False, 17 | env_populate_args_func=populate_PepperRLEnv_args, 18 | env_name="PepperRLVecEnvRVO" 19 | ) 20 | args.no_ros = True 21 | args.continuous = True 22 | args.n_envs = 8 23 | check_PepperRLEnv_args(args) 24 | 25 | env = PepperRLVecEnvRVO(args) 26 | 27 | assert args.model == "PPO2" 28 | assert args.policy == "MlpPolicy" 29 | model = PPO2(MlpPolicy, env, verbose=1, tensorboard_log=args.summary_root_dir+'/') 30 | 31 | 32 | try: 33 | model.learn(total_timesteps=args.total_timesteps, tb_log_name=args.run_name) 34 | except KeyboardInterrupt: 35 | pass 36 | 37 | if not os.path.exists(args.checkpoint_root_dir): 38 | os.makedirs(args.checkpoint_root_dir) 39 | model.save(args.checkpoint_path) 40 | print("Model '{}' saved".format(args.checkpoint_path)) 41 | 42 | mean_reward = evaluate_model(model, env, num_steps=100) 43 | 44 | env.close() 45 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_motion_planning/scripts/run_sb_load.py: -------------------------------------------------------------------------------- 1 | import gym 2 | import numpy as np 3 | 4 | from stable_baselines.common.policies import MlpPolicy 5 | from stable_baselines.common.vec_env import DummyVecEnv 6 | from stable_baselines import PPO2, A2C 7 | 8 | # from pepper_2d_iarlenv import MultiIARLEnv, parse_iaenv_args, check_iaenv_args 9 | from pepper_2d_simulator import populate_PepperRLEnv_args, check_PepperRLEnv_args 10 | from pepper_2d_iarlenv import parse_training_args 11 | from gymified_pepper_envs import PepperRLVecEnv, evaluate_model 12 | 13 | # Env 14 | args, _ = parse_training_args( 15 | ignore_unknown=False, 16 | env_populate_args_func=populate_PepperRLEnv_args, 17 | env_name="PepperRLVecEnv" 18 | ) 19 | args.no_ros = True 20 | args.continuous = True 21 | check_PepperRLEnv_args(args) 22 | env = PepperRLVecEnv(args) 23 | 24 | model = PPO2.load(args.resume_from) 25 | 26 | obs = env.reset() 27 | for i in range(1000): 28 | action, _states = model.predict(obs) 29 | obs, rewards, dones, info = env.step(action) 30 | env.render() 31 | 32 | print("Rendering done") 33 | 34 | mean_reward = evaluate_model(model, env, num_steps=10000) 35 | 36 | env.close() 37 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_motion_planning/scripts/run_sb_render_RVO.py: -------------------------------------------------------------------------------- 1 | import gym 2 | import numpy as np 3 | 4 | from stable_baselines.common.policies import MlpPolicy 5 | from stable_baselines.common.vec_env import DummyVecEnv 6 | from stable_baselines import PPO2, A2C 7 | 8 | # from pepper_2d_iarlenv import MultiIARLEnv, parse_iaenv_args, check_iaenv_args 9 | from pepper_2d_simulator import populate_PepperRLEnv_args, check_PepperRLEnv_args 10 | from pepper_2d_iarlenv import parse_training_args 11 | from gymified_pepper_envs import PepperRLVecEnvRVO, evaluate_model 12 | 13 | # Env 14 | args, _ = parse_training_args( 15 | ignore_unknown=False, 16 | env_populate_args_func=populate_PepperRLEnv_args, 17 | env_name="PepperRLVecEnvRVO" 18 | ) 19 | args.no_ros = True 20 | args.continuous = True 21 | args.n_envs = 8 22 | args.mode = "RESET_ALL" 23 | check_PepperRLEnv_args(args) 24 | 25 | env = PepperRLVecEnvRVO(args) 26 | 27 | model = PPO2(MlpPolicy, env, verbose=1) 28 | 29 | obs = env.reset() 30 | for i in range(1000): 31 | action, _states = model.predict(obs) 32 | obs, rewards, dones, info = env.step(action) 33 | env.render() 34 | 35 | print("Rendering done") 36 | 37 | env.close() 38 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_motion_planning/scripts/run_sb_train.py: -------------------------------------------------------------------------------- 1 | import gym 2 | import os 3 | 4 | from stable_baselines.common.policies import MlpPolicy 5 | from stable_baselines.common.vec_env import DummyVecEnv 6 | from stable_baselines import PPO2, A2C 7 | 8 | # from pepper_2d_iarlenv import MultiIARLEnv, parse_iaenv_args, check_iaenv_args 9 | from pepper_2d_simulator import populate_PepperRLEnv_args, check_PepperRLEnv_args 10 | from pepper_2d_iarlenv import parse_training_args 11 | from gymified_pepper_envs import PepperRLVecEnv, evaluate_model 12 | 13 | # args 14 | 15 | args, _ = parse_training_args( 16 | ignore_unknown=False, 17 | env_populate_args_func=populate_PepperRLEnv_args, 18 | env_name="PepperRLVecEnv" 19 | ) 20 | args.no_ros = True 21 | args.continuous = True 22 | check_PepperRLEnv_args(args) 23 | 24 | env = PepperRLVecEnv(args) 25 | 26 | assert args.model == "PPO2" 27 | assert args.policy == "MlpPolicy" 28 | model = PPO2(MlpPolicy, env, verbose=1) 29 | model.learn(total_timesteps=args.total_timesteps) 30 | 31 | if not os.path.exists(args.checkpoint_root_dir): 32 | os.makedirs(args.checkpoint_root_dir) 33 | model.save(args.checkpoint_path) 34 | print("Model '{}' saved".format(args.checkpoint_path)) 35 | 36 | mean_reward = evaluate_model(model, env, num_steps=100) 37 | 38 | env.close() 39 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_motion_planning/scripts/trainPathRL.py: -------------------------------------------------------------------------------- 1 | from trainPPO import * 2 | from smallMlp import smallMlp 3 | 4 | from pepper_2d_iarlenv import IARLEnv, populate_iaenv_args, check_iaenv_args 5 | from pepper_2d_iarlenv import MultiIARLEnv, parse_training_args 6 | if __name__ == '__main__': 7 | tf.reset_default_graph() 8 | 9 | env_populate_args_func = populate_iaenv_args 10 | env_check_args_func = check_iaenv_args 11 | env_type = MultiIARLEnv 12 | 13 | # args 14 | args, _ = parse_training_args( 15 | ignore_unknown=False, 16 | env_populate_args_func=env_populate_args_func, 17 | env_name="IARLEnv" 18 | ) 19 | if not args.force_publish_ros: 20 | args.no_ros = True 21 | env_check_args_func(args) 22 | print(args) 23 | 24 | if args.policy == "MlpPolicy": 25 | policy_type = MlpPPO 26 | elif args.policy == "SmallMlpPolicy": 27 | policy_type = smallMlp 28 | else: 29 | raise NotImplementedError 30 | 31 | # summary 32 | summary_writer = tf.summary.FileWriter(args.summary_path) 33 | 34 | # random seed 35 | tf.set_random_seed(args.random_seed) 36 | np.random.seed(args.random_seed) 37 | random.seed(args.random_seed) 38 | 39 | # Load environment 40 | env = env_type(args) # new PepperRLEnv( 41 | print("Created environment") 42 | 43 | # Run training 44 | chief = Worker('Chief', env, summary_writer, args, policy_type) 45 | with tf.Session() as sess: 46 | saver = tf.train.Saver(max_to_keep=3, save_relative_paths=True) 47 | best_saver = tf.train.Saver(max_to_keep=3, save_relative_paths=True) 48 | sess.run(tf.global_variables_initializer()) 49 | if args.resume_from != '': 50 | chief.ppo.load_model(sess, saver) 51 | chief.process(sess, saver, best_saver) 52 | 53 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_motion_planning/scripts/update_rospyenv.sh: -------------------------------------------------------------------------------- 1 | # Python dependencies 2 | source ~/peppervenv/bin/activate 3 | cd ~/Code/pepper_ws/src/asl_pepper/asl_pepper_2d_simulator/python 4 | pip install -e . 5 | cd ~/Code/pepper_ws/src/pyniel || cd ~/Code/pyniel 6 | git pull --ff-only 7 | pip install . 8 | cd ~/Code/pepper_ws/src/range_libc/pywrapper 9 | git pull --ff-only 10 | python setup.py install 11 | cd ~/Code/pepper_ws/src/pymap2d 12 | git pull --ff-only 13 | pip install . 14 | cd ~/Code/pepper_ws/src/pylidar2d 15 | git pull --ff-only 16 | pip install . 17 | cd ~/Code/pepper_ws/src/interaction_actions/python/cIA 18 | git pull --ff-only 19 | pip install . 20 | cd .. 21 | pip install -e . 22 | cd ~/Code/pepper_ws/src/responsive/lib_dwa 23 | git pull --ff-only 24 | pip install . 25 | cd ../lib_clustering 26 | pip install . 27 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_motion_planning/scripts/watch_plot.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from matplotlib import pyplot as plt 3 | 4 | rew_hist_filepath="/tmp/instant_rewards.txt" 5 | def watch_rewplot(mode=0, path=rew_hist_filepath, pause_duration=1): 6 | """ 7 | mode 0 is active mode, the axes limits get periodically updated 8 | mode 1 is passive mode, move freely """ 9 | try: 10 | first = True 11 | while True: 12 | try: 13 | rewplot = np.loadtxt(path, delimiter=",").T 14 | cumsum = np.cumsum(rewplot, axis=1) 15 | except: 16 | plt.clf() 17 | plt.pause(pause_duration) 18 | continue 19 | # reset sum at each crash / arrival 20 | final = rewplot * 1. 21 | final[np.abs(final) > 10] = -cumsum[np.abs(final) > 10] 22 | final_cumsum = np.cumsum(final, axis=1) 23 | # plot 24 | plt.ion() 25 | if mode == 0 or first: # Active 26 | fig = plt.gcf() 27 | fig.clear() 28 | ax1 = fig.add_subplot(211) 29 | ax2 = fig.add_subplot(212) 30 | lines1 = [] 31 | lines2 = [] 32 | for curve in rewplot: 33 | line1, = ax1.plot(curve) # Returns a tuple of line objects, thus the comma 34 | lines1.append(line1) 35 | for curve in cumsum: 36 | line2, = ax2.plot(curve) 37 | lines2.append(line2) 38 | plt.show() 39 | if mode == 1: # Passive 40 | for line1, curve in zip(lines1, rewplot): 41 | line1.set_data(np.arange(len(curve)), curve) 42 | for line2, curve in zip(lines2, cumsum): 43 | line2.set_data(np.arange(len(curve)), curve) 44 | fig.canvas.draw() 45 | fig.canvas.flush_events() 46 | plt.pause(pause_duration) 47 | first = False 48 | except KeyboardInterrupt: 49 | mode += 1 50 | if mode > 1: 51 | plt.close() 52 | raise KeyboardInterrupt 53 | print("Switching to passive mode. Graph will still update, but panning and zoom is now free.") 54 | print("Ctrl-C again to close and quit") 55 | watch_rewplot(mode=1, path=path, pause_duration=pause_duration) 56 | 57 | if __name__=="__main__": 58 | watch_rewplot() 59 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_motion_planning/src/asl_pepper_collision_check.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace asl_pepper_collision_check { 12 | 13 | class PepperCollisionChecker { 14 | public: 15 | explicit PepperCollisionChecker(ros::NodeHandle& n) : nh_(n) { 16 | // Topic names. 17 | const std::string kLaserScanTopic = "/scan1"; 18 | const std::string kCmdVelTopic = "/cmd_vel"; 19 | 20 | // Publishers and subscribers. 21 | laser_sub_ = nh_.subscribe(kLaserScanTopic, 1000, &PepperCollisionChecker::laserCallback, this); 22 | cmd_vel_pub_ = nh_.advertise(kCmdVelTopic, 1); 23 | } 24 | ~PepperCollisionChecker() { 25 | } 26 | 27 | protected: 28 | /// \brief receives joystick messages 29 | void laserCallback(const sensor_msgs::LaserScan::ConstPtr& msg) { 30 | // TODO: check safe 31 | 32 | // Send killMove message 33 | geometry_msgs::Twist cmd_vel_msg; 34 | cmd_vel_msg.angular.x = 1337.; 35 | cmd_vel_pub_.publish(cmd_vel_msg); 36 | // Raise Flag 37 | // TODO 38 | } 39 | 40 | private: 41 | ros::NodeHandle& nh_; 42 | ros::Subscriber laser_sub_; 43 | ros::Publisher cmd_vel_pub_; 44 | 45 | }; // class PepperCollisionChecker 46 | 47 | } // namespace asl_pepper_motion_planning 48 | 49 | using namespace asl_pepper_collision_check; 50 | 51 | int main(int argc, char **argv) { 52 | 53 | ros::init(argc, argv, "asl_pepper_collision_check"); 54 | ros::NodeHandle n; 55 | PepperCollisionChecker pepper_collision_checker(n); 56 | 57 | try { 58 | ros::spin(); 59 | } 60 | catch (const std::exception& e) { 61 | ROS_ERROR_STREAM("Exception: " << e.what()); 62 | return 1; 63 | } 64 | catch (...) { 65 | ROS_ERROR_STREAM("Unknown Exception."); 66 | return 1; 67 | } 68 | 69 | return 0; 70 | } 71 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_move_base/launch/auto_move_base.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_move_base/launch/live_move_base.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_move_base/launch/move_base.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_move_base/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | asl_pepper_move_base 4 | 0.0.0 5 | The asl_pepper_move_base package 6 | 7 | 8 | 9 | 10 | daniel 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | move_base 53 | move_base 54 | move_base 55 | pylidar2d_ros 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_move_base/params/base_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | # TrajectoryPlannerROS: 2 | # max_vel_x: 0.45 3 | # min_vel_x: 0.1 4 | # max_vel_theta: 1.0 5 | # min_in_place_vel_theta: 0.4 6 | # 7 | # acc_lim_theta: 3.2 8 | # acc_lim_x: 2.5 9 | # acc_lim_y: 2.5 10 | # 11 | # holonomic_robot: true 12 | 13 | TebLocalPlannerROS: 14 | 15 | odom_topic: odom 16 | map_frame: odom 17 | 18 | # Trajectory 19 | 20 | teb_autosize: True 21 | dt_ref: 0.3 22 | dt_hysteresis: 0.1 23 | global_plan_overwrite_orientation: True 24 | max_global_plan_lookahead_dist: 3.0 25 | feasibility_check_no_poses: 5 26 | 27 | # Robot 28 | 29 | max_vel_x: 0.5 30 | max_vel_x_backwards: 0.2 31 | max_vel_y: 0.4 32 | max_vel_theta: 0.3 33 | acc_lim_x: 1. 34 | acc_lim_y: 1. 35 | acc_lim_theta: 0.5 36 | min_turning_radius: 0.0 37 | footprint_model: # types: "point", "circular", "two_circles", "line", "polygon" 38 | type: "point" 39 | radius: 0.14 # for type "circular" 40 | line_start: [-0.3, 0.0] # for type "line" 41 | line_end: [0.3, 0.0] # for type "line" 42 | front_offset: 0.2 # for type "two_circles" 43 | front_radius: 0.2 # for type "two_circles" 44 | rear_offset: 0.2 # for type "two_circles" 45 | rear_radius: 0.2 # for type "two_circles" 46 | vertices: [ [0.25, -0.05], [0.18, -0.05], [0.18, -0.18], [-0.19, -0.18], [-0.25, 0], [-0.19, 0.18], [0.18, 0.18], [0.18, 0.05], [0.25, 0.05] ] # for type "polygon" 47 | 48 | # GoalTolerance 49 | 50 | xy_goal_tolerance: 0.2 51 | yaw_goal_tolerance: 0.1 52 | free_goal_vel: False 53 | 54 | # Obstacles 55 | 56 | # min_obstacle_dist: 0.1 57 | include_costmap_obstacles: True 58 | include_dynamic_obstacles: True 59 | costmap_obstacles_behind_robot_dist: 1.0 60 | obstacle_poses_affected: 30 61 | costmap_converter_plugin: "" 62 | costmap_converter_spin_thread: True 63 | costmap_converter_rate: 5 64 | 65 | # Optimization 66 | 67 | no_inner_iterations: 5 68 | no_outer_iterations: 4 69 | optimization_activate: True 70 | optimization_verbose: False 71 | penalty_epsilon: 0.1 72 | weight_max_vel_x: 1 73 | weight_max_vel_theta: 1 74 | weight_acc_lim_x: 1 75 | weight_acc_lim_theta: 1 76 | weight_kinematics_nh: 10 77 | weight_kinematics_forward_drive: 1 78 | weight_kinematics_turning_radius: 1 79 | weight_optimaltime: 1 80 | weight_obstacle: 50 81 | weight_dynamic_obstacle: 100 # not in use yet 82 | alternative_time_cost: False # not in use yet 83 | 84 | # Homotopy Class Planner 85 | 86 | enable_homotopy_class_planning: True 87 | enable_multithreading: True 88 | simple_exploration: False 89 | max_number_classes: 4 90 | roadmap_graph_no_samples: 15 91 | roadmap_graph_area_width: 5 92 | h_signature_prescaler: 0.5 93 | h_signature_threshold: 0.1 94 | obstacle_keypoint_offset: 0.1 95 | obstacle_heading_threshold: 0.45 96 | visualize_hc_graph: False 97 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_move_base/params/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | obstacle_range: 2.5 2 | raytrace_range: 3.0 3 | # footprint: [[x0, y0], [x1, y1], ... [xn, yn]] 4 | robot_radius: 0.28 5 | # inflation_radius: 1.5 6 | 7 | observation_sources: front_lidar rear_lidar # point_cloud_sensor 8 | 9 | front_lidar: {sensor_frame: sick_laser_front, data_type: LaserScan, topic: sick_laser_front/filtered, marking: true, clearing: true} 10 | rear_lidar: {sensor_frame: sick_laser_rear, data_type: LaserScan, topic: sick_laser_rear/filtered, marking: true, clearing: true} 11 | 12 | # point_cloud_sensor: {sensor_frame: frame_name, data_type: PointCloud, topic: topic_name, marking: true, clearing: true} 13 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_move_base/params/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: gmap 3 | robot_base_frame: base_footprint 4 | update_frequency: 5.0 5 | static_map: true 6 | transform_tolerance: 0.5 7 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_move_base/params/global_costmap_params_sim.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: sim_map 3 | robot_base_frame: base_footprint 4 | update_frequency: 5.0 5 | static_map: true 6 | transform_tolerance: 0.5 7 | plugins: 8 | - {name: static_layer, type: "costmap_2d::StaticLayer"} 9 | - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} 10 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 11 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_move_base/params/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: odom 3 | robot_base_frame: base_footprint 4 | update_frequency: 5.0 5 | publish_frequency: 2.0 6 | static_map: false 7 | rolling_window: true 8 | width: 10. 9 | height: 10. 10 | resolution: 0.05 11 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_move_base/scripts/obstacles_converter: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | from costmap_converter.msg import ObstacleArrayMsg, ObstacleMsg 4 | from geometry_msgs.msg import Point32 5 | from frame_msgs.msg import TrackedPersons 6 | 7 | 8 | class ObstacleConverter(object): 9 | def __init__(self): 10 | rospy.init_node("obstacles_converter") 11 | self.kObstaclesTopic = "/move_base/TebLocalPlannerROS/obstacles" 12 | self.kTracksTopic = "/rwth_tracker/tracked_persons" 13 | self.kHumanRadius = rospy.get_param("/default_human_radius", 0.3) 14 | # subscribers 15 | rospy.Subscriber(self.kTracksTopic, TrackedPersons, self.tracked_persons_callback, queue_size=1) 16 | # publishers 17 | self.obstacles_pub = rospy.Publisher(self.kObstaclesTopic, ObstacleArrayMsg, queue_size=1) 18 | # let's go 19 | rospy.spin() 20 | 21 | def tracked_persons_callback(self, msg): 22 | obstacle_msg = ObstacleArrayMsg() 23 | obstacle_msg.header = msg.header 24 | for track in msg.tracks: 25 | obst = ObstacleMsg() 26 | obst.id = track.track_id 27 | point = Point32() 28 | point.x = track.pose.pose.position.x 29 | point.y = track.pose.pose.position.y 30 | point.z = track.pose.pose.position.z 31 | obst.polygon.points.append(point) 32 | obst.radius = self.kHumanRadius 33 | obst.orientation = track.pose.pose.orientation 34 | obst.velocities = track.twist 35 | obstacle_msg.obstacles.append(obst) 36 | self.obstacles_pub.publish(obstacle_msg) 37 | 38 | 39 | if __name__ == "__main__": 40 | obstacles_converter = ObstacleConverter() 41 | 42 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_rosbags/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(asl_pepper_rosbags) 3 | find_package(catkin REQUIRED COMPONENTS) 4 | catkin_package() 5 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_rosbags/launch/asl_lap.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_rosbags/launch/cla1.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 22 | 23 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_rosbags/launch/cla2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 23 | 24 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_rosbags/launch/cla_jerky.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_rosbags/launch/corridor_koze_kids.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 27 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_rosbags/launch/demo2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_rosbags/launch/fadri_nudge.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_rosbags/launch/full_sensors.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_rosbags/launch/full_sensors2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_rosbags/launch/hg1.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 23 | 24 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_rosbags/launch/hg2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 23 | 24 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_rosbags/launch/jenjen_fadri_and_the_boys.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_rosbags/launch/juan_and_florian.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_rosbags/launch/koze.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_rosbags/launch/koze_circuit.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_rosbags/launch/koze_circuit_filtered.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_rosbags/launch/launch_rosbag_v2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_rosbags/launch/manip_corner.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_rosbags/launch/merged_demo2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_rosbags/launch/multiagent_sim_koze.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_rosbags/launch/multiagent_sim_unity.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_rosbags/launch/realsense_crowd.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_rosbags/launch/realsense_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_rosbags/launch/rik_bananas.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_rosbags/launch/simulator.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_rosbags/launch/temp.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_rosbags/misc/all_realsense.txt: -------------------------------------------------------------------------------- 1 | /camera/RGB_Camera/parameter_descriptions 2 | /camera/RGB_Camera/parameter_updates 3 | /camera/Stereo_Module/parameter_descriptions 4 | /camera/Stereo_Module/parameter_updates 5 | /camera/color/camera_info 6 | /camera/color/image_raw 7 | /camera/aligned_depth_to_color/camera_info 8 | /camera/aligned_depth_to_color/image_raw 9 | /camera/depth/camera_info 10 | /camera/depth/image_rect_raw 11 | /camera/depth_registered/points 12 | /camera/extrinsics/depth_to_color 13 | /camera/extrinsics/depth_to_infra1 14 | /camera/extrinsics/depth_to_infra2 15 | /camera/infra1/camera_info 16 | /camera/infra1/image_rect_raw 17 | /camera/infra2/camera_info 18 | /camera/infra2/image_rect_raw 19 | /camera/realsense2_camera_manager/bond 20 | /cam0/calibration 21 | /cam0/camera_info 22 | /cam0/image_raw 23 | /cam1/calibration 24 | /cam1/camera_info 25 | /cam1/image_raw 26 | /clicked_point 27 | /cmd_vel 28 | /combined_scan 29 | /cust_imu0 30 | /cust_mpu0 31 | /cust_mpu1 32 | /diagnostics 33 | /disparity 34 | /external_trigger 35 | /image_match/markers 36 | /image_match/markers_array 37 | /imu0 38 | /initialpose 39 | /joint_angles 40 | /joint_states 41 | /joy 42 | /map 43 | /map_updates 44 | /move_base_simple/goal 45 | /mpu0 46 | /mpu1 47 | /pepper_robot/audio 48 | /pepper_robot/bumper 49 | /pepper_robot/hand_touch 50 | /pepper_robot/head_touch 51 | /pepper_robot/imu/base 52 | /pepper_robot/imu/torso 53 | /pepper_robot/info 54 | /pepper_robot/laser 55 | /pepper_robot/odom 56 | /pepper_robot/pose/body_pose/cancel 57 | /pepper_robot/pose/body_pose/feedback 58 | /pepper_robot/pose/body_pose/goal 59 | /pepper_robot/pose/body_pose/result 60 | /pepper_robot/pose/body_pose/status 61 | /pepper_robot/pose/body_pose_naoqi/cancel 62 | /pepper_robot/pose/body_pose_naoqi/feedback 63 | /pepper_robot/pose/body_pose_naoqi/goal 64 | /pepper_robot/pose/body_pose_naoqi/result 65 | /pepper_robot/pose/body_pose_naoqi/status 66 | /pepper_robot/pose/joint_angles 67 | /pepper_robot/pose/joint_angles_action/cancel 68 | /pepper_robot/pose/joint_angles_action/feedback 69 | /pepper_robot/pose/joint_angles_action/goal 70 | /pepper_robot/pose/joint_angles_action/result 71 | /pepper_robot/pose/joint_angles_action/status 72 | /pepper_robot/pose/joint_stiffness 73 | /pepper_robot/pose/joint_stiffness_trajectory/cancel 74 | /pepper_robot/pose/joint_stiffness_trajectory/feedback 75 | /pepper_robot/pose/joint_stiffness_trajectory/goal 76 | /pepper_robot/pose/joint_stiffness_trajectory/result 77 | /pepper_robot/pose/joint_stiffness_trajectory/status 78 | /pepper_robot/pose/joint_trajectory/cancel 79 | /pepper_robot/pose/joint_trajectory/feedback 80 | /pepper_robot/pose/joint_trajectory/goal 81 | /pepper_robot/pose/joint_trajectory/result 82 | /pepper_robot/pose/joint_trajectory/status 83 | /points2 84 | /rosout 85 | /rosout_agg 86 | /sick_laser_front/cloud 87 | /sick_laser_front/parameter_descriptions 88 | /sick_laser_front/parameter_updates 89 | /sick_laser_front/scan 90 | /sick_laser_front/cropped_scan 91 | /sick_laser_rear/cloud 92 | /sick_laser_rear/parameter_descriptions 93 | /sick_laser_rear/parameter_updates 94 | /sick_laser_rear/scan 95 | /sick_laser_rear/cropped_scan 96 | /speech 97 | /stereo_image_proc/parameter_descriptions 98 | /stereo_image_proc/parameter_updates 99 | /stereo_image_proc_debayer_left/parameter_descriptions 100 | /stereo_image_proc_debayer_left/parameter_updates 101 | /stereo_image_proc_debayer_right/parameter_descriptions 102 | /stereo_image_proc_debayer_right/parameter_updates 103 | /stereo_image_proc_rectify_color_left/parameter_descriptions 104 | /stereo_image_proc_rectify_color_left/parameter_updates 105 | /stereo_image_proc_rectify_color_right/parameter_descriptions 106 | /stereo_image_proc_rectify_color_right/parameter_updates 107 | /stereo_image_proc_rectify_mono_left/parameter_descriptions 108 | /stereo_image_proc_rectify_mono_left/parameter_updates 109 | /stereo_image_proc_rectify_mono_right/parameter_descriptions 110 | /stereo_image_proc_rectify_mono_right/parameter_updates 111 | /tf 112 | /tf_static 113 | /time_host 114 | /visensor_node/parameter_descriptions 115 | /visensor_node/parameter_updates 116 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_rosbags/misc/open_lab.txt: -------------------------------------------------------------------------------- 1 | /camera/RGB_Camera/parameter_descriptions 2 | /camera/RGB_Camera/parameter_updates 3 | /camera/Stereo_Module/parameter_descriptions 4 | /camera/Stereo_Module/parameter_updates 5 | /camera/color/camera_info 6 | /camera/color/image_raw 7 | /camera/aligned_depth_to_color/camera_info 8 | /camera/aligned_depth_to_color/image_raw 9 | /camera/extrinsics/depth_to_color 10 | /camera/extrinsics/depth_to_infra1 11 | /camera/extrinsics/depth_to_infra2 12 | /camera/realsense2_camera_manager/bond 13 | /cam0/calibration 14 | /cam0/camera_info 15 | /cam0/image_raw 16 | /cam1/calibration 17 | /cam1/camera_info 18 | /cam1/image_raw 19 | /clicked_point 20 | /cmd_vel 21 | /cust_imu0 22 | /cust_mpu0 23 | /cust_mpu1 24 | /diagnostics 25 | /disparity 26 | /dwa_cmd_vel 27 | /external_trigger 28 | /image_match/markers 29 | /image_match/markers_array 30 | /imu0 31 | /initialpose 32 | /joint_angles 33 | /joint_states 34 | /move_base_simple/goal 35 | /mpu0 36 | /mpu1 37 | /pepper_robot/audio 38 | /pepper_robot/bumper 39 | /pepper_robot/hand_touch 40 | /pepper_robot/head_touch 41 | /pepper_robot/imu/base 42 | /pepper_robot/imu/torso 43 | /pepper_robot/info 44 | /pepper_robot/laser 45 | /pepper_robot/odom 46 | /pepper_robot/pose/body_pose/cancel 47 | /pepper_robot/pose/body_pose/feedback 48 | /pepper_robot/pose/body_pose/goal 49 | /pepper_robot/pose/body_pose/result 50 | /pepper_robot/pose/body_pose/status 51 | /pepper_robot/pose/body_pose_naoqi/cancel 52 | /pepper_robot/pose/body_pose_naoqi/feedback 53 | /pepper_robot/pose/body_pose_naoqi/goal 54 | /pepper_robot/pose/body_pose_naoqi/result 55 | /pepper_robot/pose/body_pose_naoqi/status 56 | /pepper_robot/pose/joint_angles 57 | /pepper_robot/pose/joint_angles_action/cancel 58 | /pepper_robot/pose/joint_angles_action/feedback 59 | /pepper_robot/pose/joint_angles_action/goal 60 | /pepper_robot/pose/joint_angles_action/result 61 | /pepper_robot/pose/joint_angles_action/status 62 | /pepper_robot/pose/joint_stiffness 63 | /pepper_robot/pose/joint_stiffness_trajectory/cancel 64 | /pepper_robot/pose/joint_stiffness_trajectory/feedback 65 | /pepper_robot/pose/joint_stiffness_trajectory/goal 66 | /pepper_robot/pose/joint_stiffness_trajectory/result 67 | /pepper_robot/pose/joint_stiffness_trajectory/status 68 | /pepper_robot/pose/joint_trajectory/cancel 69 | /pepper_robot/pose/joint_trajectory/feedback 70 | /pepper_robot/pose/joint_trajectory/goal 71 | /pepper_robot/pose/joint_trajectory/result 72 | /pepper_robot/pose/joint_trajectory/status 73 | /points2 74 | /rosout 75 | /rosout_agg 76 | /sick_laser_front/parameter_descriptions 77 | /sick_laser_front/parameter_updates 78 | /sick_laser_front/scan 79 | /sick_laser_rear/parameter_descriptions 80 | /sick_laser_rear/parameter_updates 81 | /sick_laser_rear/scan 82 | /speech 83 | /tf 84 | /tf_static 85 | /time_host 86 | /visensor_node/parameter_descriptions 87 | /visensor_node/parameter_updates 88 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_rosbags/misc/topics_pepper_all_sensors.txt: -------------------------------------------------------------------------------- 1 | /cam0/calibration 2 | /cam0/camera_info 3 | /cam0/image_raw 4 | /cam1/calibration 5 | /cam1/camera_info 6 | /cam1/image_raw 7 | /clicked_point 8 | /cmd_vel 9 | /combined_scan 10 | /cust_imu0 11 | /cust_mpu0 12 | /cust_mpu1 13 | /diagnostics 14 | /disparity 15 | /external_trigger 16 | /image_match/markers 17 | /image_match/markers_array 18 | /imu0 19 | /initialpose 20 | /joint_angles 21 | /joint_states 22 | /joy 23 | /map 24 | /map_updates 25 | /move_base_simple/goal 26 | /mpu0 27 | /mpu1 28 | /pepper_robot/audio 29 | /pepper_robot/bumper 30 | /pepper_robot/camera/depth/camera_info 31 | /pepper_robot/camera/depth/image_raw 32 | /pepper_robot/camera/depth_registered/points 33 | /pepper_robot/camera/front/camera_info 34 | /pepper_robot/camera/front/image_raw 35 | /pepper_robot/hand_touch 36 | /pepper_robot/head_touch 37 | /pepper_robot/imu/base 38 | /pepper_robot/imu/torso 39 | /pepper_robot/info 40 | /pepper_robot/laser 41 | /pepper_robot/odom 42 | /pepper_robot/pose/body_pose/cancel 43 | /pepper_robot/pose/body_pose/feedback 44 | /pepper_robot/pose/body_pose/goal 45 | /pepper_robot/pose/body_pose/result 46 | /pepper_robot/pose/body_pose/status 47 | /pepper_robot/pose/body_pose_naoqi/cancel 48 | /pepper_robot/pose/body_pose_naoqi/feedback 49 | /pepper_robot/pose/body_pose_naoqi/goal 50 | /pepper_robot/pose/body_pose_naoqi/result 51 | /pepper_robot/pose/body_pose_naoqi/status 52 | /pepper_robot/pose/joint_angles 53 | /pepper_robot/pose/joint_angles_action/cancel 54 | /pepper_robot/pose/joint_angles_action/feedback 55 | /pepper_robot/pose/joint_angles_action/goal 56 | /pepper_robot/pose/joint_angles_action/result 57 | /pepper_robot/pose/joint_angles_action/status 58 | /pepper_robot/pose/joint_stiffness 59 | /pepper_robot/pose/joint_stiffness_trajectory/cancel 60 | /pepper_robot/pose/joint_stiffness_trajectory/feedback 61 | /pepper_robot/pose/joint_stiffness_trajectory/goal 62 | /pepper_robot/pose/joint_stiffness_trajectory/result 63 | /pepper_robot/pose/joint_stiffness_trajectory/status 64 | /pepper_robot/pose/joint_trajectory/cancel 65 | /pepper_robot/pose/joint_trajectory/feedback 66 | /pepper_robot/pose/joint_trajectory/goal 67 | /pepper_robot/pose/joint_trajectory/result 68 | /pepper_robot/pose/joint_trajectory/status 69 | /points2 70 | /rosout 71 | /rosout_agg 72 | /sick_laser_front/cloud 73 | /sick_laser_front/parameter_descriptions 74 | /sick_laser_front/parameter_updates 75 | /sick_laser_front/scan 76 | /sick_laser_rear/cloud 77 | /sick_laser_rear/parameter_descriptions 78 | /sick_laser_rear/parameter_updates 79 | /sick_laser_rear/scan 80 | /speech 81 | /stereo_image_proc/parameter_descriptions 82 | /stereo_image_proc/parameter_updates 83 | /stereo_image_proc_debayer_left/parameter_descriptions 84 | /stereo_image_proc_debayer_left/parameter_updates 85 | /stereo_image_proc_debayer_right/parameter_descriptions 86 | /stereo_image_proc_debayer_right/parameter_updates 87 | /stereo_image_proc_rectify_color_left/parameter_descriptions 88 | /stereo_image_proc_rectify_color_left/parameter_updates 89 | /stereo_image_proc_rectify_color_right/parameter_descriptions 90 | /stereo_image_proc_rectify_color_right/parameter_updates 91 | /stereo_image_proc_rectify_mono_left/parameter_descriptions 92 | /stereo_image_proc_rectify_mono_left/parameter_updates 93 | /stereo_image_proc_rectify_mono_right/parameter_descriptions 94 | /stereo_image_proc_rectify_mono_right/parameter_updates 95 | /tf 96 | /tf_static 97 | /time_host 98 | /visensor_node/parameter_descriptions 99 | /visensor_node/parameter_updates 100 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_rosbags/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | asl_pepper_rosbags 5 | 0.0.1 6 | WIP 7 | 8 | Daniel Dugas 9 | 10 | Daniel Dugas 11 | 12 | BSD 13 | 14 | catkin 15 | catkin_simple 16 | 17 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_sensor_preprocessing/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(asl_pepper_sensor_preprocessing) 3 | 4 | add_definitions(-std=c++11) 5 | 6 | find_package(catkin_simple 0.1.0 REQUIRED) 7 | catkin_simple(ALL_DEPS_REQUIRED) 8 | 9 | cs_add_executable(combine_laser_scans src/combine_laser_scans.cpp) 10 | cs_add_executable(crop_laser_scan src/crop_laser_scan.cpp) 11 | 12 | #add_doxygen(REQUIRED) 13 | 14 | cs_install() 15 | cs_export() 16 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_sensor_preprocessing/README.md: -------------------------------------------------------------------------------- 1 | Launch files to activate sensor preprocessing for pepper 2 | 3 | depth_to_pc2.launch : Launches depth_image_proc nodelets to convert depth image to PointCloud2 XYZ cloud. 4 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_sensor_preprocessing/launch/combine_laser_scans.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_sensor_preprocessing/launch/crop_laser_scans.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_sensor_preprocessing/launch/depth_to_pc2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 11 | 13 | 14 | 15 | 16 | 17 | 18 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_sensor_preprocessing/launch/filter_laser_scans.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | 10 | 11 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_sensor_preprocessing/launch/front_laser_filters.yaml: -------------------------------------------------------------------------------- 1 | scan_filter_chain: 2 | - name: shadows 3 | type: laser_filters/ScanShadowsFilter 4 | params: 5 | min_angle: 5 6 | max_angle: 175 7 | neighbors: 5 8 | window: 1 9 | - name: angle 10 | type: laser_filters/LaserScanAngularBoundsFilter 11 | params: 12 | lower_angle: -1.82 13 | upper_angle: 1.82 14 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_sensor_preprocessing/launch/rear_laser_filters.yaml: -------------------------------------------------------------------------------- 1 | scan_filter_chain: 2 | - name: shadows 3 | type: laser_filters/ScanShadowsFilter 4 | params: 5 | min_angle: 5 6 | max_angle: 175 7 | neighbors: 5 8 | window: 1 9 | - name: angle 10 | type: laser_filters/LaserScanAngularBoundsFilter 11 | params: 12 | lower_angle: -2.12 13 | upper_angle: 2.12 14 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_sensor_preprocessing/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | asl_pepper_sensor_preprocessing 5 | 0.0.1 6 | WIP 7 | 8 | Daniel Dugas 9 | 10 | Daniel Dugas 11 | 12 | BSD 13 | 14 | catkin 15 | catkin_simple 16 | laser_geometry 17 | glog_catkin 18 | roscpp 19 | tf 20 | pcl_ros 21 | 22 | -------------------------------------------------------------------------------- /external/asl_pepper/asl_pepper_sensor_preprocessing/src/crop_laser_scan.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | namespace crop_laser_scan { 15 | 16 | 17 | class LaserScanCropper { 18 | 19 | public: 20 | explicit LaserScanCropper(ros::NodeHandle& n) : nh_(n) { 21 | // Topic names. 22 | const std::string kScanTopic = "scan"; 23 | const std::string kCroppedScanTopic = "cropped_scan"; 24 | 25 | // Publishers and subscribers. 26 | scan_sub_ = nh_.subscribe(kScanTopic, 1000, &LaserScanCropper::scanCallback, this); 27 | cropped_scan_pub_ = nh_.advertise(kCroppedScanTopic, 1); 28 | 29 | nh_.getParam("crop_angle_min", kScanCropAngleMin); 30 | nh_.getParam("crop_angle_max", kScanCropAngleMax); 31 | } 32 | ~LaserScanCropper() {} 33 | 34 | protected: 35 | /// \brief receives scan 1 messages 36 | void scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg) { 37 | VLOG(3) << "scancallback"; 38 | VLOG(5) << msg->header.frame_id; 39 | 40 | // Find the index of the first and last points within angle crop 41 | double first_scan_index_after_anglemin = (kScanCropAngleMin - msg->angle_min) / msg->angle_increment ; 42 | double last_scan_index_before_anglemax = (kScanCropAngleMax - msg->angle_min) / msg->angle_increment ; 43 | if ( first_scan_index_after_anglemin < 0 || last_scan_index_before_anglemax < 0 ) { 44 | LOG(ERROR) << "Angle index should not have negative value:" 45 | << first_scan_index_after_anglemin << " " << last_scan_index_before_anglemax << std::endl 46 | << "angle_increment: " << msg->angle_increment << ", angle_min: " << msg->angle_min 47 | << ", kScanCropAngleMin: " << kScanCropAngleMin 48 | << ", kScanCropAngleMax: " << kScanCropAngleMax; 49 | } 50 | size_t i_first = std::max(0., std::ceil(first_scan_index_after_anglemin)); 51 | size_t i_last = std::min(std::floor(last_scan_index_before_anglemax), msg->ranges.size() - 1.); 52 | size_t cropped_len = i_last - i_first + 1; 53 | // Angles 54 | double angle_first = msg->angle_min + i_first * msg->angle_increment; 55 | double angle_last = angle_first + (cropped_len - 1.) * msg->angle_increment; 56 | 57 | // Generate the scan to fill in. 58 | sensor_msgs::LaserScan cropped_scan; 59 | cropped_scan.header = msg->header; 60 | cropped_scan.angle_increment = msg->angle_increment; 61 | cropped_scan.time_increment = msg->time_increment; 62 | cropped_scan.scan_time = msg->scan_time; 63 | cropped_scan.range_min = msg->range_min; 64 | cropped_scan.range_max = msg->range_max; 65 | cropped_scan.angle_min = angle_first; 66 | cropped_scan.angle_max = angle_last; 67 | cropped_scan.ranges.resize(cropped_len); 68 | cropped_scan.intensities.resize(cropped_len); 69 | 70 | // Fill in ranges. 71 | for ( size_t j = 0; j < cropped_len; j++ ) { 72 | size_t i = j + i_first; 73 | cropped_scan.ranges.at(j) = msg->ranges.at(i); 74 | } 75 | 76 | // Fill in intensities. if no intensities, spoof intensities 77 | if ( msg->intensities.size() == 0 ) { 78 | for ( size_t j = 0; j < cropped_len; j++ ) { 79 | size_t i = j + i_first; 80 | cropped_scan.intensities.at(j) = 1.0; 81 | } 82 | } else { 83 | for ( size_t j = 0; j < cropped_len; j++ ) { 84 | size_t i = j + i_first; 85 | cropped_scan.intensities.at(j) = msg->intensities.at(i); 86 | } 87 | } 88 | 89 | 90 | VLOG(3) << ""; 91 | // publish result. 92 | cropped_scan_pub_.publish(cropped_scan); 93 | } 94 | 95 | 96 | private: 97 | // ROS 98 | ros::NodeHandle& nh_; 99 | ros::Subscriber scan_sub_; 100 | ros::Publisher cropped_scan_pub_; 101 | // Params 102 | double kScanCropAngleMin; // = -1.82; 103 | double kScanCropAngleMax; // = 1.87; 104 | 105 | }; // class LaserScanCropper 106 | 107 | } // namespace crop_laser_scan 108 | 109 | using namespace crop_laser_scan; 110 | 111 | int main(int argc, char **argv) { 112 | 113 | ros::init(argc, argv, "crop_laser_scan"); 114 | ros::NodeHandle n("~"); // private node handle (~ gets replaced with node name) 115 | LaserScanCropper laser_scan_cropper(n); 116 | 117 | try { 118 | // ros::MultiThreadedSpinner spinner(2); // Necessary to allow concurrent callbacks. 119 | // spinner.spin(); 120 | ros::spin(); 121 | } 122 | catch (const std::exception& e) { 123 | ROS_ERROR_STREAM("Exception: " << e.what()); 124 | return 1; 125 | } 126 | catch (...) { 127 | ROS_ERROR_STREAM("Unknown Exception."); 128 | return 1; 129 | } 130 | 131 | return 0; 132 | } 133 | 134 | -------------------------------------------------------------------------------- /external/asl_pepper/dependencies.rosinstall: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: eigen_catkin 3 | uri: https://github.com/ethz-asl/eigen_catkin.git 4 | version: master 5 | - git: 6 | local-name: glog_catkin 7 | uri: https://github.com/ethz-asl/glog_catkin.git 8 | version: master 9 | - git: 10 | local-name: catkin_simple 11 | uri: https://github.com/catkin/catkin_simple.git 12 | version: master 13 | - git: 14 | local-name: shlopenslam_gmapping 15 | uri: https://github.com/danieldugas/openslam_gmapping.git 16 | version: shlopenslam 17 | - git: 18 | local-name: slam_gmapping 19 | uri: https://github.com/danieldugas/slam_gmapping.git 20 | version: shlopenslam 21 | - git: 22 | local-name: opencv3_catkin 23 | uri: https://github.com/ethz-asl/opencv3_catkin.git 24 | version: master 25 | - git: 26 | local-name: naoqi_driver 27 | uri: https://github.com/danieldugas/naoqi_driver.git 28 | version: devel 29 | - git: 30 | local-name: naoqi_bridge_msgs 31 | uri: https://github.com/ros-naoqi/naoqi_bridge_msgs.git 32 | version: master 33 | - git: 34 | local-name: libvisensor_devel 35 | uri: git@github.com:ethz-asl/libvisensor_devel.git 36 | version: master 37 | - git: 38 | local-name: visensor_node_devel 39 | uri: git@github.com:ethz-asl/visensor_node_devel.git 40 | version: master 41 | - git: 42 | local-name: sick_scan 43 | uri: https://github.com/SICKAG/sick_scan.git 44 | version: master 45 | - git: 46 | local-name: map_matcher 47 | uri: https://github.com/danieldugas/map_matcher.git 48 | version: master 49 | - git: 50 | local-name: ros_run_command 51 | uri: https://github.com/danieldugas/ros_run_command.git 52 | version: master 53 | -------------------------------------------------------------------------------- /external/asl_pepper/maps/asl.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/interaction_actions_for_navigation/e38ca94127ada5a72389c661829cb2fa85db782e/external/asl_pepper/maps/asl.pgm -------------------------------------------------------------------------------- /external/asl_pepper/maps/asl.yaml: -------------------------------------------------------------------------------- 1 | image: asl.pgm 2 | resolution: 0.16000 3 | origin: [-12.8, -12.8, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.15 #0.196 7 | 8 | -------------------------------------------------------------------------------- /external/asl_pepper/maps/asl_office_j.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/interaction_actions_for_navigation/e38ca94127ada5a72389c661829cb2fa85db782e/external/asl_pepper/maps/asl_office_j.pgm -------------------------------------------------------------------------------- /external/asl_pepper/maps/asl_office_j.yaml: -------------------------------------------------------------------------------- 1 | image: asl_office_j.pgm 2 | resolution: 0.020000 3 | origin: [-32.160000, -11.520000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.15 #0.196 7 | 8 | -------------------------------------------------------------------------------- /external/asl_pepper/maps/cla_glass_hall.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/interaction_actions_for_navigation/e38ca94127ada5a72389c661829cb2fa85db782e/external/asl_pepper/maps/cla_glass_hall.pgm -------------------------------------------------------------------------------- /external/asl_pepper/maps/cla_glass_hall.yaml: -------------------------------------------------------------------------------- 1 | image: /home/daniel/maps/cla_glass_hall.pgm 2 | resolution: 0.020000 3 | origin: [-43.920000, -33.680000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /external/asl_pepper/maps/empty.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/interaction_actions_for_navigation/e38ca94127ada5a72389c661829cb2fa85db782e/external/asl_pepper/maps/empty.pgm -------------------------------------------------------------------------------- /external/asl_pepper/maps/empty.yaml: -------------------------------------------------------------------------------- 1 | image: empty.pgm 2 | resolution: 0.0100000 3 | origin: [-5.480000, -5.480000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.15 #0.196 7 | 8 | -------------------------------------------------------------------------------- /external/asl_pepper/maps/hg_main_hall.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/interaction_actions_for_navigation/e38ca94127ada5a72389c661829cb2fa85db782e/external/asl_pepper/maps/hg_main_hall.pgm -------------------------------------------------------------------------------- /external/asl_pepper/maps/hg_main_hall.yaml: -------------------------------------------------------------------------------- 1 | image: hg_main_hall.pgm 2 | resolution: 0.020000 3 | origin: [-31.760000, -10.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /external/asl_pepper/maps/markclutter.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/interaction_actions_for_navigation/e38ca94127ada5a72389c661829cb2fa85db782e/external/asl_pepper/maps/markclutter.pgm -------------------------------------------------------------------------------- /external/asl_pepper/maps/markclutter.yaml: -------------------------------------------------------------------------------- 1 | image: markclutter.pgm 2 | resolution: 0.046 3 | origin: [-8.5, -8.5, 0.000000] 4 | # resolution: 0.027 5 | # origin: [-5., -5., 0.000000] 6 | negate: 0 7 | occupied_thresh: 0.65 8 | free_thresh: 0.15 #0.196 9 | -------------------------------------------------------------------------------- /external/asl_pepper/maps/markcomplex.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/interaction_actions_for_navigation/e38ca94127ada5a72389c661829cb2fa85db782e/external/asl_pepper/maps/markcomplex.pgm -------------------------------------------------------------------------------- /external/asl_pepper/maps/markcomplex.yaml: -------------------------------------------------------------------------------- 1 | image: markcomplex.pgm 2 | resolution: 0.017 # 1.7x bigger as robot radius larger 3 | origin: [-12, -6, 0.] 4 | # resolution: 0.01 5 | # origin: [-7., -3.5, 0.] 6 | negate: 0 7 | occupied_thresh: 0.65 8 | free_thresh: 0.15 #0.196 9 | 10 | -------------------------------------------------------------------------------- /external/asl_pepper/maps/markmaze.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/interaction_actions_for_navigation/e38ca94127ada5a72389c661829cb2fa85db782e/external/asl_pepper/maps/markmaze.pgm -------------------------------------------------------------------------------- /external/asl_pepper/maps/markmaze.yaml: -------------------------------------------------------------------------------- 1 | image: markmaze.pgm 2 | resolution: 0.046 3 | origin: [-8.5, -8.5, 0.000000] 4 | # resolution: 0.027 5 | # origin: [-5., -5., 0.000000] 6 | negate: 0 7 | occupied_thresh: 0.65 8 | free_thresh: 0.15 #0.196 9 | -------------------------------------------------------------------------------- /external/asl_pepper/maps/marksimple.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/interaction_actions_for_navigation/e38ca94127ada5a72389c661829cb2fa85db782e/external/asl_pepper/maps/marksimple.pgm -------------------------------------------------------------------------------- /external/asl_pepper/maps/marksimple.yaml: -------------------------------------------------------------------------------- 1 | image: marksimple.pgm 2 | resolution: 0.017 3 | origin: [-8.5, -8.5, 0.000000] 4 | # resolution: 0.01 5 | # origin: [-5., -5., 0.] 6 | negate: 0 7 | occupied_thresh: 0.65 8 | free_thresh: 0.15 #0.196 9 | 10 | -------------------------------------------------------------------------------- /external/asl_pepper/maps/marktm1.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/interaction_actions_for_navigation/e38ca94127ada5a72389c661829cb2fa85db782e/external/asl_pepper/maps/marktm1.pgm -------------------------------------------------------------------------------- /external/asl_pepper/maps/marktm1.yaml: -------------------------------------------------------------------------------- 1 | image: marktm1.pgm 2 | resolution: 0.0170000 3 | origin: [-8.5, -8.5, 0.000000] 4 | # resolution: 0.01 5 | # origin: [-5., -5., 0.] 6 | negate: 0 7 | occupied_thresh: 0.65 8 | free_thresh: 0.15 #0.196 9 | 10 | -------------------------------------------------------------------------------- /external/asl_pepper/maps/marktm2.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/interaction_actions_for_navigation/e38ca94127ada5a72389c661829cb2fa85db782e/external/asl_pepper/maps/marktm2.pgm -------------------------------------------------------------------------------- /external/asl_pepper/maps/marktm2.yaml: -------------------------------------------------------------------------------- 1 | image: marktm2.pgm 2 | resolution: 0.0170000 3 | origin: [-8.5, -8.5, 0.000000] 4 | # resolution: 0.01 5 | # origin: [-5., -5., 0.] 6 | negate: 0 7 | occupied_thresh: 0.65 8 | free_thresh: 0.15 #0.196 9 | 10 | -------------------------------------------------------------------------------- /external/asl_pepper/maps/marktm3.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/interaction_actions_for_navigation/e38ca94127ada5a72389c661829cb2fa85db782e/external/asl_pepper/maps/marktm3.pgm -------------------------------------------------------------------------------- /external/asl_pepper/maps/marktm3.yaml: -------------------------------------------------------------------------------- 1 | image: marktm3.pgm 2 | resolution: 0.0170000 3 | origin: [-8.5, -6.5, 0.000000] 4 | # resolution: 0.01 5 | # origin: [-5., -5., 0.] 6 | negate: 0 7 | occupied_thresh: 0.65 8 | free_thresh: 0.15 #0.196 9 | 10 | -------------------------------------------------------------------------------- /external/asl_pepper/maps/marktrain.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/interaction_actions_for_navigation/e38ca94127ada5a72389c661829cb2fa85db782e/external/asl_pepper/maps/marktrain.pgm -------------------------------------------------------------------------------- /external/asl_pepper/maps/marktrain.yaml: -------------------------------------------------------------------------------- 1 | image: marktrain.pgm 2 | resolution: 0.0170000 3 | origin: [-3.5, -8.5, 0.000000] 4 | # resolution: 0.0100000 5 | # origin: [-2., -5., 0.000000] 6 | negate: 0 7 | occupied_thresh: 0.65 8 | free_thresh: 0.15 #0.196 9 | 10 | -------------------------------------------------------------------------------- /external/asl_pepper/maps/openlab_office_j.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/interaction_actions_for_navigation/e38ca94127ada5a72389c661829cb2fa85db782e/external/asl_pepper/maps/openlab_office_j.pgm -------------------------------------------------------------------------------- /external/asl_pepper/maps/openlab_office_j.yaml: -------------------------------------------------------------------------------- 1 | image: openlab_office_j.pgm 2 | resolution: 0.020000 3 | origin: [-22.160000, -59.920000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /external/asl_pepper/maps/original_ckk.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/interaction_actions_for_navigation/e38ca94127ada5a72389c661829cb2fa85db782e/external/asl_pepper/maps/original_ckk.pgm -------------------------------------------------------------------------------- /external/asl_pepper/maps/original_ckk.yaml: -------------------------------------------------------------------------------- 1 | image: original_ckk.pgm 2 | resolution: 0.020000 3 | origin: [-10.000000, -10.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /external/asl_pepper/maps/scrubbed_ckk.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/interaction_actions_for_navigation/e38ca94127ada5a72389c661829cb2fa85db782e/external/asl_pepper/maps/scrubbed_ckk.pgm -------------------------------------------------------------------------------- /external/asl_pepper/maps/scrubbed_ckk.yaml: -------------------------------------------------------------------------------- 1 | image: scrubbed_ckk.pgm 2 | resolution: 0.020000 3 | origin: [-10.000000, -10.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /external/asl_pepper/maps/unity_scene_map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/interaction_actions_for_navigation/e38ca94127ada5a72389c661829cb2fa85db782e/external/asl_pepper/maps/unity_scene_map.pgm -------------------------------------------------------------------------------- /external/asl_pepper/maps/unity_scene_map.yaml: -------------------------------------------------------------------------------- 1 | image: unity_scene_map.pgm 2 | resolution: 0.026 3 | origin: [-23.0, -12.0, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /external/asl_pepper/rviz/persistent_settings: -------------------------------------------------------------------------------- 1 | Last Config Dir: /home/daniel/.rviz 2 | Last Image Dir: "" 3 | Recent Configs: 4 | - /home/daniel/.rviz/rvo.rviz 5 | - /home/daniel/.rviz/rds.rviz 6 | - /home/daniel/.rviz/localization.rviz 7 | - compare_maps.rviz 8 | - /home/daniel/.rviz/default.rviz 9 | - /home/daniel/.rviz/localization_scrub.rviz 10 | - /home/daniel/.rviz/iros_navrep.rviz 11 | - /home/daniel/.rviz/iros.rviz 12 | - /home/daniel/.rviz/vae.rviz 13 | - /home/daniel/.rviz/ia.rviz 14 | -------------------------------------------------------------------------------- /external/frame_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(frame_msgs) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | message_generation 10 | std_msgs 11 | geometry_msgs 12 | ) 13 | 14 | ################################################ 15 | ## Declare ROS messages, services and actions ## 16 | ################################################ 17 | 18 | ## Generate messages in the 'msg' folder 19 | add_message_files( 20 | FILES 21 | DetectedPerson.msg 22 | DetectedPersons.msg 23 | TrackedPerson.msg 24 | TrackedPersons.msg 25 | TrackedPerson2d.msg 26 | TrackedPersons2d.msg 27 | PersonTrajectoryEntry.msg 28 | PersonTrajectory.msg 29 | PersonTrajectories.msg 30 | ) 31 | 32 | ## Generate added messages and services with any dependencies listed here 33 | generate_messages( 34 | DEPENDENCIES 35 | std_msgs 36 | geometry_msgs 37 | ) 38 | 39 | ################################### 40 | ## catkin specific configuration ## 41 | ################################### 42 | ## The catkin_package macro generates cmake config files for your package 43 | catkin_package( 44 | CATKIN_DEPENDS message_runtime 45 | ) 46 | 47 | 48 | ########### 49 | ## Build ## 50 | ########### 51 | 52 | ## Specify additional locations of header files 53 | include_directories( 54 | ${catkin_INCLUDE_DIRS} 55 | ) 56 | -------------------------------------------------------------------------------- /external/frame_msgs/msg/DetectedPerson.msg: -------------------------------------------------------------------------------- 1 | # Message describing a detection of a person 2 | # 3 | 4 | # Unique id of the detection, monotonically increasing over time 5 | uint64 detection_id 6 | 7 | # (Pseudo-)probabilistic value between 0.0 and 1.0 describing the detector's confidence in the detection 8 | float64 confidence 9 | 10 | # 3D pose (position + orientation) of the *center* of the detection 11 | # check covariance to see which dimensions are actually set! unset dimensions shall have a covariance > 9999 12 | geometry_msgs/PoseWithCovariance pose 13 | 14 | # height of detected person in 3D (in meters) 15 | float64 height 16 | 17 | # 2D bbox position (x and y is the top-left corner, width, height) 18 | float64 bbox_x 19 | float64 bbox_y 20 | float64 bbox_w 21 | float64 bbox_h 22 | 23 | # Sensor modality / detector type, see example constants below. 24 | # used e.g. later in tracking to check which tracks have been visually confirmed 25 | string modality 26 | 27 | 28 | string MODALITY_GENERIC_LASER_2D = laser2d 29 | string MODALITY_GENERIC_LASER_3D = laser3d 30 | string MODALITY_GENERIC_MONOCULAR_VISION = mono 31 | string MODALITY_GENERIC_STEREO_VISION = stereo 32 | string MODALITY_GENERIC_RGBD = rgbd 33 | 34 | # ReID output 35 | float32[] embed_vector 36 | -------------------------------------------------------------------------------- /external/frame_msgs/msg/DetectedPersons.msg: -------------------------------------------------------------------------------- 1 | # Message with all currently detected persons 2 | # 3 | 4 | Header header # Header containing timestamp etc. of this message 5 | DetectedPerson[] detections # All persons that are currently being detected -------------------------------------------------------------------------------- /external/frame_msgs/msg/PersonTrajectories.msg: -------------------------------------------------------------------------------- 1 | # Message with all currently tracked person trajectories 2 | # 3 | 4 | Header header # Header containing timestamp etc. of this message 5 | PersonTrajectory[] trajectories # All states of the last T frames of the most likely trajectory of that tracked person. 6 | -------------------------------------------------------------------------------- /external/frame_msgs/msg/PersonTrajectory.msg: -------------------------------------------------------------------------------- 1 | # Message defining the trajectory of a tracked person. 2 | # 3 | # The distinction between track and trajectory is that, depending on the 4 | # implementation of the tracker, a single track (i.e. tracked person) might 5 | # change the trajectory if at some point a new trajectory "fits" that track (person) 6 | # better. 7 | # 8 | 9 | uint64 track_id # Unique identifier of the tracked person. 10 | PersonTrajectoryEntry[] trajectory # All states of the last T frames of the most likely trajectory of that tracked person. 11 | -------------------------------------------------------------------------------- /external/frame_msgs/msg/PersonTrajectoryEntry.msg: -------------------------------------------------------------------------------- 1 | # Message defining an entry of a person trajectory. 2 | # 3 | 4 | duration age # age of the track 5 | bool is_occluded # if the track is currently not matched by a detection 6 | uint64 detection_id # id of the corresponding detection in the current cycle (undefined if occluded) 7 | 8 | # The following fields are extracted from the Kalman state x and its covariance C 9 | 10 | geometry_msgs/PoseWithCovariance pose # pose of the track (z value and orientation might not be set, check if corresponding variance on diagonal is > 99999) 11 | geometry_msgs/TwistWithCovariance twist # velocity of the track (z value and rotational velocities might not be set, check if corresponding variance on diagonal is > 99999) 12 | -------------------------------------------------------------------------------- /external/frame_msgs/msg/TrackedPerson.msg: -------------------------------------------------------------------------------- 1 | # Message defining a tracked person 2 | # 3 | 4 | uint64 track_id # unique identifier of the target, consistent over time 5 | bool is_occluded # if the track is currently not observable in a physical way 6 | bool is_matched # if the track is currently matched by a detection 7 | uint64 detection_id # id of the corresponding detection in the current cycle (undefined if occluded) 8 | duration age # age of the track 9 | 10 | # height of tracked person in 3D (in meters) 11 | float64 height 12 | 13 | # The following fields are extracted from the Kalman state x and its covariance C 14 | 15 | geometry_msgs/PoseWithCovariance pose # pose of the track (z value and orientation might not be set, check if corresponding variance on diagonal is > 99999) 16 | 17 | geometry_msgs/TwistWithCovariance twist # velocity of the track (z value and rotational velocities might not be set, check if corresponding variance on diagonal is > 99999) 18 | 19 | # ReID output 20 | float32[] embed_vector 21 | -------------------------------------------------------------------------------- /external/frame_msgs/msg/TrackedPerson2d.msg: -------------------------------------------------------------------------------- 1 | # Message defining a 2d image bbox of a tracked person 2 | # 3 | 4 | uint64 track_id # unique identifier of the target, consistent over time 5 | float32 person_height # 3d height of person in m 6 | int32 x # top left corner x of 2d image bbox 7 | int32 y # top left corner y of 2d image bbox 8 | uint32 w # width of 2d image bbox 9 | uint32 h # height of 2d image bbox 10 | float32 depth # distance from the camera in m 11 | -------------------------------------------------------------------------------- /external/frame_msgs/msg/TrackedPersons.msg: -------------------------------------------------------------------------------- 1 | # Message with all currently tracked persons 2 | # 3 | 4 | Header header # Header containing timestamp etc. of this message 5 | TrackedPerson[] tracks # All persons that are currently being tracked -------------------------------------------------------------------------------- /external/frame_msgs/msg/TrackedPersons2d.msg: -------------------------------------------------------------------------------- 1 | # Message with all 2d bbox in image of currently tracked persons 2 | # 3 | 4 | Header header # Header containing timestamp etc. of this message 5 | TrackedPerson2d[] boxes # All persons that are currently being tracked (2d image bbox) 6 | -------------------------------------------------------------------------------- /external/frame_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | frame_msgs 4 | 0.0.1 5 | Messages for FRAME project 6 | 7 | Stefan Breuers 8 | Tim van der Grinten 9 | 10 | GPLv3 11 | 12 | Stefan Breuers 13 | Tim van der Grinten 14 | 15 | catkin 16 | message_generation 17 | roscpp 18 | std_msgs 19 | geometry_msgs 20 | 21 | message_runtime 22 | roscpp 23 | std_msgs 24 | geometry_msgs 25 | 26 | -------------------------------------------------------------------------------- /ia_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ia_ros) 3 | find_package(catkin_simple REQUIRED) 4 | catkin_simple() 5 | cs_install() 6 | cs_install_scripts( 7 | scripts/ros_ia_node 8 | scripts/ros_sim_node 9 | scripts/ia_dashboard 10 | ) 11 | -------------------------------------------------------------------------------- /ia_ros/launch/auto_ros_ia_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 36 | 37 | 38 | 39 | 40 | 41 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | -------------------------------------------------------------------------------- /ia_ros/launch/example_move_base.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /ia_ros/launch/example_move_base_no_slam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /ia_ros/launch/example_real_map_sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /ia_ros/launch/example_sim_and_planner.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 42 | 43 | 44 | 45 | 46 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /ia_ros/launch/live_ros_ia_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | -------------------------------------------------------------------------------- /ia_ros/launch/ros_sim_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /ia_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ia_ros 4 | 0.0.1 5 | interaction-actions planner for ROS 6 | Daniel Dugas 7 | Daniel Dugas 8 | MIT 9 | 10 | catkin 11 | catkin_simple 12 | 13 | ros_run_command 14 | 15 | 16 | -------------------------------------------------------------------------------- /ia_ros/scripts/TODO.txt: -------------------------------------------------------------------------------- 1 | IA state from worldstate 2 | IA valid actions 3 | multiple versions of Intend for multiple goals 4 | find reasonable paths 5 | IA tree generation 6 | 7 | 8 | flesh out actions 9 | Intend 10 | Say 11 | Nudge 12 | Crawl 13 | 14 | Add Interaction in Simulator 15 | Permissivity? 16 | Perceptivity? 17 | Add agents with IA to simulator 18 | 19 | MILESTONE: planner running in simulator shows attempt at passing non-permissive area, replans around it 20 | -------------------------------------------------------------------------------- /ia_ros/scripts/ros_ia_node: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from ros_ia_node import IAPlanningNode 3 | from pepper_2d_iarlenv import parse_iaenv_args 4 | 5 | if __name__ == "__main__": 6 | args = parse_iaenv_args() 7 | planner = IAPlanningNode(args) 8 | 9 | -------------------------------------------------------------------------------- /ia_ros/scripts/ros_sim_node: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from pepper_2d_iarlenv import parse_iaenv_args, IARLEnv, check_iaenv_args 3 | 4 | if __name__ == '__main__': 5 | # args 6 | args = parse_iaenv_args() 7 | args.unmerged_scans = 1 8 | args.continuous = True 9 | check_iaenv_args(args) 10 | # env 11 | iarlenv = IARLEnv(args) 12 | iarlenv.run() 13 | -------------------------------------------------------------------------------- /install_ros.sh: -------------------------------------------------------------------------------- 1 | sudo apt install -y curl 2 | sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' 3 | sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 4 | curl -sSL 'http://keyserver.ubuntu.com/pks/lookup?op=get&search=0xC1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' | sudo apt-key add - 5 | sudo apt update 6 | sudo apt update 7 | sudo apt install -y ros-melodic-desktop-full 8 | -------------------------------------------------------------------------------- /media/ian_iros_thumbnail.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/interaction_actions_for_navigation/e38ca94127ada5a72389c661829cb2fa85db782e/media/ian_iros_thumbnail.png -------------------------------------------------------------------------------- /media/ian_toy_example.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/interaction_actions_for_navigation/e38ca94127ada5a72389c661829cb2fa85db782e/media/ian_toy_example.png -------------------------------------------------------------------------------- /python/cIA/setup.py: -------------------------------------------------------------------------------- 1 | from distutils.core import setup 2 | from Cython.Build import cythonize 3 | import os 4 | import numpy 5 | 6 | 7 | setup( 8 | name="cIA", 9 | description='Cythonized methods for pyIA', 10 | author='Daniel Dugas', 11 | version='1.0', 12 | py_modules=[], 13 | ext_modules = cythonize("cia.pyx", annotate=True), 14 | include_dirs=[numpy.get_include()], 15 | ) 16 | -------------------------------------------------------------------------------- /python/examples/test_ia.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | from timeit import default_timer as timer 3 | import matplotlib.pyplot as plt 4 | import traceback 5 | 6 | from pyIA import ia_planning 7 | from pyIA.arguments import parse_sim_args 8 | from pyIA.simulator import Environment 9 | from pyIA import state_estimation 10 | 11 | import numpy as np 12 | np.random.seed(1) 13 | 14 | args = parse_sim_args() 15 | env = Environment(args) 16 | env.populate() 17 | 18 | agent_index = 0 19 | worldstate = env.get_worldstate() 20 | possible_actions = worldstate.get_agents()[agent_index].actions 21 | 22 | # ----------------- PROCESSING ------------------------------------------------------------------- 23 | tic = timer() 24 | state_e, fixed_state = state_estimation.state_estimate_from_sim_worldstate( 25 | agent_index, worldstate 26 | ) 27 | toc = timer() 28 | print("State Estimation: {:.1f}s".format(toc - tic)) 29 | 30 | tic = timer() 31 | try: 32 | pruned_stochastic_tree = ia_planning.StochasticTree() 33 | optimistic_sequence = [] 34 | firstfailure_sequence = [] 35 | byproducts = {} 36 | pruned_stochastic_tree, optimistic_sequence, firstfailure_sequence = ia_planning.plan( 37 | state_e, 38 | fixed_state, 39 | possible_actions, 40 | n_trials=args.n_trials, 41 | max_depth=args.max_depth, 42 | BYPRODUCTS=byproducts, 43 | ) 44 | except: 45 | traceback.print_exc() 46 | toc = timer() 47 | print("Planning: {:.1f}s ({} trials)".format(toc - tic, args.n_trials)) 48 | # ---------------- PLOTTING ---------------------------------------------------------------------- 49 | if not args.plotless: 50 | try: 51 | ia_planning.visualize_planning_process( 52 | state_e, 53 | fixed_state, 54 | possible_actions, 55 | pruned_stochastic_tree, 56 | optimistic_sequence, 57 | firstfailure_sequence, 58 | byproducts, 59 | env=env, 60 | ) 61 | plt.show() 62 | except: 63 | traceback.print_exc() 64 | -------------------------------------------------------------------------------- /python/examples/test_simulator.py: -------------------------------------------------------------------------------- 1 | from timeit import default_timer as timer 2 | import matplotlib.pyplot as plt 3 | import matplotlib 4 | from pyniel.pyplot_tools.realtime import plotpause, plotshow 5 | from pyniel.pyplot_tools.windows import movefigure 6 | from pyniel.pyplot_tools.colormaps import Greys_c10 7 | from CMap2D import path_from_dijkstra_field 8 | 9 | from pyIA.arguments import parse_sim_args 10 | from pyIA.simulator import Environment, Worldstate 11 | 12 | args = parse_sim_args() 13 | env = Environment(args) 14 | 15 | # Add agents and precompute dijkstras 16 | tic=timer() 17 | env.populate() 18 | toc=timer() 19 | print("Environment populated. ({:.2f}s)".format(toc-tic)) 20 | 21 | # Visuals 22 | # fig = plt.figure("situation") 23 | # movefigure(fig, (100, 100)) 24 | # fig = plt.figure("dijkstra") 25 | # movefigure(fig, (500, 100)) 26 | # fig = plt.figure("density") 27 | # movefigure(fig, (100, 500)) 28 | try: 29 | for i in range(100): 30 | S_w = env.fullstate() 31 | actions = env.policies(S_w) 32 | 33 | worldstate, relstate = S_w 34 | 35 | plt.ion() 36 | plt.figure("situation") 37 | plt.cla() 38 | # plot map and agents 39 | env.plot_map() 40 | env.plot_agents_ij() 41 | # plot samples 42 | x_samples = env.x_samples(worldstate.get_agents()[0], 43 | worldstate.get_innerstates()[0], 44 | worldstate.get_xystates()[0], 45 | relstate[0]) 46 | ij_samples = env.worldstate.map.xy_to_ij(x_samples) 47 | # plt.scatter(ij_samples[:,0], ij_samples[:,1], marker="o", color='g') 48 | # plot goals 49 | env.plot_goals() 50 | 51 | # example of value predicition heuristics for worldstates 52 | # dijkstra-distance 53 | plt.figure("dijkstra") 54 | plt.cla() 55 | cached_ = env.worldstate.derived_state.agent_precomputed_ctg(0, env.worldstate) 56 | gridshow(cached_, cmap=Greys_c10) 57 | path, jumps = path_from_dijkstra_field(cached_, [0,0], connectedness=32) 58 | plt.plot(path[:,0], path[:,1], ':or', mec='red', linewidth=0, mfc=[0,0,0,0]) 59 | # avg-velocity-map-estimation 60 | plt.figure("density") 61 | plt.cla() 62 | density = env.worldstate.derived_state.smooth_density_map(env.worldstate, 4.) 63 | gridshow(density, ) 64 | env.plot_map_contours_ij('k-,') 65 | # ... 66 | 67 | t_horizon=1. 68 | # run action simulation for agent 1 (should be an ORCA agent) 69 | inputs = worldstate.get_agents()[2].actions[0].worldstate_to_inputs(worldstate) 70 | # worldstate.get_agents()[1].actions[0].planner(inputs, t_horizon=t_horizon, DEBUG=True) 71 | 72 | # Run outcomes and update simulation 73 | outcomes, prob = worldstate.get_agents()[2].actions[0].old_predict_outcome(1, worldstate, t_horizon) 74 | new_S_w = outcomes[0] 75 | 76 | plotpause(0.01) 77 | env.worldstate = new_S_w 78 | 79 | if np.linalg.norm(np.array(new_S_w.get_xystates()[0]) - np.array(new_S_w.get_innerstates()[0].goal)) < 0.05: 80 | break 81 | except KeyboardInterrupt: 82 | print("Simulation stopped. (^C again to quit)") 83 | pass 84 | 85 | plotshow() 86 | 87 | -------------------------------------------------------------------------------- /python/maps/asl.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/interaction_actions_for_navigation/e38ca94127ada5a72389c661829cb2fa85db782e/python/maps/asl.pgm -------------------------------------------------------------------------------- /python/maps/asl.yaml: -------------------------------------------------------------------------------- 1 | image: asl.pgm 2 | resolution: 0.16000 3 | origin: [-12.8, -12.8, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.15 #0.196 7 | 8 | -------------------------------------------------------------------------------- /python/maps/corridors.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/interaction_actions_for_navigation/e38ca94127ada5a72389c661829cb2fa85db782e/python/maps/corridors.pgm -------------------------------------------------------------------------------- /python/maps/corridors.yaml: -------------------------------------------------------------------------------- 1 | image: corridors.pgm 2 | resolution: 0.20000 3 | origin: [-12.8, -12.8, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.15 #0.196 7 | 8 | -------------------------------------------------------------------------------- /python/maps/default.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/interaction_actions_for_navigation/e38ca94127ada5a72389c661829cb2fa85db782e/python/maps/default.pgm -------------------------------------------------------------------------------- /python/maps/default.yaml: -------------------------------------------------------------------------------- 1 | image: default.pgm 2 | resolution: 0.20000 3 | origin: [-12.8, -12.8, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.15 #0.196 7 | 8 | -------------------------------------------------------------------------------- /python/pyIA/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/interaction_actions_for_navigation/e38ca94127ada5a72389c661829cb2fa85db782e/python/pyIA/__init__.py -------------------------------------------------------------------------------- /python/pyIA/agents.py: -------------------------------------------------------------------------------- 1 | import pyIA.actions as actions 2 | 3 | def agent_creator(agent_type): 4 | """ Creates an agent based on a type string """ 5 | # print("creating agent of type {}".format(str(agent_type))) 6 | if "Robot" in str(agent_type): 7 | return Robot() 8 | if "Person" in str(agent_type): 9 | return Person() 10 | if "ORCAAgent" in str(agent_type): 11 | return ORCAAgent() 12 | else: 13 | raise ValueError("agent type {} does not exist".format(str(agent_type))) 14 | 15 | ## Agents 16 | class Agent(object): 17 | """ Prototype for agent class """ 18 | def type(self): 19 | return type(self) 20 | 21 | 22 | class Robot(Agent): 23 | def __init__(self): 24 | self.radius = 0.3 25 | self.actions = [ 26 | actions.Intend(), 27 | actions.Say(), 28 | # actions.Crawl(), 29 | actions.Nudge(), 30 | ] 31 | 32 | class Person(Agent): 33 | def __init__(self): 34 | self.radius = 0.3 35 | self.actions = [ 36 | # actions.Disturb(), # targeted 37 | # actions.Intend(), 38 | # actions.Loiter(), 39 | ] 40 | 41 | # NIY or Deprecated ------------------------------ 42 | class MovableObstacle(Agent): 43 | def __init__(self): 44 | self.is_passive = True 45 | 46 | class ORCAAgent(Agent): 47 | """ A cylindrical entity which moves around according to simple ORCA rules """ 48 | def __init__(self): 49 | self.actions = [ 50 | actions.Intend() 51 | ] 52 | self.radius = 0.3 53 | # ------------------------------------------------ 54 | 55 | ## inner states 56 | class Innerstate(object): 57 | def __init__(self): 58 | self.patience = 1 59 | self.permissivity = 0.5 60 | self.perceptivity = 0.9 61 | self.goal = [0., 0.] # TODO: several goals with preference? 62 | self.undertaking = None 63 | 64 | 65 | -------------------------------------------------------------------------------- /python/pyIA/arguments.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import os 3 | 4 | try: 5 | import rospy 6 | ROS_FOUND = True 7 | except ImportError: 8 | ROS_FOUND = False 9 | 10 | 11 | def parse_sim_args(): 12 | # Arguments 13 | parser = argparse.ArgumentParser(description='Test node for the pepper RL path planning algorithm.') 14 | populate_sim_args(parser) 15 | # add external args 16 | # from xxx import parse_xxx_args 17 | # ARGS, unknown = parse_xxx_args(ignore_unknown=True, parser=parser) 18 | ARGS, unknown_args = parser.parse_known_args() 19 | 20 | # deal with unknown arguments 21 | # ROS appends some weird args, ignore those, but not the rest 22 | if unknown_args: 23 | if ROS_FOUND: 24 | non_ros_unknown_args = rospy.myargv(unknown_args) 25 | else: 26 | non_ros_unknown_args = unknown_args 27 | if non_ros_unknown_args: 28 | print("unknown arguments:") 29 | print(non_ros_unknown_args) 30 | parser.parse_args(args=["--help"]) 31 | raise ValueError 32 | 33 | return ARGS 34 | 35 | 36 | def populate_sim_args(parser): 37 | argscriptdir = os.path.dirname(os.path.realpath(__file__)) 38 | parser.add_argument('--map-folder', type=str, default=os.path.join(argscriptdir, "../maps"),) 39 | parser.add_argument('--map-name', type=str, default="asl",) 40 | parser.add_argument('--map-downsampling-passes', type=int, default=0,) 41 | # planning args 42 | parser.add_argument('--n-trials', type=int, default=1000,) 43 | parser.add_argument('--max-depth', type=int, default=10,) 44 | single_action = parser.add_mutually_exclusive_group() 45 | single_action.add_argument('--only-intend', action='store_true',) 46 | single_action.add_argument('--only-say', action='store_true',) 47 | single_action.add_argument('--only-nudge', action='store_true',) 48 | # scenario args 49 | parser.add_argument( 50 | '--scenario-folder', 51 | type=str, 52 | default=os.path.join(argscriptdir, "../scenarios"), 53 | ) 54 | parser.add_argument( 55 | '--scenario', 56 | type=str, 57 | default="aslquiet", 58 | help="""Which named scenario to load from the scenarios/ folder. 59 | The scenario defines initial agents and their state""", 60 | ) 61 | # utilities args 62 | parser.add_argument('--debug', action='store_true',) 63 | parser.add_argument('--plotless', action='store_true',) 64 | -------------------------------------------------------------------------------- /python/pyIA/mdptree.py: -------------------------------------------------------------------------------- 1 | class Node(object): 2 | def __init__(self, worldstate, neighbors, edges, rank, values): 3 | self.worldstate = worldstate 4 | self.neighbors = neighbors 5 | self.edges = edges 6 | self.rank = rank 7 | self.values = values 8 | -------------------------------------------------------------------------------- /python/pyIA/paths.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from pyniel.numpy_tools import indexing 4 | 5 | from cia import kStateFeatures as kSF 6 | 7 | WARMUP_INITIAL_RADIUS = 1. # warmup means the path conditions are relaxed for the first few meters 8 | class Path(object): 9 | def type(self): 10 | return type(self) 11 | 12 | def typestr(self): 13 | return str(self.type()).replace("", '') 14 | 15 | def letterstr(self): 16 | return self.typestr()[0] 17 | 18 | def linestyle(self): 19 | return '-' 20 | def linecolor(self): 21 | return 'black' 22 | 23 | class NaivePath(Path): 24 | def linestyle(self): 25 | return '-' 26 | def linecolor(self): 27 | return 'tab:blue' 28 | def is_state_traversable(self, state, fixed_state): 29 | is_traversable = \ 30 | fixed_state.map.occupancy() < fixed_state.map.thresh_occupied() 31 | return is_traversable 32 | class PermissivePath(Path): 33 | def linestyle(self): 34 | return '-.' 35 | def linecolor(self): 36 | return 'orange' 37 | def is_state_traversable(self, state, fixed_state): 38 | is_traversable = \ 39 | np.logical_and.reduce([ 40 | np.logical_or( # high or unknown permissivity 41 | state.grid_features_values()[kSF["permissivity"]] >= 0.2, # high permissivity 42 | state.grid_features_uncertainties()[kSF["permissivity"]] >= 1., # low confidence 43 | ), 44 | np.logical_or( # low or unknown crowdedness 45 | state.grid_features_values()[kSF["crowdedness"]] <= 1., # low crowdedness 46 | state.grid_features_uncertainties()[kSF["crowdedness"]] >= 1., # low crowdedness 47 | ), 48 | fixed_state.map.occupancy() < fixed_state.map.thresh_occupied() 49 | ]) 50 | return is_traversable 51 | class StrictPermissivePath(Path): 52 | def linestyle(self): 53 | return '--' 54 | def linecolor(self): 55 | return 'tab:red' 56 | def is_state_traversable(self, state, fixed_state): 57 | is_traversable = \ 58 | np.logical_and.reduce([ 59 | np.logical_or( # very high or unknown permissivity 60 | state.grid_features_values()[kSF["permissivity"]] >= 0.8, # high permissivity 61 | state.grid_features_uncertainties()[kSF["permissivity"]] >= 1., # low confidence 62 | ), 63 | np.logical_or( # low or unknown crowdedness 64 | state.grid_features_values()[kSF["crowdedness"]] <= 1., # low crowdedness 65 | state.grid_features_uncertainties()[kSF["crowdedness"]] >= 1., # low crowdedness 66 | ), 67 | fixed_state.map.occupancy() < fixed_state.map.thresh_occupied() 68 | ]) 69 | return is_traversable 70 | class WarmupStrictPermissivePath(Path): 71 | def linestyle(self): 72 | return ':' 73 | def linecolor(self): 74 | return 'tab:purple' 75 | def is_state_traversable(self, state, fixed_state): 76 | sqr_initial_radius_ij = (WARMUP_INITIAL_RADIUS / fixed_state.map.resolution())**2 77 | ij = indexing.as_idx_array(fixed_state.map.occupancy(), axis='all') 78 | ii = ij[...,0] 79 | jj = ij[...,1] 80 | ai, aj = fixed_state.map.xy_to_floatij([state.get_pos()])[0][:2] 81 | is_in_initial_radius = ((ii - ai)**2 + (jj - aj)**2) < sqr_initial_radius_ij 82 | is_traversable = \ 83 | np.logical_and( 84 | np.logical_or( 85 | np.logical_and( 86 | np.logical_or( # very high or unknown permissivity 87 | state.grid_features_values()[kSF["permissivity"]] >= 0.8, # high permissivity 88 | state.grid_features_uncertainties()[kSF["permissivity"]] >= 1., # low confidence 89 | ), 90 | np.logical_or( # low or unknown crowdedness 91 | state.grid_features_values()[kSF["crowdedness"]] <= 1., # low crowdedness 92 | state.grid_features_uncertainties()[kSF["crowdedness"]] >= 1., # low crowdedness 93 | ) 94 | ), 95 | is_in_initial_radius 96 | ), 97 | fixed_state.map.occupancy() < fixed_state.map.thresh_occupied() 98 | ) 99 | return is_traversable 100 | 101 | PATH_VARIANTS = [NaivePath(), PermissivePath(), StrictPermissivePath(), WarmupStrictPermissivePath()] 102 | 103 | def is_path_equivalent(a_path, b_path): 104 | if not np.allclose(np.shape(a_path), np.shape(b_path)): 105 | return False 106 | return np.allclose(a_path, b_path) 107 | 108 | def is_path_in_list(path, list_): 109 | for b_path in list_: 110 | if is_path_equivalent(path, b_path): 111 | return True 112 | return False 113 | -------------------------------------------------------------------------------- /python/pyIA/profile_ia.sh: -------------------------------------------------------------------------------- 1 | python -m cProfile -o /tmp/test_ia.prof test_ia.py --plotless 2 | snakeviz /tmp/test_ia.prof 3 | -------------------------------------------------------------------------------- /python/scenarios/aslquiet.pickle: -------------------------------------------------------------------------------- 1 | (dp0 2 | S'uvstates' 3 | p1 4 | (lp2 5 | (lp3 6 | F0.0 7 | aF0.0 8 | aa(lp4 9 | F0.0 10 | aF0.0 11 | aa(lp5 12 | F0.0 13 | aF0.0 14 | aa(lp6 15 | F0.0 16 | aF0.0 17 | aasS'agents' 18 | p7 19 | (lp8 20 | ccopy_reg 21 | _reconstructor 22 | p9 23 | (cpyIA.agents 24 | Robot 25 | p10 26 | c__builtin__ 27 | object 28 | p11 29 | Ntp12 30 | Rp13 31 | (dp14 32 | S'radius' 33 | p15 34 | F0.3 35 | sS'actions' 36 | p16 37 | (lp17 38 | g9 39 | (cpyIA.actions 40 | Intend 41 | p18 42 | g11 43 | Ntp19 44 | Rp20 45 | ag9 46 | (cpyIA.actions 47 | Say 48 | p21 49 | g11 50 | Ntp22 51 | Rp23 52 | ag9 53 | (cpyIA.actions 54 | Crawl 55 | p24 56 | g11 57 | Ntp25 58 | Rp26 59 | ag9 60 | (cpyIA.actions 61 | Nudge 62 | p27 63 | g11 64 | Ntp28 65 | Rp29 66 | asbag9 67 | (cpyIA.agents 68 | Person 69 | p30 70 | g11 71 | Ntp31 72 | Rp32 73 | (dp33 74 | g15 75 | F0.3 76 | sg16 77 | (lp34 78 | sbag9 79 | (g30 80 | g11 81 | Ntp35 82 | Rp36 83 | (dp37 84 | g15 85 | F0.3 86 | sg16 87 | (lp38 88 | sbag9 89 | (g30 90 | g11 91 | Ntp39 92 | Rp40 93 | (dp41 94 | g15 95 | F0.3 96 | sg16 97 | (lp42 98 | sbasS'innerstates' 99 | p43 100 | (lp44 101 | g9 102 | (cpyIA.agents 103 | Innerstate 104 | p45 105 | g11 106 | Ntp46 107 | Rp47 108 | (dp48 109 | S'permissivity' 110 | p49 111 | F0.5 112 | sS'patience' 113 | p50 114 | I1 115 | sS'goal' 116 | p51 117 | (lp52 118 | cnumpy.core.multiarray 119 | scalar 120 | p53 121 | (cnumpy 122 | dtype 123 | p54 124 | (S'f8' 125 | p55 126 | I0 127 | I1 128 | tp56 129 | Rp57 130 | (I3 131 | S'<' 132 | p58 133 | NNNI-1 134 | I-1 135 | I0 136 | tp59 137 | bS'P\xcf\xf31\xa8H\xff\xbf' 138 | p60 139 | tp61 140 | Rp62 141 | ag53 142 | (g57 143 | S'\x14_Qx\xfa\xf8&\xc0' 144 | p63 145 | tp64 146 | Rp65 147 | asS'undertaking' 148 | p66 149 | Nsbag9 150 | (g45 151 | g11 152 | Ntp67 153 | Rp68 154 | (dp69 155 | g49 156 | F0.5 157 | sg50 158 | I1 159 | sg51 160 | (lp70 161 | g53 162 | (g57 163 | S'\xdcR\x1a_\xe5\xcb\x19\xc0' 164 | p71 165 | tp72 166 | Rp73 167 | ag53 168 | (g57 169 | S'B\xad\n\xf4Sk\x19\xc0' 170 | p74 171 | tp75 172 | Rp76 173 | asg66 174 | Nsbag9 175 | (g45 176 | g11 177 | Ntp77 178 | Rp78 179 | (dp79 180 | g49 181 | F0.5 182 | sg50 183 | I1 184 | sg51 185 | (lp80 186 | g53 187 | (g57 188 | S'0\xce\xc6H\xe52\xf1?' 189 | p81 190 | tp82 191 | Rp83 192 | ag53 193 | (g57 194 | S'\x1e\x02:\xd3$\xcb\x11\xc0' 195 | p84 196 | tp85 197 | Rp86 198 | asg66 199 | Nsbag9 200 | (g45 201 | g11 202 | Ntp87 203 | Rp88 204 | (dp89 205 | g49 206 | F0.5 207 | sg50 208 | I1 209 | sg51 210 | (lp90 211 | g53 212 | (g57 213 | S'\x00\xf9\x101\x034\x1a\xc0' 214 | p91 215 | tp92 216 | Rp93 217 | ag53 218 | (g57 219 | S'>3\xefb\x13\x0c\x15\xc0' 220 | p94 221 | tp95 222 | Rp96 223 | asg66 224 | NsbasS'xystates' 225 | p97 226 | (lp98 227 | (lp99 228 | g53 229 | (g57 230 | S'X\x1e\xde,\r\xd9\x16\xc0' 231 | p100 232 | tp101 233 | Rp102 234 | ag53 235 | (g57 236 | S'@\xc6\xeaD\xbd\xb5\xe2\xbf' 237 | p103 238 | tp104 239 | Rp105 240 | aa(lp106 241 | g53 242 | (g57 243 | S'h\xfc\x97\xd3\xec\xe5\x19\xc0' 244 | p107 245 | tp108 246 | Rp109 247 | ag53 248 | (g57 249 | S'B\xad\n\xf4Sk\x19\xc0' 250 | p110 251 | tp111 252 | Rp112 253 | aa(lp113 254 | g53 255 | (g57 256 | S'0\xce\xc6H\xe52\xf1?' 257 | p114 258 | tp115 259 | Rp116 260 | ag53 261 | (g57 262 | S'\x1e\x02:\xd3$\xcb\x11\xc0' 263 | p117 264 | tp118 265 | Rp119 266 | aa(lp120 267 | g53 268 | (g57 269 | S'\x8c\xa2\x8e\xa5\nN\x1a\xc0' 270 | p121 271 | tp122 272 | Rp123 273 | ag53 274 | (g57 275 | S'>3\xefb\x13\x0c\x15\xc0' 276 | p124 277 | tp125 278 | Rp126 279 | aas. -------------------------------------------------------------------------------- /python/scenarios/asltrap.pickle: -------------------------------------------------------------------------------- 1 | (dp0 2 | S'uvstates' 3 | p1 4 | (lp2 5 | (lp3 6 | F0.0 7 | aF0.0 8 | aa(lp4 9 | F0.0 10 | aF0.0 11 | aasS'agents' 12 | p5 13 | (lp6 14 | ccopy_reg 15 | _reconstructor 16 | p7 17 | (cpyIA.agents 18 | Robot 19 | p8 20 | c__builtin__ 21 | object 22 | p9 23 | Ntp10 24 | Rp11 25 | (dp12 26 | S'radius' 27 | p13 28 | F0.3 29 | sS'actions' 30 | p14 31 | (lp15 32 | g7 33 | (cpyIA.actions 34 | Intend 35 | p16 36 | g9 37 | Ntp17 38 | Rp18 39 | ag7 40 | (cpyIA.actions 41 | Say 42 | p19 43 | g9 44 | Ntp20 45 | Rp21 46 | ag7 47 | (cpyIA.actions 48 | Crawl 49 | p22 50 | g9 51 | Ntp23 52 | Rp24 53 | ag7 54 | (cpyIA.actions 55 | Nudge 56 | p25 57 | g9 58 | Ntp26 59 | Rp27 60 | asbag7 61 | (cpyIA.agents 62 | Person 63 | p28 64 | g9 65 | Ntp29 66 | Rp30 67 | (dp31 68 | g13 69 | F0.3 70 | sg14 71 | (lp32 72 | sbasS'innerstates' 73 | p33 74 | (lp34 75 | g7 76 | (cpyIA.agents 77 | Innerstate 78 | p35 79 | g9 80 | Ntp36 81 | Rp37 82 | (dp38 83 | S'permissivity' 84 | p39 85 | F1.0 86 | sS'patience' 87 | p40 88 | I1 89 | sS'perceptivity' 90 | p41 91 | F0.9 92 | sS'goal' 93 | p42 94 | (lp43 95 | cnumpy.core.multiarray 96 | scalar 97 | p44 98 | (cnumpy 99 | dtype 100 | p45 101 | (S'f8' 102 | p46 103 | I0 104 | I1 105 | tp47 106 | Rp48 107 | (I3 108 | S'<' 109 | p49 110 | NNNI-1 111 | I-1 112 | I0 113 | tp50 114 | bS'\xb8(T\x1e\xd5\xd1\x00\xc0' 115 | p51 116 | tp52 117 | Rp53 118 | ag44 119 | (g48 120 | S"r\x0e\xd8\xa9\x9e\x07'\xc0" 121 | p54 122 | tp55 123 | Rp56 124 | asS'undertaking' 125 | p57 126 | Nsbag7 127 | (g35 128 | g9 129 | Ntp58 130 | Rp59 131 | (dp60 132 | g39 133 | F0.0 134 | sg40 135 | I1 136 | sg41 137 | F0.9 138 | sg42 139 | (lp61 140 | g44 141 | (g48 142 | S'\xc4\x15\x9c\xf1wI\x1f\xc0' 143 | p62 144 | tp63 145 | Rp64 146 | ag44 147 | (g48 148 | S't\xc6\x8b\xa3-\xff!\xc0' 149 | p65 150 | tp66 151 | Rp67 152 | asg57 153 | NsbasS'xystates' 154 | p68 155 | (lp69 156 | (lp70 157 | g44 158 | (g48 159 | S"A\xe1__\xe77'\xc0" 160 | p71 161 | tp72 162 | Rp73 163 | ag44 164 | (g48 165 | S'\xf1N\xdd\xb9\xf5;!\xc0' 166 | p74 167 | tp75 168 | Rp76 169 | aa(lp77 170 | g44 171 | (g48 172 | S'\xc4\x15\x9c\xf1wI\x1f\xc0' 173 | p78 174 | tp79 175 | Rp80 176 | ag44 177 | (g48 178 | S'9\x9b\xca]1\x0c"\xc0' 179 | p81 180 | tp82 181 | Rp83 182 | aas. -------------------------------------------------------------------------------- /python/scenarios/irosasl2.pickle: -------------------------------------------------------------------------------- 1 | (dp0 2 | S'uvstates' 3 | p1 4 | (lp2 5 | (lp3 6 | F0.0 7 | aF0.0 8 | aasS'agents' 9 | p4 10 | (lp5 11 | ccopy_reg 12 | _reconstructor 13 | p6 14 | (cpyIA.agents 15 | Robot 16 | p7 17 | c__builtin__ 18 | object 19 | p8 20 | Ntp9 21 | Rp10 22 | (dp11 23 | S'radius' 24 | p12 25 | F0.3 26 | sS'actions' 27 | p13 28 | (lp14 29 | g6 30 | (cpyIA.actions 31 | Intend 32 | p15 33 | g8 34 | Ntp16 35 | Rp17 36 | ag6 37 | (cpyIA.actions 38 | Say 39 | p18 40 | g8 41 | Ntp19 42 | Rp20 43 | ag6 44 | (cpyIA.actions 45 | Nudge 46 | p21 47 | g8 48 | Ntp22 49 | Rp23 50 | asbasS'innerstates' 51 | p24 52 | (lp25 53 | g6 54 | (cpyIA.agents 55 | Innerstate 56 | p26 57 | g8 58 | Ntp27 59 | Rp28 60 | (dp29 61 | S'permissivity' 62 | p30 63 | F0.5 64 | sS'patience' 65 | p31 66 | I1 67 | sS'perceptivity' 68 | p32 69 | F0.9 70 | sS'goal' 71 | p33 72 | (lp34 73 | cnumpy.core.multiarray 74 | scalar 75 | p35 76 | (cnumpy 77 | dtype 78 | p36 79 | (S'f8' 80 | p37 81 | I0 82 | I1 83 | tp38 84 | Rp39 85 | (I3 86 | S'<' 87 | p40 88 | NNNI-1 89 | I-1 90 | I0 91 | tp41 92 | bS'\x9c\xe7\x99\xf4]{\x14@' 93 | p42 94 | tp43 95 | Rp44 96 | ag35 97 | (g39 98 | S'\xd8A=gH\x18\x14@' 99 | p45 100 | tp46 101 | Rp47 102 | asS'undertaking' 103 | p48 104 | NsbasS'xystates' 105 | p49 106 | (lp50 107 | (lp51 108 | g35 109 | (g39 110 | S'p\xdb\xa62\r\xb9$\xc0' 111 | p52 112 | tp53 113 | Rp54 114 | ag35 115 | (g39 116 | S'\xa7\x83\x1a\xd4\xd8\x11$\xc0' 117 | p55 118 | tp56 119 | Rp57 120 | aas. -------------------------------------------------------------------------------- /python/scenarios/irosasl_office_j2.pickle: -------------------------------------------------------------------------------- 1 | (dp0 2 | S'uvstates' 3 | p1 4 | (lp2 5 | (lp3 6 | F0.0 7 | aF0.0 8 | aasS'agents' 9 | p4 10 | (lp5 11 | ccopy_reg 12 | _reconstructor 13 | p6 14 | (cpyIA.agents 15 | Robot 16 | p7 17 | c__builtin__ 18 | object 19 | p8 20 | Ntp9 21 | Rp10 22 | (dp11 23 | S'radius' 24 | p12 25 | F0.3 26 | sS'actions' 27 | p13 28 | (lp14 29 | g6 30 | (cpyIA.actions 31 | Intend 32 | p15 33 | g8 34 | Ntp16 35 | Rp17 36 | ag6 37 | (cpyIA.actions 38 | Say 39 | p18 40 | g8 41 | Ntp19 42 | Rp20 43 | ag6 44 | (cpyIA.actions 45 | Nudge 46 | p21 47 | g8 48 | Ntp22 49 | Rp23 50 | asbasS'innerstates' 51 | p24 52 | (lp25 53 | g6 54 | (cpyIA.agents 55 | Innerstate 56 | p26 57 | g8 58 | Ntp27 59 | Rp28 60 | (dp29 61 | S'permissivity' 62 | p30 63 | F0.5 64 | sS'patience' 65 | p31 66 | I1 67 | sS'perceptivity' 68 | p32 69 | F0.9 70 | sS'goal' 71 | p33 72 | (lp34 73 | cnumpy.core.multiarray 74 | scalar 75 | p35 76 | (cnumpy 77 | dtype 78 | p36 79 | (S'f8' 80 | p37 81 | I0 82 | I1 83 | tp38 84 | Rp39 85 | (I3 86 | S'<' 87 | p40 88 | NNNI-1 89 | I-1 90 | I0 91 | tp41 92 | bS'\x1c\xe6N\xfe\xd1\x03+@' 93 | p42 94 | tp43 95 | Rp44 96 | ag35 97 | (g39 98 | S'\xa0\xbc<\xad\xb8\x93\x1d@' 99 | p45 100 | tp46 101 | Rp47 102 | asS'undertaking' 103 | p48 104 | NsbasS'xystates' 105 | p49 106 | (lp50 107 | (lp51 108 | g35 109 | (g39 110 | S'`E<\x0f\x85\x865\xc0' 111 | p52 112 | tp53 113 | Rp54 114 | ag35 115 | (g39 116 | S'\x08\x7f4\xdd\xc2\xf7\x0b\xc0' 117 | p55 118 | tp56 119 | Rp57 120 | aas. -------------------------------------------------------------------------------- /python/scenarios/irosunity_scene_map2.pickle: -------------------------------------------------------------------------------- 1 | (dp0 2 | S'uvstates' 3 | p1 4 | (lp2 5 | (lp3 6 | F0.0 7 | aF0.0 8 | aasS'agents' 9 | p4 10 | (lp5 11 | ccopy_reg 12 | _reconstructor 13 | p6 14 | (cpyIA.agents 15 | Robot 16 | p7 17 | c__builtin__ 18 | object 19 | p8 20 | Ntp9 21 | Rp10 22 | (dp11 23 | S'radius' 24 | p12 25 | F0.3 26 | sS'actions' 27 | p13 28 | (lp14 29 | g6 30 | (cpyIA.actions 31 | Intend 32 | p15 33 | g8 34 | Ntp16 35 | Rp17 36 | ag6 37 | (cpyIA.actions 38 | Say 39 | p18 40 | g8 41 | Ntp19 42 | Rp20 43 | ag6 44 | (cpyIA.actions 45 | Nudge 46 | p21 47 | g8 48 | Ntp22 49 | Rp23 50 | asbasS'innerstates' 51 | p24 52 | (lp25 53 | g6 54 | (cpyIA.agents 55 | Innerstate 56 | p26 57 | g8 58 | Ntp27 59 | Rp28 60 | (dp29 61 | S'permissivity' 62 | p30 63 | F0.5 64 | sS'patience' 65 | p31 66 | I1 67 | sS'perceptivity' 68 | p32 69 | F0.9 70 | sS'goal' 71 | p33 72 | (lp34 73 | cnumpy.core.multiarray 74 | scalar 75 | p35 76 | (cnumpy 77 | dtype 78 | p36 79 | (S'f8' 80 | p37 81 | I0 82 | I1 83 | tp38 84 | Rp39 85 | (I3 86 | S'<' 87 | p40 88 | NNNI-1 89 | I-1 90 | I0 91 | tp41 92 | bS'J)GN\xd6p1\xc0' 93 | p42 94 | tp43 95 | Rp44 96 | ag35 97 | (g39 98 | S'\x98\x01\x92\x1b\x85\xfc\x15@' 99 | p45 100 | tp46 101 | Rp47 102 | asS'undertaking' 103 | p48 104 | NsbasS'xystates' 105 | p49 106 | (lp50 107 | (lp51 108 | g35 109 | (g39 110 | S'P\xbb\xdc\x83x\x7f\xf0?' 111 | p52 112 | tp53 113 | Rp54 114 | ag35 115 | (g39 116 | S'\x82>(0N\x97!\xc0' 117 | p55 118 | tp56 119 | Rp57 120 | aas. -------------------------------------------------------------------------------- /python/scenarios/none.pickle: -------------------------------------------------------------------------------- 1 | (dp0 2 | S'uvstates' 3 | p1 4 | (lp2 5 | sS'agents' 6 | p3 7 | (lp4 8 | sS'innerstates' 9 | p5 10 | (lp6 11 | sS'xystates' 12 | p7 13 | (lp8 14 | s. -------------------------------------------------------------------------------- /python/scenarios/rlasl1.pickle: -------------------------------------------------------------------------------- 1 | (dp0 2 | S'uvstates' 3 | p1 4 | (lp2 5 | (lp3 6 | F0.0 7 | aF0.0 8 | aasS'agents' 9 | p4 10 | (lp5 11 | ccopy_reg 12 | _reconstructor 13 | p6 14 | (cpyIA.agents 15 | Robot 16 | p7 17 | c__builtin__ 18 | object 19 | p8 20 | Ntp9 21 | Rp10 22 | (dp11 23 | S'radius' 24 | p12 25 | F0.3 26 | sS'actions' 27 | p13 28 | (lp14 29 | g6 30 | (cpyIA.actions 31 | Intend 32 | p15 33 | g8 34 | Ntp16 35 | Rp17 36 | ag6 37 | (cpyIA.actions 38 | Say 39 | p18 40 | g8 41 | Ntp19 42 | Rp20 43 | ag6 44 | (cpyIA.actions 45 | Nudge 46 | p21 47 | g8 48 | Ntp22 49 | Rp23 50 | asbasS'innerstates' 51 | p24 52 | (lp25 53 | g6 54 | (cpyIA.agents 55 | Innerstate 56 | p26 57 | g8 58 | Ntp27 59 | Rp28 60 | (dp29 61 | S'permissivity' 62 | p30 63 | F0.5 64 | sS'patience' 65 | p31 66 | I1 67 | sS'perceptivity' 68 | p32 69 | F0.9 70 | sS'goal' 71 | p33 72 | (lp34 73 | cnumpy.core.multiarray 74 | scalar 75 | p35 76 | (cnumpy 77 | dtype 78 | p36 79 | (S'f8' 80 | p37 81 | I0 82 | I1 83 | tp38 84 | Rp39 85 | (I3 86 | S'<' 87 | p40 88 | NNNI-1 89 | I-1 90 | I0 91 | tp41 92 | bS'\x90\x19\xe8\x90|\xa9\xf3\xbf' 93 | p42 94 | tp43 95 | Rp44 96 | ag35 97 | (g39 98 | S'\xa0\x9e\x05\x85\xe4\x98\xf4\xbf' 99 | p45 100 | tp46 101 | Rp47 102 | asS'undertaking' 103 | p48 104 | NsbasS'xystates' 105 | p49 106 | (lp50 107 | (lp51 108 | g35 109 | (g39 110 | S'D\x04\x1dYyo\x0f\xc0' 111 | p52 112 | tp53 113 | Rp54 114 | ag35 115 | (g39 116 | S'\xfe \x8f5\xf2\x8e\x10\xc0' 117 | p55 118 | tp56 119 | Rp57 120 | aas. -------------------------------------------------------------------------------- /python/scenarios/rlasl2.pickle: -------------------------------------------------------------------------------- 1 | (dp0 2 | S'uvstates' 3 | p1 4 | (lp2 5 | (lp3 6 | F0.0 7 | aF0.0 8 | aasS'agents' 9 | p4 10 | (lp5 11 | ccopy_reg 12 | _reconstructor 13 | p6 14 | (cpyIA.agents 15 | Robot 16 | p7 17 | c__builtin__ 18 | object 19 | p8 20 | Ntp9 21 | Rp10 22 | (dp11 23 | S'radius' 24 | p12 25 | F0.3 26 | sS'actions' 27 | p13 28 | (lp14 29 | g6 30 | (cpyIA.actions 31 | Intend 32 | p15 33 | g8 34 | Ntp16 35 | Rp17 36 | ag6 37 | (cpyIA.actions 38 | Say 39 | p18 40 | g8 41 | Ntp19 42 | Rp20 43 | ag6 44 | (cpyIA.actions 45 | Nudge 46 | p21 47 | g8 48 | Ntp22 49 | Rp23 50 | asbasS'innerstates' 51 | p24 52 | (lp25 53 | g6 54 | (cpyIA.agents 55 | Innerstate 56 | p26 57 | g8 58 | Ntp27 59 | Rp28 60 | (dp29 61 | S'permissivity' 62 | p30 63 | F0.5 64 | sS'patience' 65 | p31 66 | I1 67 | sS'perceptivity' 68 | p32 69 | F0.9 70 | sS'goal' 71 | p33 72 | (lp34 73 | cnumpy.core.multiarray 74 | scalar 75 | p35 76 | (cnumpy 77 | dtype 78 | p36 79 | (S'f8' 80 | p37 81 | I0 82 | I1 83 | tp38 84 | Rp39 85 | (I3 86 | S'<' 87 | p40 88 | NNNI-1 89 | I-1 90 | I0 91 | tp41 92 | bS'fW\xcd\xa7\xfer\x1c\xc0' 93 | p42 94 | tp43 95 | Rp44 96 | ag35 97 | (g39 98 | S'\xee\x13\xf5\x95\xc5& \xc0' 99 | p45 100 | tp46 101 | Rp47 102 | asS'undertaking' 103 | p48 104 | NsbasS'xystates' 105 | p49 106 | (lp50 107 | (lp51 108 | g35 109 | (g39 110 | S',1\xb6\xbf\xaf\xc6 \xc0' 111 | p52 112 | tp53 113 | Rp54 114 | ag35 115 | (g39 116 | S't\xf1\x8f\x18A\xba\x13\xc0' 117 | p55 118 | tp56 119 | Rp57 120 | aas. -------------------------------------------------------------------------------- /python/scenarios/rlasl_office_j1.pickle: -------------------------------------------------------------------------------- 1 | (dp0 2 | S'uvstates' 3 | p1 4 | (lp2 5 | (lp3 6 | F0.0 7 | aF0.0 8 | aasS'agents' 9 | p4 10 | (lp5 11 | ccopy_reg 12 | _reconstructor 13 | p6 14 | (cpyIA.agents 15 | Robot 16 | p7 17 | c__builtin__ 18 | object 19 | p8 20 | Ntp9 21 | Rp10 22 | (dp11 23 | S'radius' 24 | p12 25 | F0.3 26 | sS'actions' 27 | p13 28 | (lp14 29 | g6 30 | (cpyIA.actions 31 | Intend 32 | p15 33 | g8 34 | Ntp16 35 | Rp17 36 | ag6 37 | (cpyIA.actions 38 | Say 39 | p18 40 | g8 41 | Ntp19 42 | Rp20 43 | ag6 44 | (cpyIA.actions 45 | Nudge 46 | p21 47 | g8 48 | Ntp22 49 | Rp23 50 | asbasS'innerstates' 51 | p24 52 | (lp25 53 | g6 54 | (cpyIA.agents 55 | Innerstate 56 | p26 57 | g8 58 | Ntp27 59 | Rp28 60 | (dp29 61 | S'permissivity' 62 | p30 63 | F0.5 64 | sS'patience' 65 | p31 66 | I1 67 | sS'perceptivity' 68 | p32 69 | F0.9 70 | sS'goal' 71 | p33 72 | (lp34 73 | cnumpy.core.multiarray 74 | scalar 75 | p35 76 | (cnumpy 77 | dtype 78 | p36 79 | (S'f8' 80 | p37 81 | I0 82 | I1 83 | tp38 84 | Rp39 85 | (I3 86 | S'<' 87 | p40 88 | NNNI-1 89 | I-1 90 | I0 91 | tp41 92 | bS'\xbb\xfb\x93\xc4\xed\xc15\xc0' 93 | p42 94 | tp43 95 | Rp44 96 | ag35 97 | (g39 98 | S'\x80&\xd6\x9cjb\xf5\xbf' 99 | p45 100 | tp46 101 | Rp47 102 | asS'undertaking' 103 | p48 104 | NsbasS'xystates' 105 | p49 106 | (lp50 107 | (lp51 108 | g35 109 | (g39 110 | S'oU\xd0(e\x075\xc0' 111 | p52 112 | tp53 113 | Rp54 114 | ag35 115 | (g39 116 | S'`\xb2Sj\x81\x94\x0f\xc0' 117 | p55 118 | tp56 119 | Rp57 120 | aas. -------------------------------------------------------------------------------- /python/scenarios/rlasl_office_j2.pickle: -------------------------------------------------------------------------------- 1 | (dp0 2 | S'uvstates' 3 | p1 4 | (lp2 5 | (lp3 6 | F0.0 7 | aF0.0 8 | aasS'agents' 9 | p4 10 | (lp5 11 | ccopy_reg 12 | _reconstructor 13 | p6 14 | (cpyIA.agents 15 | Robot 16 | p7 17 | c__builtin__ 18 | object 19 | p8 20 | Ntp9 21 | Rp10 22 | (dp11 23 | S'radius' 24 | p12 25 | F0.3 26 | sS'actions' 27 | p13 28 | (lp14 29 | g6 30 | (cpyIA.actions 31 | Intend 32 | p15 33 | g8 34 | Ntp16 35 | Rp17 36 | ag6 37 | (cpyIA.actions 38 | Say 39 | p18 40 | g8 41 | Ntp19 42 | Rp20 43 | ag6 44 | (cpyIA.actions 45 | Nudge 46 | p21 47 | g8 48 | Ntp22 49 | Rp23 50 | asbasS'innerstates' 51 | p24 52 | (lp25 53 | g6 54 | (cpyIA.agents 55 | Innerstate 56 | p26 57 | g8 58 | Ntp27 59 | Rp28 60 | (dp29 61 | S'permissivity' 62 | p30 63 | F0.5 64 | sS'patience' 65 | p31 66 | I1 67 | sS'perceptivity' 68 | p32 69 | F0.9 70 | sS'goal' 71 | p33 72 | (lp34 73 | cnumpy.core.multiarray 74 | scalar 75 | p35 76 | (cnumpy 77 | dtype 78 | p36 79 | (S'f8' 80 | p37 81 | I0 82 | I1 83 | tp38 84 | Rp39 85 | (I3 86 | S'<' 87 | p40 88 | NNNI-1 89 | I-1 90 | I0 91 | tp41 92 | bS'\xe5\x8f\xbau\xca\xab6\xc0' 93 | p42 94 | tp43 95 | Rp44 96 | ag35 97 | (g39 98 | S'Ph\xa1z\x11\t\x0b@' 99 | p45 100 | tp46 101 | Rp47 102 | asS'undertaking' 103 | p48 104 | NsbasS'xystates' 105 | p49 106 | (lp50 107 | (lp51 108 | g35 109 | (g39 110 | S'\x84F\xce\xda\xd8\xca6\xc0' 111 | p52 112 | tp53 113 | Rp54 114 | ag35 115 | (g39 116 | S'\x00d\xce>j\xa3\xd0\xbf' 117 | p55 118 | tp56 119 | Rp57 120 | aas. -------------------------------------------------------------------------------- /python/scenarios/rlunity_scene_map1.pickle: -------------------------------------------------------------------------------- 1 | (dp0 2 | S'uvstates' 3 | p1 4 | (lp2 5 | (lp3 6 | F0.0 7 | aF0.0 8 | aasS'agents' 9 | p4 10 | (lp5 11 | ccopy_reg 12 | _reconstructor 13 | p6 14 | (cpyIA.agents 15 | Robot 16 | p7 17 | c__builtin__ 18 | object 19 | p8 20 | Ntp9 21 | Rp10 22 | (dp11 23 | S'radius' 24 | p12 25 | F0.3 26 | sS'actions' 27 | p13 28 | (lp14 29 | g6 30 | (cpyIA.actions 31 | Intend 32 | p15 33 | g8 34 | Ntp16 35 | Rp17 36 | ag6 37 | (cpyIA.actions 38 | Say 39 | p18 40 | g8 41 | Ntp19 42 | Rp20 43 | ag6 44 | (cpyIA.actions 45 | Nudge 46 | p21 47 | g8 48 | Ntp22 49 | Rp23 50 | asbasS'innerstates' 51 | p24 52 | (lp25 53 | g6 54 | (cpyIA.agents 55 | Innerstate 56 | p26 57 | g8 58 | Ntp27 59 | Rp28 60 | (dp29 61 | S'permissivity' 62 | p30 63 | F0.5 64 | sS'patience' 65 | p31 66 | I1 67 | sS'perceptivity' 68 | p32 69 | F0.9 70 | sS'goal' 71 | p33 72 | (lp34 73 | cnumpy.core.multiarray 74 | scalar 75 | p35 76 | (cnumpy 77 | dtype 78 | p36 79 | (S'f8' 80 | p37 81 | I0 82 | I1 83 | tp38 84 | Rp39 85 | (I3 86 | S'<' 87 | p40 88 | NNNI-1 89 | I-1 90 | I0 91 | tp41 92 | bS'\x80B\xeb"\t\xfe\xec\xbf' 93 | p42 94 | tp43 95 | Rp44 96 | ag35 97 | (g39 98 | S'\x00\x9fR\x1f\x9d\x07\xd3?' 99 | p45 100 | tp46 101 | Rp47 102 | asS'undertaking' 103 | p48 104 | NsbasS'xystates' 105 | p49 106 | (lp50 107 | (lp51 108 | g35 109 | (g39 110 | S'X\x1a\xae\xa3\xc1\t\x07@' 111 | p52 112 | tp53 113 | Rp54 114 | ag35 115 | (g39 116 | S'@j@\xc4\x89\x00\xda?' 117 | p55 118 | tp56 119 | Rp57 120 | aas. -------------------------------------------------------------------------------- /python/scenarios/rlunity_scene_map2.pickle: -------------------------------------------------------------------------------- 1 | (dp0 2 | S'uvstates' 3 | p1 4 | (lp2 5 | (lp3 6 | F0.0 7 | aF0.0 8 | aasS'agents' 9 | p4 10 | (lp5 11 | ccopy_reg 12 | _reconstructor 13 | p6 14 | (cpyIA.agents 15 | Robot 16 | p7 17 | c__builtin__ 18 | object 19 | p8 20 | Ntp9 21 | Rp10 22 | (dp11 23 | S'radius' 24 | p12 25 | F0.3 26 | sS'actions' 27 | p13 28 | (lp14 29 | g6 30 | (cpyIA.actions 31 | Intend 32 | p15 33 | g8 34 | Ntp16 35 | Rp17 36 | ag6 37 | (cpyIA.actions 38 | Say 39 | p18 40 | g8 41 | Ntp19 42 | Rp20 43 | ag6 44 | (cpyIA.actions 45 | Nudge 46 | p21 47 | g8 48 | Ntp22 49 | Rp23 50 | asbasS'innerstates' 51 | p24 52 | (lp25 53 | g6 54 | (cpyIA.agents 55 | Innerstate 56 | p26 57 | g8 58 | Ntp27 59 | Rp28 60 | (dp29 61 | S'permissivity' 62 | p30 63 | F0.5 64 | sS'patience' 65 | p31 66 | I1 67 | sS'perceptivity' 68 | p32 69 | F0.9 70 | sS'goal' 71 | p33 72 | (lp34 73 | cnumpy.core.multiarray 74 | scalar 75 | p35 76 | (cnumpy 77 | dtype 78 | p36 79 | (S'f8' 80 | p37 81 | I0 82 | I1 83 | tp38 84 | Rp39 85 | (I3 86 | S'<' 87 | p40 88 | NNNI-1 89 | I-1 90 | I0 91 | tp41 92 | bS'\xe0*V\xe9A\xc0\xf1?' 93 | p42 94 | tp43 95 | Rp44 96 | ag35 97 | (g39 98 | S'\xfcc\xf9\xc6Z\x04\x15@' 99 | p45 100 | tp46 101 | Rp47 102 | asS'undertaking' 103 | p48 104 | NsbasS'xystates' 105 | p49 106 | (lp50 107 | (lp51 108 | g35 109 | (g39 110 | S'`L\x95_o\xd0\xe3?' 111 | p52 112 | tp53 113 | Rp54 114 | ag35 115 | (g39 116 | S"\xf0#'\xe460\xf6?" 117 | p55 118 | tp56 119 | Rp57 120 | aas. -------------------------------------------------------------------------------- /python/scenarios/scenarios.txt: -------------------------------------------------------------------------------- 1 | irosasl1 dense crowd between start and goal 2 | irosasl2 aslgroups 3 | 4 | irosunity_scene_map1 5 | 6 | irosasl_office_j1 7 | -------------------------------------------------------------------------------- /python/scenarios/toyempty1.pickle: -------------------------------------------------------------------------------- 1 | (dp0 2 | S'uvstates' 3 | p1 4 | (lp2 5 | (lp3 6 | F0.0 7 | aF0.0 8 | aasS'agents' 9 | p4 10 | (lp5 11 | ccopy_reg 12 | _reconstructor 13 | p6 14 | (cpyIA.agents 15 | Robot 16 | p7 17 | c__builtin__ 18 | object 19 | p8 20 | Ntp9 21 | Rp10 22 | (dp11 23 | S'radius' 24 | p12 25 | F0.3 26 | sS'actions' 27 | p13 28 | (lp14 29 | g6 30 | (cpyIA.actions 31 | Intend 32 | p15 33 | g8 34 | Ntp16 35 | Rp17 36 | ag6 37 | (cpyIA.actions 38 | Say 39 | p18 40 | g8 41 | Ntp19 42 | Rp20 43 | ag6 44 | (cpyIA.actions 45 | Nudge 46 | p21 47 | g8 48 | Ntp22 49 | Rp23 50 | asbasS'innerstates' 51 | p24 52 | (lp25 53 | g6 54 | (cpyIA.agents 55 | Innerstate 56 | p26 57 | g8 58 | Ntp27 59 | Rp28 60 | (dp29 61 | S'permissivity' 62 | p30 63 | F0.5 64 | sS'patience' 65 | p31 66 | I1 67 | sS'perceptivity' 68 | p32 69 | F0.9 70 | sS'goal' 71 | p33 72 | (lp34 73 | cnumpy.core.multiarray 74 | scalar 75 | p35 76 | (cnumpy 77 | dtype 78 | p36 79 | (S'f8' 80 | p37 81 | I0 82 | I1 83 | tp38 84 | Rp39 85 | (I3 86 | S'<' 87 | p40 88 | NNNI-1 89 | I-1 90 | I0 91 | tp41 92 | bS'\xc0N\xeeu\xe4\x87\xfd?' 93 | p42 94 | tp43 95 | Rp44 96 | ag35 97 | (g39 98 | S'0\xc9\xda\x9f\x8f\x9a\xfc?' 99 | p45 100 | tp46 101 | Rp47 102 | asS'undertaking' 103 | p48 104 | NsbasS'xystates' 105 | p49 106 | (lp50 107 | (lp51 108 | g35 109 | (g39 110 | S'\x94o\xcftnO\xfd\xbf' 111 | p52 112 | tp53 113 | Rp54 114 | ag35 115 | (g39 116 | S'H\x82\xa0\xd4Sb\xfc\xbf' 117 | p55 118 | tp56 119 | Rp57 120 | aas. -------------------------------------------------------------------------------- /python/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup, find_packages 2 | from Cython.Build import cythonize 3 | import os 4 | 5 | 6 | 7 | setup( 8 | name="pyIA", 9 | description='Python library for interaction-actions planner', 10 | author='Daniel Dugas', 11 | version='0.0', 12 | packages=find_packages(), 13 | # ext_modules = cythonize("clib_clustering/lidar_clustering.pyx", annotate=True), 14 | ) 15 | 16 | # install cIA separately by running pip install . in the cIA subdir! 17 | -------------------------------------------------------------------------------- /python/tools/check_agent0_is_robot.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | import os 3 | 4 | from pyIA.arguments import parse_sim_args 5 | from pyIA.simulator import Environment 6 | import pyIA.agents as agents 7 | 8 | 9 | if __name__ == "__main__": 10 | args = parse_sim_args() 11 | scenario_files = sorted(os.listdir(args.scenario_folder)) 12 | print("checking for {} scenarios in {}".format(len(scenario_files), args.scenario_folder)) 13 | print("------------------------------------------------------------") 14 | for file in scenario_files: 15 | name, ext = os.path.splitext(file) 16 | if ext == ".pickle": 17 | args.scenario = name 18 | env = Environment(args, silent=True) 19 | try: 20 | env.populate(silent=True) 21 | except AttributeError as e: 22 | print("could not populate ", name, "(", e, ")") 23 | continue 24 | if not env.worldstate.get_agents(): 25 | print(name, "has no agents!") 26 | continue 27 | if not isinstance(env.worldstate.get_agents()[0], agents.Robot): 28 | print(name, "does not have robot as agent 0!") 29 | continue 30 | print(name, len(env.worldstate.get_agents()), "agents, ok.") 31 | -------------------------------------------------------------------------------- /python/tools/draw_maps.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | import os 3 | import numpy as np 4 | import matplotlib.pyplot as plt 5 | import matplotlib 6 | 7 | from pyIA.arguments import parse_sim_args 8 | from pyIA.simulator import Environment 9 | import pyIA.agents as agents 10 | import pyIA.state_estimation as state_estimation 11 | import pyIA.paths as paths 12 | import pyIA.ia_planning as ia_planning 13 | 14 | matplotlib.rcParams['mathtext.fontset'] = 'stix' 15 | matplotlib.rcParams['font.family'] = 'STIXGeneral' 16 | 17 | mapnames = ["asl", "unity_scene_map", "asl_office_j"] 18 | 19 | # map should be given as arg 20 | 21 | if __name__ == "__main__": 22 | args = parse_sim_args() 23 | print("------------------------------------------------------------") 24 | plt.figure() 25 | for mapname in mapnames: 26 | print(mapname) 27 | args.map_name = mapname 28 | env = Environment(args, silent=True) 29 | contours = env.worldstate.map.as_closed_obst_vertices() 30 | env.worldstate.map.plot_contours(contours) 31 | plt.axis('off') 32 | plt.gca().set_xticklabels([]) 33 | plt.gca().set_yticklabels([]) 34 | plt.show() 35 | -------------------------------------------------------------------------------- /python/tools/draw_rl_scenes.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | import os 3 | import numpy as np 4 | import matplotlib.pyplot as plt 5 | import matplotlib 6 | 7 | from pyIA.arguments import parse_sim_args 8 | from pyIA.simulator import Environment 9 | import pyIA.agents as agents 10 | import pyIA.state_estimation as state_estimation 11 | import pyIA.paths as paths 12 | import pyIA.ia_planning as ia_planning 13 | 14 | matplotlib.rcParams['mathtext.fontset'] = 'stix' 15 | matplotlib.rcParams['font.family'] = 'STIXGeneral' 16 | 17 | FILTER = "rl" 18 | 19 | # map should be given as arg 20 | 21 | if __name__ == "__main__": 22 | args = parse_sim_args() 23 | scenario_files = sorted(os.listdir(args.scenario_folder)) 24 | filtered_scenarios = [] 25 | for file in scenario_files: 26 | name, ext = os.path.splitext(file) 27 | if FILTER not in name: 28 | continue 29 | if ext != ".pickle": 30 | continue 31 | if args.map_name != name[len(FILTER):-1]: 32 | continue 33 | filtered_scenarios.append(name) 34 | 35 | print("filtered to {} scenarios in {}".format(len(filtered_scenarios), args.scenario_folder)) 36 | print("------------------------------------------------------------") 37 | fig, axes = plt.subplots(1, len(filtered_scenarios)) 38 | fig.subplots_adjust(hspace=0.3, wspace=0) 39 | axes = np.array(axes).flatten() 40 | fixed_state = None 41 | for name, ax in zip(filtered_scenarios, axes): 42 | print(name) 43 | args.scenario = name 44 | env = Environment(args, silent=True) 45 | try: 46 | env.populate(silent=True) 47 | except AttributeError as e: 48 | print("could not populate ", name, "(", e, ")") 49 | continue 50 | all_agents = env.worldstate.get_agents() 51 | if not all_agents: 52 | print(name, "has no agents!") 53 | continue 54 | robot_agent = all_agents[0] 55 | if not isinstance(robot_agent, agents.Robot): 56 | print(name, "does not have robot as agent 0!") 57 | agent_index = 0 58 | state_e, fixed_state = state_estimation.state_estimate_from_sim_worldstate( 59 | agent_index, env.worldstate, reuse_state_cache=fixed_state 60 | ) 61 | plt.sca(ax) 62 | # agent goals 63 | xys = env.worldstate.get_xystates() 64 | inners = env.worldstate.get_innerstates() 65 | for inner, xy in zip(inners[1:], xys[1:]): 66 | plt.plot([xy[0], inner.goal[0]], [xy[1], inner.goal[1]], '--', color="Grey") 67 | # world 68 | ia_planning.visualize_world(state_e, fixed_state, [], env, 69 | fig=fig, possible_path_variants=[paths.NaivePath()]) 70 | ax.set_xticklabels([]) 71 | ax.set_yticklabels([]) 72 | plt.axis('off') 73 | plt.title("Scene " + name[-1]) 74 | plt.show() 75 | -------------------------------------------------------------------------------- /python/tools/draw_scenes.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | import os 3 | import numpy as np 4 | import matplotlib.pyplot as plt 5 | import matplotlib 6 | 7 | from pyIA.arguments import parse_sim_args 8 | from pyIA.simulator import Environment 9 | import pyIA.agents as agents 10 | import pyIA.state_estimation as state_estimation 11 | import pyIA.paths as paths 12 | import pyIA.ia_planning as ia_planning 13 | 14 | matplotlib.rcParams['mathtext.fontset'] = 'stix' 15 | matplotlib.rcParams['font.family'] = 'STIXGeneral' 16 | 17 | FILTER = "iros" 18 | 19 | # map should be given as arg 20 | 21 | if __name__ == "__main__": 22 | args = parse_sim_args() 23 | scenario_files = sorted(os.listdir(args.scenario_folder)) 24 | filtered_scenarios = [] 25 | for file in scenario_files: 26 | name, ext = os.path.splitext(file) 27 | if FILTER not in name: 28 | continue 29 | if ext != ".pickle": 30 | continue 31 | if args.map_name != name[4:-1]: 32 | continue 33 | filtered_scenarios.append(name) 34 | 35 | print("filtered to {} scenarios in {}".format(len(filtered_scenarios), args.scenario_folder)) 36 | print("------------------------------------------------------------") 37 | fig, axes = plt.subplots(2, len(filtered_scenarios)/2) 38 | fig.subplots_adjust(hspace=0.3, wspace=0) 39 | axes = np.array(axes).flatten() 40 | fixed_state = None 41 | for name, ax in zip(filtered_scenarios, axes): 42 | print(name) 43 | args.scenario = name 44 | env = Environment(args, silent=True) 45 | try: 46 | env.populate(silent=True) 47 | except AttributeError as e: 48 | print("could not populate ", name, "(", e, ")") 49 | continue 50 | all_agents = env.worldstate.get_agents() 51 | if not all_agents: 52 | print(name, "has no agents!") 53 | continue 54 | robot_agent = all_agents[0] 55 | if not isinstance(robot_agent, agents.Robot): 56 | print(name, "does not have robot as agent 0!") 57 | agent_index = 0 58 | state_e, fixed_state = state_estimation.state_estimate_from_sim_worldstate( 59 | agent_index, env.worldstate, reuse_state_cache=fixed_state 60 | ) 61 | plt.sca(ax) 62 | # agent goals 63 | xys = env.worldstate.get_xystates() 64 | inners = env.worldstate.get_innerstates() 65 | for inner, xy in zip(inners[1:], xys[1:]): 66 | plt.plot([xy[0], inner.goal[0]], [xy[1], inner.goal[1]], '--', color="Grey") 67 | # world 68 | ia_planning.visualize_world(state_e, fixed_state, [], env, 69 | fig=fig, possible_path_variants=[paths.NaivePath()]) 70 | ax.set_xticklabels([]) 71 | ax.set_yticklabels([]) 72 | plt.axis('off') 73 | plt.title("Scene " + name[-1]) 74 | plt.show() 75 | -------------------------------------------------------------------------------- /python/tools/get_distance_to_goal.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | import os 3 | import numpy as np 4 | 5 | from pyIA.arguments import parse_sim_args 6 | from pyIA.simulator import Environment 7 | import pyIA.agents as agents 8 | import pyIA.state_estimation as state_estimation 9 | import pyIA.paths as paths 10 | 11 | FILTER = "iros" 12 | 13 | if __name__ == "__main__": 14 | args = parse_sim_args() 15 | scenario_files = sorted(os.listdir(args.scenario_folder)) 16 | print("checking for {} scenarios in {}".format(len(scenario_files), args.scenario_folder)) 17 | print("------------------------------------------------------------") 18 | for file in scenario_files: 19 | name, ext = os.path.splitext(file) 20 | if FILTER not in name: 21 | continue 22 | if ext == ".pickle": 23 | args.scenario = name 24 | if "asl_office_j" in name: 25 | args.map_name = "asl_office_j" 26 | elif "unity_scene_map" in name: 27 | args.map_name = "unity_scene_map" 28 | else: 29 | args.map_name = "asl" 30 | env = Environment(args, silent=True) 31 | try: 32 | env.populate(silent=True) 33 | except AttributeError as e: 34 | print("could not populate ", name, "(", e, ")") 35 | continue 36 | if not env.worldstate.get_agents(): 37 | print(name, "has no agents!") 38 | continue 39 | robot_agent = env.worldstate.get_agents()[0] 40 | if not isinstance(robot_agent, agents.Robot): 41 | print(name, "does not have robot as agent 0!") 42 | agent_index = 0 43 | state_e, fixed_state = state_estimation.state_estimate_from_sim_worldstate( 44 | agent_index, env.worldstate 45 | ) 46 | path_xy = fixed_state.derived_state.path_to_goal(state_e, fixed_state, paths.NaivePath()) 47 | length = 0 48 | print(name) 49 | try: 50 | dxy = np.diff(path_xy, axis=0) 51 | except: 52 | import pyIA.ia_planning as ia_planning 53 | import matplotlib.pyplot as plt 54 | plt.figure() 55 | ia_planning.visualize_state_features(state_e, fixed_state, hide_uncertain=False) 56 | ia_planning.visualize_traversabilities(state_e, fixed_state, [paths.NaivePath()]) 57 | ia_planning.visualize_dijkstra_fields(state_e, fixed_state, [paths.NaivePath()]) 58 | plt.show() 59 | print(path_xy) 60 | length = np.sum(np.linalg.norm(dxy, axis=-1)) 61 | print(" manhattan dist: ", length) 62 | dxy = np.diff(path_xy[::5], axis=0) 63 | length = np.sum(np.linalg.norm(dxy, axis=-1)) 64 | print(" 5-sample dist: ", length) 65 | dxy = np.diff(path_xy[::10], axis=0) 66 | length = np.sum(np.linalg.norm(dxy, axis=-1)) 67 | print(" 10-sample dist: ", length) 68 | -------------------------------------------------------------------------------- /python/tools/gui_graph_editor.py: -------------------------------------------------------------------------------- 1 | import os 2 | from builtins import input 3 | import matplotlib.pyplot as plt 4 | from pyniel.pyplot_tools.graph_creator_gui import GraphCreatorGui 5 | 6 | from pyIA.arguments import parse_sim_args 7 | from pyIA.simulator import Environment 8 | 9 | 10 | args = parse_sim_args() 11 | env = Environment(args) 12 | env.populate() 13 | 14 | 15 | # plot map 16 | worldstate = env.get_worldstate() 17 | fig = plt.figure("map") 18 | contours = worldstate.map.as_closed_obst_vertices() 19 | worldstate.map.plot_contours(contours, '-k') 20 | plt.axis('equal') 21 | 22 | gcg = GraphCreatorGui(figure=fig, debug=args.debug) 23 | # load graph if exists 24 | filedir = os.path.expanduser(os.path.expandvars(args.map_folder)) 25 | filepath = os.path.join(filedir, args.map_name + "_graph.pickle") 26 | if os.path.isfile(filepath): 27 | print("Previous graph for map found. Loading...") 28 | gcg.graph.restore_from_file(filepath) 29 | gcg.run() 30 | 31 | graph = gcg.graph 32 | 33 | # change node ids 34 | graph.reassign_ids() 35 | 36 | print(str(graph)) 37 | 38 | yesno = input("Save graph to file? [Y/n]") 39 | if yesno not in ["n", "N", "no", "NO", "No"]: 40 | if os.path.isfile(filepath): 41 | overwrite = input("Graph already exists for current map. Overwrite? [y/N]") 42 | if overwrite not in ["y", "Y", "yes", "YES", "Yes"]: 43 | i = 1 44 | while True: 45 | filepath = os.path.join(filedir, args.map_name + "_graph({}).pickle".format(i)) 46 | if os.path.isfile(filepath): 47 | i += 1 48 | continue 49 | else: 50 | break 51 | graph.save_to_file(filepath) 52 | --------------------------------------------------------------------------------