├── .gitignore ├── CMakeLists.txt ├── README.md ├── exploration_benchmark ├── CMakeLists.txt ├── blueprints │ ├── corner.pgm │ ├── corner.yaml │ ├── corridor.pgm │ ├── corridor.yaml │ ├── loop.pgm │ ├── loop.yaml │ ├── loop_with_corridor.pgm │ ├── loop_with_corridor.yaml │ ├── room.pgm │ ├── room.yaml │ ├── room_with_corner.pgm │ └── room_with_corner.yaml ├── include │ ├── functions.h │ └── mtrand.h ├── launch │ ├── single_cost_node.launch │ ├── single_mmpf_node.launch │ ├── single_rl_node.launch │ ├── single_rrt_node.launch │ ├── two_cost_node.launch │ ├── two_mmpf_node.launch │ ├── two_rl_node.launch │ └── two_rrt_node.launch ├── msg │ ├── PointArray.msg │ └── PointStampedArray.msg ├── package.xml ├── rviz │ ├── robot1_cost_node.rviz │ ├── robot1_mmpf_node.rviz │ ├── robot1_rl_node.rviz │ ├── robot1_rrt_node.rviz │ ├── robot2_cost_node.rviz │ ├── robot2_mmpf_node.rviz │ ├── robot2_rl_node.rviz │ └── robot2_rrt_node.rviz ├── scripts │ ├── DRL │ │ ├── models │ │ │ ├── actor.pt │ │ │ └── critic.pt │ │ ├── run_single_robot.sh │ │ ├── run_two_robots.sh │ │ ├── single_robot_1.py │ │ ├── two_robot_1.py │ │ ├── two_robot_2.py │ │ └── window.py │ ├── exploration_metric_for_single_robot.py │ ├── exploration_metric_for_two_robots.py │ └── publish_tf_for_two_robots.py └── src │ ├── cost_node.cpp │ ├── functions.cpp │ ├── mmpf_node.cpp │ ├── mtrand.cpp │ └── rrt_node_tf.cpp ├── graythree ├── CMakeLists.txt ├── launch │ ├── mapmerge.launch │ ├── mapmerge_real_single_car.launch │ └── mapmerge_real_two_cars.launch ├── package.xml └── src │ └── mapmerge_node.cpp ├── grid_simulator ├── Astar.py ├── GridEnv.py └── window.py ├── map_merge ├── CHANGELOG.rst ├── CMakeLists.txt ├── doc │ ├── architecture.dia │ ├── screenshot.jpg │ └── wiki_doc.txt ├── include │ ├── combine_grids │ │ ├── grid_compositor.h │ │ ├── grid_warper.h │ │ └── merging_pipeline.h │ └── map_merge │ │ └── map_merge.h ├── launch │ ├── experiments │ │ ├── 3-robots.ttt.launch │ │ ├── demo-scene-1.ttt.launch │ │ └── demo-scene-orientation-exp.ttt.launch │ └── map_merge.launch ├── package.xml ├── script │ ├── matchStamps2tf.py │ └── msg_remap.py ├── src │ ├── combine_grids │ │ ├── estimation_internal.h │ │ ├── grid_compositor.cpp │ │ ├── grid_warper.cpp │ │ └── merging_pipeline.cpp │ ├── map_merge.cpp │ ├── map_merge_new.cpp │ ├── map_register.cpp │ └── mapmerge_node.cpp ├── srv │ └── mapPair2tf.srv └── test │ ├── test_merging_pipeline.cpp │ └── testing_helpers.h ├── onpolicy ├── .gitignore ├── .gitmodules ├── .idea │ ├── .gitignore │ ├── inspectionProfiles │ │ └── profiles_settings.xml │ ├── misc.xml │ ├── modules.xml │ ├── onpolicy.iml │ └── vcs.xml ├── README.md ├── environment.yaml ├── onpolicy │ ├── __init__.py │ ├── algorithms │ │ ├── __init__.py │ │ ├── r_mappg │ │ │ ├── __init__.py │ │ │ ├── algorithm │ │ │ │ ├── rMAPPGPolicy.py │ │ │ │ └── r_actor_critic.py │ │ │ └── r_mappg.py │ │ ├── r_mappg_single │ │ │ ├── __init__.py │ │ │ ├── algorithm │ │ │ │ ├── rMAPPGPolicy.py │ │ │ │ └── r_actor_critic.py │ │ │ └── r_mappg_single.py │ │ ├── r_mappo │ │ │ ├── __init__.py │ │ │ ├── algorithm │ │ │ │ ├── rMAPPOPolicy.py │ │ │ │ └── r_actor_critic.py │ │ │ └── r_mappo.py │ │ ├── r_mappo_single │ │ │ ├── __init__.py │ │ │ ├── algorithm │ │ │ │ ├── rMAPPOPolicy.py │ │ │ │ └── r_actor_critic.py │ │ │ └── r_mappo_single.py │ │ └── utils │ │ │ ├── act.py │ │ │ ├── attention.py │ │ │ ├── cnn.py │ │ │ ├── distributions.py │ │ │ ├── mix.py │ │ │ ├── mlp.py │ │ │ ├── popart.py │ │ │ ├── rnn.py │ │ │ └── util.py │ ├── config.py │ ├── docs │ │ ├── Makefile │ │ ├── make.bat │ │ └── source │ │ │ ├── _templates │ │ │ ├── module.rst_t │ │ │ ├── package.rst_t │ │ │ └── toc.rst_t │ │ │ ├── conf.py │ │ │ ├── full.gif │ │ │ ├── index.rst │ │ │ ├── quickstart.rst │ │ │ └── setup.rst │ ├── envs │ │ ├── GridEnv │ │ │ ├── Astar.py │ │ │ ├── Gazebo2D (another copy).py │ │ │ ├── Gazebo2D (copy).py │ │ │ ├── GridEnv.py │ │ │ ├── __init__.py │ │ │ ├── datasets │ │ │ │ ├── corner.pgm │ │ │ │ ├── corridor.pgm │ │ │ │ ├── corridor_asym.pgm │ │ │ │ ├── corridor_sym.pgm │ │ │ │ ├── loop_with_corridor_sym.pgm │ │ │ │ ├── room1_modified.pgm │ │ │ │ ├── room_1.pgm │ │ │ │ ├── room_2.pgm │ │ │ │ ├── room_3.pgm │ │ │ │ ├── room_with_corner.pgm │ │ │ │ ├── room_with_corridor.pgm │ │ │ │ └── square_loop.pgm │ │ │ └── window.py │ │ ├── __init__.py │ │ └── env_wrappers.py │ ├── runner │ │ ├── separated │ │ │ └── base_runner.py │ │ └── shared │ │ │ ├── base_runner.py │ │ │ └── grid_runner.py │ ├── scripts │ │ ├── .png │ │ ├── eval │ │ │ ├── eval_habitat.py │ │ │ └── eval_hanabi.py │ │ ├── train │ │ │ ├── __init__.py │ │ │ └── train_grid.py │ │ └── train_grid.sh │ └── utils │ │ ├── __init__.py │ │ ├── multi_discrete.py │ │ ├── separated_buffer.py │ │ ├── shared_buffer.py │ │ ├── util.py │ │ └── valuenorm.py ├── render_human.sh ├── requirements.txt └── setup.py └── sim_env ├── CMakeLists.txt ├── config ├── turtlebot3_lds_2d.lua ├── turtlebot3_robot1.lua ├── turtlebot3_robot2.lua ├── turtlebot3_robot3.lua ├── turtlebot3_robot4.lua ├── turtlebot3_robot5.lua └── turtlebot3_robot6.lua ├── gazebo_images ├── main_body.jpg ├── meshes │ ├── MTR_map.dae │ ├── hokuyo.dae │ ├── images │ │ ├── main_body.jpg │ │ └── wheel.jpg │ ├── largeMap.dae │ ├── main_body.dae │ └── wheel.dae └── wheel.jpg ├── launch ├── gmapping.launch ├── includes │ ├── cartographer_teb.launch │ ├── move_base_robot.launch │ └── robot.launch.xml ├── robots_mapping │ ├── single_robot.launch │ └── two_robots.launch └── simulation_environment │ ├── corner_close_env_two_robots.launch │ ├── corner_env_single_robot.launch │ ├── corner_far_env_two_robots.launch │ ├── corridor_close_env_two_robots.launch │ ├── corridor_env_single_robot.launch │ ├── corridor_far_env_two_robots.launch │ ├── loop_env_close_two_robots.launch │ ├── loop_env_far_two_robots.launch │ ├── loop_env_single_robot.launch │ ├── loop_with_corridor_close_env_two_robots.launch │ ├── loop_with_corridor_env_single_robot.launch │ ├── loop_with_corridor_far_env_two_robots.launch │ ├── room_close_env_two_robots.launch │ ├── room_env_single_robot.launch │ ├── room_far_env_two_robots.launch │ ├── room_with_corner_close_env_two_robots.launch │ ├── room_with_corner_env_single_robot.launch │ └── room_with_corner_far_env_two_robots.launch ├── package.xml ├── param_teb ├── costmap_common_params_burger.yaml ├── global_costmap_params.yaml ├── local_costmap_params.yaml ├── move_base_params.yaml └── teb_local_planner_params_burger.yaml ├── script ├── ImuChange.py ├── exploration_metric_for_two_robots.py └── publish_tf_for_two_robots.py ├── urdf └── turtlebot3_burger.urdf.xacro └── worlds ├── corner.world ├── corridor.world ├── loop.world ├── loop_with_corridor.world ├── room.world └── room_with_corner.world /.gitignore: -------------------------------------------------------------------------------- 1 | __pycache__/ 2 | */__pycache__/ 3 | *.pyc 4 | */gif/ 5 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/melodic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /exploration_benchmark/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(exploration_benchmark) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | actionlib 6 | actionlib_msgs 7 | cartographer_ros_msgs 8 | geometry_msgs 9 | message_generation 10 | message_runtime 11 | nav_msgs 12 | roscpp 13 | rospy 14 | std_msgs 15 | tf 16 | visualization_msgs 17 | message_filters 18 | ) 19 | 20 | generate_messages( 21 | DEPENDENCIES 22 | std_msgs 23 | geometry_msgs 24 | actionlib_msgs 25 | ) 26 | 27 | catkin_package( 28 | CATKIN_DEPENDS actionlib_msgs 29 | ) 30 | 31 | include_directories(include ${catkin_INCLUDE_DIRS}) 32 | 33 | add_executable(rrt_node src/rrt_node_tf.cpp src/functions.cpp src/mtrand.cpp) 34 | target_link_libraries(rrt_node ${catkin_LIBRARIES}) 35 | 36 | add_executable(mmpf_node src/mmpf_node.cpp) 37 | target_link_libraries(mmpf_node ${catkin_LIBRARIES}) 38 | 39 | add_executable(cost_node src/cost_node.cpp) 40 | target_link_libraries(cost_node ${catkin_LIBRARIES}) 41 | -------------------------------------------------------------------------------- /exploration_benchmark/blueprints/corner.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/exploration_benchmark/blueprints/corner.pgm -------------------------------------------------------------------------------- /exploration_benchmark/blueprints/corner.yaml: -------------------------------------------------------------------------------- 1 | image: corner.pgm 2 | resolution: 0.100000 3 | origin: [-12.500000, -12.500000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /exploration_benchmark/blueprints/corridor.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/exploration_benchmark/blueprints/corridor.pgm -------------------------------------------------------------------------------- /exploration_benchmark/blueprints/corridor.yaml: -------------------------------------------------------------------------------- 1 | image: corridor.pgm 2 | resolution: 0.100000 3 | origin: [-12.500000, -12.500000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /exploration_benchmark/blueprints/loop.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/exploration_benchmark/blueprints/loop.pgm -------------------------------------------------------------------------------- /exploration_benchmark/blueprints/loop.yaml: -------------------------------------------------------------------------------- 1 | image: square_loop.pgm 2 | resolution: 0.100000 3 | origin: [-12.500000, -12.500000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /exploration_benchmark/blueprints/loop_with_corridor.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/exploration_benchmark/blueprints/loop_with_corridor.pgm -------------------------------------------------------------------------------- /exploration_benchmark/blueprints/loop_with_corridor.yaml: -------------------------------------------------------------------------------- 1 | image: loop_with_corridor_sym.pgm 2 | resolution: 0.100000 3 | origin: [-12.500000, -12.500000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /exploration_benchmark/blueprints/room.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/exploration_benchmark/blueprints/room.pgm -------------------------------------------------------------------------------- /exploration_benchmark/blueprints/room.yaml: -------------------------------------------------------------------------------- 1 | image: room1_modified.pgm 2 | resolution: 0.100000 3 | origin: [-12.500000, -12.500000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /exploration_benchmark/blueprints/room_with_corner.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/exploration_benchmark/blueprints/room_with_corner.pgm -------------------------------------------------------------------------------- /exploration_benchmark/blueprints/room_with_corner.yaml: -------------------------------------------------------------------------------- 1 | image: room_with_corner.pgm 2 | resolution: 0.100000 3 | origin: [-12.500000, -12.500000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /exploration_benchmark/include/functions.h: -------------------------------------------------------------------------------- 1 | #ifndef functions_H 2 | #define functions_H 3 | #include "ros/ros.h" 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "nav_msgs/OccupancyGrid.h" 9 | #include "geometry_msgs/Point.h" 10 | #include "visualization_msgs/Marker.h" 11 | 12 | // rdm class, for gentaring random flot numbers 13 | class rdm{ 14 | int i; 15 | public: 16 | rdm(); 17 | float randomize(); 18 | }; 19 | 20 | 21 | //Norm function prototype 22 | float Norm( std::vector , std::vector ); 23 | 24 | //sign function prototype 25 | float sign(float ); 26 | 27 | //Nearest function prototype 28 | std::vector Nearest( std::vector< std::vector > , std::vector ); 29 | 30 | //Steer function prototype 31 | std::vector Steer( std::vector, std::vector, float ); 32 | 33 | //gridValue function prototype 34 | int gridValue(nav_msgs::OccupancyGrid &,std::vector); 35 | 36 | //ObstacleFree function prototype 37 | char ObstacleFree(std::vector , std::vector & , nav_msgs::OccupancyGrid); 38 | #endif 39 | -------------------------------------------------------------------------------- /exploration_benchmark/launch/single_cost_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 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /exploration_benchmark/launch/single_mmpf_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 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /exploration_benchmark/launch/single_rl_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /exploration_benchmark/launch/single_rrt_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 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /exploration_benchmark/launch/two_cost_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 | 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 | -------------------------------------------------------------------------------- /exploration_benchmark/launch/two_mmpf_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 | 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 | -------------------------------------------------------------------------------- /exploration_benchmark/launch/two_rl_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /exploration_benchmark/launch/two_rrt_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 | 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 | -------------------------------------------------------------------------------- /exploration_benchmark/msg/PointArray.msg: -------------------------------------------------------------------------------- 1 | # An array of points 2 | 3 | geometry_msgs/Point[] points 4 | -------------------------------------------------------------------------------- /exploration_benchmark/msg/PointStampedArray.msg: -------------------------------------------------------------------------------- 1 | # An array of points 2 | 3 | geometry_msgs/PointStamped[] pointstamps 4 | -------------------------------------------------------------------------------- /exploration_benchmark/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | exploration_benchmark 4 | 0.0.0 5 | The explore-bench package 6 | 7 | 8 | 9 | 10 | nics 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 | actionlib 53 | actionlib_msgs 54 | cartographer_ros_msgs 55 | geometry_msgs 56 | message_generation 57 | nav_msgs 58 | roscpp 59 | rospy 60 | std_msgs 61 | tf 62 | visualization_msgs 63 | actionlib 64 | actionlib_msgs 65 | cartographer_ros_msgs 66 | geometry_msgs 67 | nav_msgs 68 | roscpp 69 | rospy 70 | std_msgs 71 | tf 72 | visualization_msgs 73 | actionlib 74 | actionlib_msgs 75 | cartographer_ros_msgs 76 | geometry_msgs 77 | message_runtime 78 | nav_msgs 79 | roscpp 80 | rospy 81 | std_msgs 82 | tf 83 | visualization_msgs 84 | message_filters 85 | message_filters 86 | message_filters 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | -------------------------------------------------------------------------------- /exploration_benchmark/scripts/DRL/models/actor.pt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/exploration_benchmark/scripts/DRL/models/actor.pt -------------------------------------------------------------------------------- /exploration_benchmark/scripts/DRL/models/critic.pt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/exploration_benchmark/scripts/DRL/models/critic.pt -------------------------------------------------------------------------------- /exploration_benchmark/scripts/DRL/run_single_robot.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | env="Gazebo" 3 | num_agents=1 4 | algo="mappo" 5 | exp="robot1" 6 | 7 | echo "env is ${env}, algo is ${algo}, exp is ${exp1}" 8 | CUDA_VISIBLE_DEVICES=0 python single_robot_1.py \ 9 | --env_name ${env} --algorithm_name ${algo} --experiment_name ${exp1} \ 10 | --user_name "xyf" --num_agents ${num_agents} \ 11 | --cnn_layers_params '16,3,1,1 32,3,1,1 16,3,1,1' --hidden_size 64 \ 12 | --use_merge --use_recurrent_policy 13 | -------------------------------------------------------------------------------- /exploration_benchmark/scripts/DRL/run_two_robots.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | env="Gazebo" 3 | num_agents=2 4 | algo="mappo" 5 | exp1="robot1" 6 | exp2="robot2" 7 | 8 | echo "env is ${env}, algo is ${algo}, exp is ${exp1}" 9 | CUDA_VISIBLE_DEVICES=0 python two_robot_1.py \ 10 | --env_name ${env} --algorithm_name ${algo} --experiment_name ${exp1} \ 11 | --user_name "xyf" --num_agents ${num_agents} \ 12 | --cnn_layers_params '16,3,1,1 32,3,1,1 16,3,1,1' --hidden_size 64 \ 13 | --use_merge --use_recurrent_policy & 14 | 15 | echo "env is ${env}, algo is ${algo}, exp is ${exp2}" 16 | CUDA_VISIBLE_DEVICES=0 python two_robot_2.py \ 17 | --env_name ${env} --algorithm_name ${algo} --experiment_name ${exp2} \ 18 | --user_name "xyf" --num_agents ${num_agents} \ 19 | --cnn_layers_params '16,3,1,1 32,3,1,1 16,3,1,1' --hidden_size 64 \ 20 | --use_merge --use_recurrent_policy & 21 | -------------------------------------------------------------------------------- /exploration_benchmark/scripts/DRL/window.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import numpy as np 3 | 4 | # Only ask users to install matplotlib if they actually need it 5 | try: 6 | import matplotlib.pyplot as plt 7 | except: 8 | print('To display the environment in a window, please install matplotlib, eg:') 9 | print('pip3 install --user matplotlib') 10 | sys.exit(-1) 11 | 12 | class Window: 13 | """ 14 | Window to draw a gridworld instance using Matplotlib 15 | """ 16 | 17 | def __init__(self, title): 18 | self.fig = None 19 | 20 | self.imshow_obj = None 21 | 22 | # Create the figure and axes 23 | self.fig, self.ax = plt.subplots() 24 | 25 | # Show the env name in the window title 26 | self.fig.canvas.set_window_title(title) 27 | 28 | # Turn off x/y axis numbering/ticks 29 | self.ax.xaxis.set_ticks_position('none') 30 | self.ax.yaxis.set_ticks_position('none') 31 | _ = self.ax.set_xticklabels([]) 32 | _ = self.ax.set_yticklabels([]) 33 | 34 | # Flag indicating the window was closed 35 | self.closed = False 36 | 37 | def close_handler(evt): 38 | self.closed = True 39 | 40 | self.fig.canvas.mpl_connect('close_event', close_handler) 41 | 42 | def show_img(self, img): 43 | """ 44 | Show an image or update the image being shown 45 | """ 46 | 47 | # Show the first image of the environment 48 | if self.imshow_obj is None: 49 | self.imshow_obj = self.ax.imshow(img, interpolation='bilinear') 50 | 51 | self.imshow_obj.set_data(img) 52 | self.fig.canvas.draw() 53 | 54 | # Let matplotlib process UI events 55 | # This is needed for interactive mode to work properly 56 | plt.pause(0.001) 57 | 58 | def set_caption(self, text): 59 | """ 60 | Set/update the caption text below the image 61 | """ 62 | 63 | plt.xlabel(text) 64 | 65 | def reg_key_handler(self, key_handler): 66 | """ 67 | Register a keyboard event handler 68 | """ 69 | 70 | # Keyboard handler 71 | self.fig.canvas.mpl_connect('key_press_event', key_handler) 72 | 73 | def show(self, block=True): 74 | """ 75 | Show the window, and start an event loop 76 | """ 77 | 78 | # If not blocking, trigger interactive mode 79 | if not block: 80 | plt.ion() 81 | 82 | # Show the plot 83 | # In non-interative mode, this enters the matplotlib event loop 84 | # In interactive mode, this call does not block 85 | plt.show() 86 | 87 | def close(self): 88 | """ 89 | Close the window 90 | """ 91 | 92 | plt.close() 93 | self.closed = True -------------------------------------------------------------------------------- /exploration_benchmark/scripts/exploration_metric_for_single_robot.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | ''' 4 | subscribe /robot1/scan_map /robot2/scan_map /robot1/map /robot2/map /robot1/odom /robot2/odom 5 | 6 | ''' 7 | import sys 8 | import time 9 | import os 10 | import numpy as np 11 | import rospy 12 | from std_msgs.msg import String 13 | from nav_msgs.msg import OccupancyGrid, Odometry 14 | from geometry_msgs.msg import PointStamped 15 | import tf 16 | import pickle 17 | import yaml 18 | from PIL import Image 19 | import math 20 | 21 | exploration_rate_log = [] 22 | odom_log = [] 23 | path_length_log = [] 24 | time_log = [] 25 | end_flag = False 26 | achieve_90 = False 27 | start_time = 0 28 | 29 | gt_area = 0 30 | 31 | num_robots = 1 32 | 33 | single_map_list = [OccupancyGrid() for i in range(num_robots)] 34 | single_robot_coverage_rate_list = [0 for i in range(num_robots)] 35 | 36 | def get_gt(pgm_file, yaml_file): 37 | map_img = np.array(Image.open(pgm_file)) 38 | map_info = yaml.load(open(yaml_file, mode='r')) 39 | gt_area = (np.sum((map_img != 205).astype(int)))*map_info['resolution']*map_info['resolution'] 40 | return gt_area 41 | 42 | def callback(data): 43 | # -1:unkown 0:free 100:obstacle 44 | global end_flag, start_time, achieve_90 45 | if not end_flag: 46 | gridmap = np.array(data.data).reshape((data.info.height, data.info.width)) 47 | explored_map = (gridmap != -1).astype(int) 48 | explored_area = explored_map.sum()*data.info.resolution*data.info.resolution 49 | exploration_rate = explored_area / gt_area 50 | exploration_rate_over_time = dict() 51 | exploration_rate_over_time['time'] = data.header.stamp 52 | exploration_rate_over_time['rate'] = exploration_rate 53 | 54 | exploration_rate_log.append(exploration_rate_over_time) 55 | if exploration_rate >= 0.9 and (not achieve_90): 56 | print("achieve 0.9 coverage rate!") 57 | print("T_90: ", time.time()-start_time) 58 | achieve_90 = True 59 | # print(exploration_rate_log) 60 | if exploration_rate >= 0.99: 61 | print("exploration ends!") 62 | print("T_total: ", time.time()-start_time) 63 | # # compute coverage std 64 | # coverage_std = np.std(np.array(single_robot_coverage_rate_list)) 65 | # print("exploration coverage std: ", coverage_std) 66 | # # compute overlap rate 67 | # overlap_rate = np.sum(np.array(single_robot_coverage_rate_list)) - 1 68 | # print("exploration overlap rate: ", overlap_rate) 69 | 70 | end_flag = True 71 | time.sleep(1) 72 | else: 73 | time.sleep(1) 74 | 75 | def odom_callback(data): 76 | global end_flag 77 | if not end_flag: 78 | current_pos = [data.pose.pose.position.x, data.pose.pose.position.y] 79 | odom_over_time = dict() 80 | odom_over_time['time'] = data.header.stamp 81 | odom_over_time['odom'] = current_pos 82 | if len(odom_log) == 0: 83 | odom_log.append(odom_over_time) 84 | path_length_over_time = dict() 85 | path_length_over_time['time'] = data.header.stamp 86 | path_length_over_time['path_length'] = 0 87 | path_length_log.append(path_length_over_time) 88 | else: 89 | path_length_over_time = dict() 90 | path_length_over_time['time'] = data.header.stamp 91 | path_length_over_time['path_length'] = path_length_log[-1]['path_length'] + math.hypot(odom_log[-1]['odom'][0]-current_pos[0], odom_log[-1]['odom'][1]-current_pos[1]) 92 | path_length_log.append(path_length_over_time) 93 | odom_log.append(odom_over_time) 94 | time.sleep(1) 95 | else: 96 | time.sleep(1) 97 | 98 | def single_map_callback(data): 99 | global single_map_list 100 | # print(int(data.header.frame_id[5])) 101 | single_map_list[int(data.header.frame_id[5])-1] = data 102 | 103 | def single_robot_coverage_rate_callback(data): 104 | global single_robot_coverage_rate_list, gt_area 105 | gridmap = np.array(data.data).reshape((data.info.height, data.info.width)) 106 | explored_map = (gridmap != -1).astype(int) 107 | explored_area = explored_map.sum()*data.info.resolution*data.info.resolution 108 | exploration_rate = explored_area / gt_area 109 | single_robot_coverage_rate_list[int(data.header.frame_id[5])-1] = exploration_rate 110 | 111 | def RvizCallback(data): 112 | global start_time 113 | start_time = time.time() 114 | print("Exploration start!") 115 | print("Start time: ", start_time) 116 | 117 | def main(argv): 118 | global gt_area 119 | gt_area = get_gt(argv[1], argv[2]) 120 | rospy.init_node('exploration_metric', anonymous=True) 121 | rospy.Subscriber("/clicked_point", PointStamped, RvizCallback) 122 | rospy.Subscriber("/robot1/map", OccupancyGrid, callback, queue_size=1) 123 | rospy.Subscriber("/robot1/odom", Odometry, odom_callback, queue_size=1) 124 | rospy.spin() 125 | 126 | 127 | if __name__ == '__main__': 128 | main(sys.argv) 129 | 130 | -------------------------------------------------------------------------------- /exploration_benchmark/scripts/publish_tf_for_two_robots.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | import sys 3 | import time 4 | import os 5 | import numpy as np 6 | import rospy 7 | from std_msgs.msg import String, Float32MultiArray 8 | from nav_msgs.msg import OccupancyGrid 9 | from geometry_msgs.msg import Pose, PoseStamped, Transform, TransformStamped, Quaternion 10 | import tf 11 | import pickle 12 | from autolab_core import RigidTransform 13 | 14 | def trans2pose(trans): 15 | pose = Pose() 16 | pose.orientation = trans.rotation 17 | pose.position.x = trans.translation.x 18 | pose.position.y = trans.translation.y 19 | pose.position.z = trans.translation.z 20 | return pose 21 | 22 | def pose2trans(pose): 23 | trans = Transform() 24 | trans.rotation = pose.orientation 25 | trans.translation =pose.position 26 | return trans 27 | 28 | def trans2Rigidtrans(trans, from_frame, to_frame): 29 | rotation_quaternion = np.asarray([trans.rotation.w, trans.rotation.x, trans.rotation.y, trans.rotation.z]) 30 | T_trans = np.asarray([trans.translation.x, trans.translation.y, trans.translation.z]) 31 | T_qua2rota = RigidTransform(rotation_quaternion, T_trans, from_frame = from_frame, to_frame=to_frame) 32 | return T_qua2rota 33 | 34 | def pose2Rigidtrans(pose, from_frame, to_frame): 35 | trans = pose2trans(pose) 36 | T_qua2rota = trans2Rigidtrans(trans, from_frame, to_frame) 37 | return T_qua2rota 38 | 39 | def transstamp2Rigidtrans(trans): 40 | from_frame = trans.child_frame_id 41 | # to_frame = trans.child_frame_id 42 | to_frame = trans.header.frame_id 43 | T_qua2rota = trans2Rigidtrans(trans.transform, from_frame, to_frame) 44 | return T_qua2rota 45 | 46 | def Rigidtrans2transstamped(Rigidtrans): 47 | trans = TransformStamped() 48 | trans.header.stamp = rospy.Time.now() 49 | trans.header.frame_id = Rigidtrans.to_frame 50 | trans.child_frame_id = Rigidtrans.from_frame 51 | trans.transform = pose2trans(Rigidtrans.pose_msg) 52 | return trans 53 | 54 | def tf2Rigidtrans(trans, from_frame, to_frame): 55 | rotation_quaternion = np.asarray([trans[1][3], trans[1][0], trans[1][1], trans[1][2]]) 56 | T_trans = np.asarray([trans[0][0], trans[0][1], trans[0][2]]) 57 | T_qua2rota = RigidTransform(rotation_quaternion, T_trans, from_frame = from_frame, to_frame=to_frame) 58 | return T_qua2rota 59 | 60 | if __name__ == '__main__': 61 | rospy.init_node('test_publish_tf', anonymous=True) 62 | robot1_pub = rospy.Publisher('robot1/robot_pos', Float32MultiArray, queue_size=10) 63 | robot2_pub = rospy.Publisher('robot2/robot_pos', Float32MultiArray, queue_size=10) 64 | br = tf.TransformBroadcaster() 65 | tf_listener = tf.TransformListener() 66 | rate = rospy.Rate(10) 67 | # from, source: child_id to, target: frame_id 68 | while not rospy.is_shutdown(): 69 | try: 70 | #now = rospy.Time.now() 71 | #tf_listener.waitForTransform('robot1/map','robot1/odom', rospy.Time.now(), rospy.Duration(10.0)) 72 | robot1_map_to_odom = tf_listener.lookupTransform('robot1/map', 'robot1/odom', rospy.Time(0)) 73 | robot2_map_to_odom = tf_listener.lookupTransform('robot2/map', 'robot2/odom', rospy.Time(0)) 74 | except: 75 | print('no tf') 76 | rate.sleep() 77 | continue 78 | robot1_map_to_odom_Rigidtrans = tf2Rigidtrans(robot1_map_to_odom, 'robot1/odom', 'robot1/map') 79 | robot2_map_to_odom_Rigidtrans = tf2Rigidtrans(robot2_map_to_odom, 'robot2/odom', 'robot2/map') 80 | robot1_odom_to_robot2_odom_Rigidtrans = tf2Rigidtrans(([0,0,0],[0,0,0,1]), 'robot2/odom', 'robot1/odom') 81 | robot1_map_to_robot2_map_Rigidtrans = robot1_map_to_odom_Rigidtrans*robot1_odom_to_robot2_odom_Rigidtrans*robot2_map_to_odom_Rigidtrans.inverse() 82 | 83 | tf = Rigidtrans2transstamped(robot1_map_to_robot2_map_Rigidtrans) 84 | # print("publish tf: " + tf.header.frame_id + " to " + tf.child_frame_id) 85 | br.sendTransformMessage(tf) 86 | # send robot pos 87 | try: 88 | #now = rospy.Time.now() 89 | #tf_listener.waitForTransform('robot1/map','robot1/odom', rospy.Time.now(), rospy.Duration(10.0)) 90 | robot1_map_to_base = tf_listener.lookupTransform('robot1/map', 'robot1/base_footprint', rospy.Time(0)) 91 | robot2_map_to_base = tf_listener.lookupTransform('robot2/map', 'robot2/base_footprint', rospy.Time(0)) 92 | except: 93 | print('no tf') 94 | rate.sleep() 95 | continue 96 | robot1_pos_msg = Float32MultiArray() 97 | robot1_pos_msg.data.append(robot1_map_to_base[0][0]) 98 | robot1_pos_msg.data.append(robot1_map_to_base[0][1]) 99 | robot2_pos_msg = Float32MultiArray() 100 | robot2_pos_msg.data.append(robot2_map_to_base[0][0]) 101 | robot2_pos_msg.data.append(robot2_map_to_base[0][1]) 102 | robot1_pub.publish(robot1_pos_msg) 103 | robot2_pub.publish(robot2_pos_msg) 104 | time.sleep(1) -------------------------------------------------------------------------------- /exploration_benchmark/src/functions.cpp: -------------------------------------------------------------------------------- 1 | #include "functions.h" 2 | 3 | 4 | // rdm class, for gentaring random flot numbers 5 | rdm::rdm() {i=time(0);} 6 | float rdm::randomize() { i=i+1; srand (i); return float(rand())/float(RAND_MAX);} 7 | 8 | 9 | 10 | //Norm function 11 | float Norm(std::vector x1,std::vector x2) 12 | { 13 | return pow( (pow((x2[0]-x1[0]),2)+pow((x2[1]-x1[1]),2)) ,0.5); 14 | } 15 | 16 | 17 | //sign function 18 | float sign(float n) 19 | { 20 | if (n<0.0){return -1.0;} 21 | else{return 1.0;} 22 | } 23 | 24 | 25 | //Nearest function 26 | std::vector Nearest( std::vector< std::vector > V, std::vector x){ 27 | 28 | float min=Norm(V[0],x); 29 | int min_index; 30 | float temp; 31 | 32 | for (int j=0;j Steer( std::vector x_nearest , std::vector x_rand, float eta){ 47 | std::vector x_new; 48 | 49 | if (Norm(x_nearest,x_rand)<=eta){ 50 | x_new=x_rand; 51 | } 52 | else{ 53 | float m=(x_rand[1]-x_nearest[1])/(x_rand[0]-x_nearest[0]); 54 | 55 | x_new.push_back( (sign(x_rand[0]-x_nearest[0]))* ( sqrt( (pow(eta,2)) / ((pow(m,2))+1) ) )+x_nearest[0] ); 56 | x_new.push_back( m*(x_new[0]-x_nearest[0])+x_nearest[1] ); 57 | 58 | } 59 | return x_new; 60 | } 61 | 62 | 63 | 64 | 65 | 66 | //gridValue function 67 | int gridValue(nav_msgs::OccupancyGrid &mapData,std::vector Xp){ 68 | 69 | float resolution=mapData.info.resolution; 70 | float Xstartx=mapData.info.origin.position.x; 71 | float Xstarty=mapData.info.origin.position.y; 72 | 73 | float width=mapData.info.width; 74 | std::vector Data=mapData.data; 75 | 76 | //returns grid value at "Xp" location 77 | //map data: 100 occupied -1 unknown 0 free 78 | float indx=( floor((Xp[1]-Xstarty)/resolution)*width)+( floor((Xp[0]-Xstartx)/resolution) ); 79 | 80 | return Data[int(indx)]; 81 | } 82 | 83 | 84 | 85 | // ObstacleFree function------------------------------------- 86 | // rrt cannot grow through obstacle or unknown 87 | char ObstacleFree(std::vector xnear, std::vector &xnew, nav_msgs::OccupancyGrid mapsub){ 88 | float resolution = mapsub.info.resolution; 89 | float rez=resolution*.2; 90 | float stepz=int(ceil(Norm(xnew,xnear))/rez); 91 | std::vector xi=xnear; 92 | char obs=0; char unk=0; char out=0; 93 | geometry_msgs::Point p; 94 | enum enumType {Free='0', Frontier='2', Obstacle='1'}; 95 | for (int c=0;c1){xnew=xi; return enumType::Frontier;} 111 | // else{return enumType::Obstacle;} 112 | // } 113 | } 114 | return enumType::Free; 115 | } 116 | -------------------------------------------------------------------------------- /exploration_benchmark/src/mtrand.cpp: -------------------------------------------------------------------------------- 1 | // mtrand.cpp, see include file mtrand.h for information 2 | 3 | #include "mtrand.h" 4 | // non-inline function definitions and static member definitions cannot 5 | // reside in header file because of the risk of multiple declarations 6 | 7 | // initialization of static private members 8 | unsigned long MTRand_int32::state[n] = {0x0UL}; 9 | int MTRand_int32::p = 0; 10 | bool MTRand_int32::init = false; 11 | 12 | void MTRand_int32::gen_state() { // generate new state vector 13 | for (int i = 0; i < (n - m); ++i) 14 | state[i] = state[i + m] ^ twiddle(state[i], state[i + 1]); 15 | for (int i = n - m; i < (n - 1); ++i) 16 | state[i] = state[i + m - n] ^ twiddle(state[i], state[i + 1]); 17 | state[n - 1] = state[m - 1] ^ twiddle(state[n - 1], state[0]); 18 | p = 0; // reset position 19 | } 20 | 21 | void MTRand_int32::seed(unsigned long s) { // init by 32 bit seed 22 | state[0] = s & 0xFFFFFFFFUL; // for > 32 bit machines 23 | for (int i = 1; i < n; ++i) { 24 | state[i] = 1812433253UL * (state[i - 1] ^ (state[i - 1] >> 30)) + i; 25 | // see Knuth TAOCP Vol2. 3rd Ed. P.106 for multiplier 26 | // in the previous versions, MSBs of the seed affect only MSBs of the array state 27 | // 2002/01/09 modified by Makoto Matsumoto 28 | state[i] &= 0xFFFFFFFFUL; // for > 32 bit machines 29 | } 30 | p = n; // force gen_state() to be called for next random number 31 | } 32 | 33 | void MTRand_int32::seed(const unsigned long* array, int size) { // init by array 34 | seed(19650218UL); 35 | int i = 1, j = 0; 36 | for (int k = ((n > size) ? n : size); k; --k) { 37 | state[i] = (state[i] ^ ((state[i - 1] ^ (state[i - 1] >> 30)) * 1664525UL)) 38 | + array[j] + j; // non linear 39 | state[i] &= 0xFFFFFFFFUL; // for > 32 bit machines 40 | ++j; j %= size; 41 | if ((++i) == n) { state[0] = state[n - 1]; i = 1; } 42 | } 43 | for (int k = n - 1; k; --k) { 44 | state[i] = (state[i] ^ ((state[i - 1] ^ (state[i - 1] >> 30)) * 1566083941UL)) - i; 45 | state[i] &= 0xFFFFFFFFUL; // for > 32 bit machines 46 | if ((++i) == n) { state[0] = state[n - 1]; i = 1; } 47 | } 48 | state[0] = 0x80000000UL; // MSB is 1; assuring non-zero initial array 49 | p = n; // force gen_state() to be called for next random number 50 | } 51 | -------------------------------------------------------------------------------- /graythree/launch/mapmerge.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /graythree/launch/mapmerge_real_single_car.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /graythree/launch/mapmerge_real_two_cars.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /graythree/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | graythree 4 | 0.0.0 5 | The mapmerge package 6 | 7 | 8 | 9 | 10 | jimmy 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 | geometry_msgs 53 | message_generation 54 | nav_msgs 55 | roscpp 56 | rospy 57 | std_msgs 58 | tf 59 | visualization_msgs 60 | geometry_msgs 61 | nav_msgs 62 | roscpp 63 | rospy 64 | std_msgs 65 | tf 66 | visualization_msgs 67 | geometry_msgs 68 | nav_msgs 69 | roscpp 70 | rospy 71 | std_msgs 72 | tf 73 | visualization_msgs 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | -------------------------------------------------------------------------------- /graythree/src/mapmerge_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "ros/ros.h" 3 | #include "nav_msgs/OccupancyGrid.h" 4 | #include 5 | 6 | nav_msgs::OccupancyGrid CartographerMap; 7 | nav_msgs::OccupancyGrid grid_; 8 | unsigned char* cost_translation_table_ = new unsigned char[256]; 9 | 10 | void mapCallBack(const nav_msgs::OccupancyGrid::ConstPtr& msg) 11 | { 12 | CartographerMap=*msg; 13 | // ROS_INFO("gointo callback"); 14 | } 15 | 16 | 17 | int main(int argc, char **argv){ 18 | 19 | ros::init(argc, argv, "cartographer2discrete"); 20 | ros::NodeHandle n; 21 | ros::Subscriber sub=n.subscribe("cartographerMap", 1, mapCallBack); 22 | ros::Publisher pub=n.advertise("map", 1); 23 | ros::Rate loop_rate(1); 24 | 25 | int lower_threshold = 50; 26 | int upper_threshold = 51; 27 | 28 | std::string ns; 29 | 30 | 31 | ns=ros::this_node::getName(); 32 | // ros::param::param(ns+"/threshold", threshold, 5); 33 | 34 | 35 | if (cost_translation_table_ != NULL) 36 | { 37 | // special values: 38 | for (int i = 0; i < lower_threshold; i++) 39 | { 40 | cost_translation_table_[ i ] = 0; 41 | } 42 | for (int i = lower_threshold; i < upper_threshold; i++) 43 | { 44 | cost_translation_table_[ i ] = 255; 45 | } 46 | for (int i = upper_threshold; i < 255; i++) 47 | { 48 | cost_translation_table_[ i ] = 100; 49 | } 50 | cost_translation_table_[255] = 255; // unknown 51 | } 52 | 53 | 54 | while(ros::ok()) 55 | { 56 | ros::spinOnce(); 57 | grid_.header = CartographerMap.header; 58 | grid_.info = CartographerMap.info; 59 | grid_.info.map_load_time = grid_.header.stamp; 60 | grid_.data.resize(CartographerMap.info.width * CartographerMap.info.height); 61 | 62 | 63 | for (unsigned int i = 0; i < grid_.data.size(); i++) 64 | { 65 | unsigned char temp = CartographerMap.data[ i ]; 66 | 67 | grid_.data[i] = cost_translation_table_[int(temp)]; 68 | // if(temp == 255){ROS_INFO("%d -> %d: using %d", int(temp), cost_translation_table_[int(temp)], cost_translation_table_[255]);} 69 | } 70 | 71 | pub.publish(grid_); 72 | loop_rate.sleep(); 73 | } 74 | 75 | return 0; 76 | } 77 | -------------------------------------------------------------------------------- /grid_simulator/window.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import numpy as np 3 | 4 | # Only ask users to install matplotlib if they actually need it 5 | try: 6 | import matplotlib.pyplot as plt 7 | except: 8 | print('To display the environment in a window, please install matplotlib, eg:') 9 | print('pip3 install --user matplotlib') 10 | sys.exit(-1) 11 | 12 | class Window: 13 | """ 14 | Window to draw a gridworld instance using Matplotlib 15 | """ 16 | 17 | def __init__(self, title): 18 | self.fig = None 19 | 20 | self.imshow_obj = None 21 | 22 | # Create the figure and axes 23 | self.fig, self.ax = plt.subplots() 24 | 25 | # Show the env name in the window title 26 | self.fig.canvas.set_window_title(title) 27 | 28 | # Turn off x/y axis numbering/ticks 29 | self.ax.xaxis.set_ticks_position('none') 30 | self.ax.yaxis.set_ticks_position('none') 31 | _ = self.ax.set_xticklabels([]) 32 | _ = self.ax.set_yticklabels([]) 33 | 34 | # Flag indicating the window was closed 35 | self.closed = False 36 | 37 | def close_handler(evt): 38 | self.closed = True 39 | 40 | self.fig.canvas.mpl_connect('close_event', close_handler) 41 | 42 | def show_img(self, img): 43 | """ 44 | Show an image or update the image being shown 45 | """ 46 | 47 | # Show the first image of the environment 48 | if self.imshow_obj is None: 49 | self.imshow_obj = self.ax.imshow(img, interpolation='bilinear') 50 | 51 | self.imshow_obj.set_data(img) 52 | self.fig.canvas.draw() 53 | 54 | # Let matplotlib process UI events 55 | # This is needed for interactive mode to work properly 56 | plt.pause(0.001) 57 | 58 | def set_caption(self, text): 59 | """ 60 | Set/update the caption text below the image 61 | """ 62 | 63 | plt.xlabel(text) 64 | 65 | def reg_key_handler(self, key_handler): 66 | """ 67 | Register a keyboard event handler 68 | """ 69 | 70 | # Keyboard handler 71 | self.fig.canvas.mpl_connect('key_press_event', key_handler) 72 | 73 | def show(self, block=True): 74 | """ 75 | Show the window, and start an event loop 76 | """ 77 | 78 | # If not blocking, trigger interactive mode 79 | if not block: 80 | plt.ion() 81 | 82 | # Show the plot 83 | # In non-interative mode, this enters the matplotlib event loop 84 | # In interactive mode, this call does not block 85 | plt.show() 86 | 87 | def close(self): 88 | """ 89 | Close the window 90 | """ 91 | 92 | plt.close() 93 | self.closed = True -------------------------------------------------------------------------------- /map_merge/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package multirobot_map_merge 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.1.1 (2017-12-16) 6 | ------------------ 7 | * fix bugs in CMakeLists.txt: install nodes in packages, so they get shipped in debian packages. fixes `#11 `_ 8 | * map_merge: add bibtex to wiki page 9 | * Contributors: Jiri Horner 10 | 11 | 2.1.0 (2017-10-30) 12 | ------------------ 13 | * no major changes. Released together with explore_lite. 14 | 15 | 2.0.0 (2017-03-26) 16 | ------------------ 17 | * map_merge: upgrade to package format 2 18 | * node completely rewritten based on my work included in opencv 19 | * uses more reliable features by default -> more robust merging 20 | * known_init_poses is now by default false to make it easy to start for new users 21 | * Contributors: Jiri Horner 22 | 23 | 1.0.1 (2017-03-25) 24 | ------------------ 25 | * map_merge: use inverted tranform 26 | * transform needs to be inverted before using 27 | * map_merge: change package description 28 | * we support merging with unknown initial positions 29 | * Contributors: Jiri Horner 30 | 31 | 1.0.0 (2016-05-11) 32 | ------------------ 33 | * initial release 34 | * Contributors: Jiri Horner 35 | -------------------------------------------------------------------------------- /map_merge/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1) 2 | project(multirobot_map_merge) 3 | 4 | ## Find catkin macros and libraries 5 | find_package(catkin REQUIRED COMPONENTS 6 | message_generation 7 | geometry_msgs 8 | image_geometry 9 | map_msgs 10 | nav_msgs 11 | roscpp 12 | tf2_geometry_msgs 13 | tf 14 | ) 15 | 16 | find_package(Boost REQUIRED COMPONENTS thread) 17 | 18 | # OpenCV is required for merging without initial positions 19 | find_package(OpenCV 3 REQUIRED) 20 | if("${OpenCV_VERSION}" VERSION_LESS "3.0") 21 | message(FATAL_ERROR "This package needs OpenCV >= 3.0") 22 | endif() 23 | 24 | ################################################ 25 | ## Declare ROS messages, services and actions ## 26 | ################################################ 27 | add_service_files( 28 | FILES 29 | mapPair2tf.srv 30 | ) 31 | 32 | generate_messages( 33 | DEPENDENCIES 34 | std_msgs 35 | nav_msgs 36 | geometry_msgs 37 | ) 38 | 39 | ################################### 40 | ## catkin specific configuration ## 41 | ################################### 42 | catkin_package( 43 | CATKIN_DEPENDS 44 | geometry_msgs 45 | map_msgs 46 | nav_msgs 47 | tf2_geometry_msgs 48 | ) 49 | 50 | ########### 51 | ## Build ## 52 | ########### 53 | # c++11 support required 54 | set(CMAKE_CXX_STANDARD 11) 55 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 56 | set(CMAKE_CXX_EXTENSIONS OFF) 57 | 58 | ## Specify additional locations of header files 59 | include_directories( 60 | ${catkin_INCLUDE_DIRS} 61 | ${Boost_INCLUDE_DIRS} 62 | ${OpenCV_INCLUDE_DIRS} 63 | include 64 | ) 65 | 66 | # we want static linking for now 67 | add_library(combine_grids STATIC 68 | src/combine_grids/grid_compositor.cpp 69 | src/combine_grids/grid_warper.cpp 70 | src/combine_grids/merging_pipeline.cpp 71 | ) 72 | add_dependencies(combine_grids ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 73 | target_link_libraries(combine_grids ${OpenCV_LIBRARIES}) 74 | 75 | add_executable(map_merge 76 | src/map_merge_new.cpp 77 | ) 78 | add_dependencies(map_merge ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 79 | target_link_libraries(map_merge combine_grids ${catkin_LIBRARIES}) 80 | 81 | add_executable(map_register 82 | src/map_register.cpp 83 | ) 84 | add_dependencies(map_register ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 85 | target_link_libraries(map_register combine_grids ${catkin_LIBRARIES}) 86 | 87 | 88 | ############# 89 | ## Install ## 90 | ############# 91 | 92 | # install nodes, installing combine_grids should not be necessary, 93 | # but lets make catkin_lint happy 94 | install(TARGETS combine_grids map_merge 95 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 96 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 97 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 98 | ) 99 | 100 | # install roslaunch files 101 | install(DIRECTORY launch/ 102 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 103 | ) 104 | 105 | ############# 106 | ## Testing ## 107 | ############# 108 | if(CATKIN_ENABLE_TESTING) 109 | find_package(roslaunch REQUIRED) 110 | 111 | # download test data 112 | set(base_url https://raw.githubusercontent.com/hrnr/m-explore-extra/master/map_merge) 113 | catkin_download_test_data(${PROJECT_NAME}_map00.pgm ${base_url}/hector_maps/map00.pgm MD5 915609a85793ec1375f310d44f2daf87) 114 | catkin_download_test_data(${PROJECT_NAME}_map05.pgm ${base_url}/hector_maps/map05.pgm MD5 cb9154c9fa3d97e5e992592daca9853a) 115 | catkin_download_test_data(${PROJECT_NAME}_2011-08-09-12-22-52.pgm ${base_url}/gmapping_maps/2011-08-09-12-22-52.pgm MD5 3c2c38e7dec2b7a67f41069ab58badaa) 116 | catkin_download_test_data(${PROJECT_NAME}_2012-01-28-11-12-01.pgm ${base_url}/gmapping_maps/2012-01-28-11-12-01.pgm MD5 681e704044889c95e47b0c3aadd81f1e) 117 | 118 | catkin_add_gtest(test_merging_pipeline test/test_merging_pipeline.cpp) 119 | # ensure that test data are downloaded before we run tests 120 | add_dependencies(test_merging_pipeline ${PROJECT_NAME}_map00.pgm ${PROJECT_NAME}_map05.pgm ${PROJECT_NAME}_2011-08-09-12-22-52.pgm ${PROJECT_NAME}_2012-01-28-11-12-01.pgm) 121 | target_link_libraries(test_merging_pipeline combine_grids ${catkin_LIBRARIES}) 122 | 123 | # test all launch files 124 | # do not test from_map_server.launch as we don't want to add dependency on map_server and this 125 | # launchfile is not critical 126 | roslaunch_add_file_check(launch/map_merge.launch) 127 | roslaunch_add_file_check(launch/experiments) 128 | endif() 129 | -------------------------------------------------------------------------------- /map_merge/doc/architecture.dia: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/map_merge/doc/architecture.dia -------------------------------------------------------------------------------- /map_merge/doc/screenshot.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/map_merge/doc/screenshot.jpg -------------------------------------------------------------------------------- /map_merge/include/combine_grids/grid_compositor.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015-2016, Jiri Horner. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Jiri Horner nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | *********************************************************************/ 36 | 37 | #ifndef GRID_COMPOSITOR_H_ 38 | #define GRID_COMPOSITOR_H_ 39 | 40 | #include 41 | 42 | #include 43 | 44 | namespace combine_grids 45 | { 46 | namespace internal 47 | { 48 | class GridCompositor 49 | { 50 | public: 51 | nav_msgs::OccupancyGrid::Ptr compose(const std::vector& grids, 52 | const std::vector& rois); 53 | }; 54 | 55 | } // namespace internal 56 | 57 | } // namespace combine_grids 58 | 59 | #endif // GRID_COMPOSITOR_H_ 60 | -------------------------------------------------------------------------------- /map_merge/include/combine_grids/grid_warper.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015-2016, Jiri Horner. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Jiri Horner nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | *********************************************************************/ 36 | 37 | #ifndef GRID_WARPER_H_ 38 | #define GRID_WARPER_H_ 39 | 40 | #include 41 | 42 | namespace combine_grids 43 | { 44 | namespace internal 45 | { 46 | class GridWarper 47 | { 48 | public: 49 | cv::Rect warp(const cv::Mat& grid, const cv::Mat& transform, 50 | cv::Mat& warped_grid); 51 | 52 | private: 53 | cv::Rect warpRoi(const cv::Mat& grid, const cv::Mat& transform); 54 | }; 55 | 56 | } // namespace internal 57 | 58 | } // namespace combine_grids 59 | 60 | #endif // GRID_WARPER_H_ 61 | -------------------------------------------------------------------------------- /map_merge/include/map_merge/map_merge.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2014, Zhi Yan. 6 | * Copyright (c) 2015-2016, Jiri Horner. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the Jiri Horner nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | *********************************************************************/ 37 | 38 | #ifndef MAP_MERGE_H_ 39 | #define MAP_MERGE_H_ 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | 54 | namespace map_merge 55 | { 56 | struct MapSubscription { 57 | // protects consistency of writable_map and readonly_map 58 | // also protects reads and writes of shared_ptrs 59 | std::mutex mutex; 60 | 61 | geometry_msgs::Transform initial_pose; 62 | nav_msgs::OccupancyGrid::Ptr writable_map; 63 | nav_msgs::OccupancyGrid::ConstPtr readonly_map; 64 | 65 | ros::Subscriber map_sub; 66 | ros::Subscriber map_updates_sub; 67 | }; 68 | 69 | class MapMerge 70 | { 71 | private: 72 | ros::NodeHandle node_; 73 | 74 | /* parameters */ 75 | double merging_rate_; 76 | double discovery_rate_; 77 | double estimation_rate_; 78 | double confidence_threshold_; 79 | std::string robot_map_topic_; 80 | std::string robot_map_updates_topic_; 81 | std::string robot_namespace_; 82 | std::string world_frame_; 83 | std::string robot_map_frame_; 84 | bool have_initial_poses_; 85 | 86 | // publishing 87 | ros::Publisher merged_map_publisher_; 88 | // maps robots namespaces to maps. does not own 89 | std::unordered_map robots_; 90 | // owns maps -- iterator safe 91 | std::forward_list subscriptions_; 92 | size_t subscriptions_size_; 93 | boost::shared_mutex subscriptions_mutex_; 94 | combine_grids::MergingPipeline pipeline_; 95 | std::mutex pipeline_mutex_; 96 | 97 | tf::TransformListener listener_; 98 | 99 | std::string robotNameFromTopic(const std::string& topic); 100 | bool isRobotMapTopic(const ros::master::TopicInfo& topic); 101 | bool getInitPose(const std::string& name, geometry_msgs::Transform& pose); 102 | 103 | void fullMapUpdate(const nav_msgs::OccupancyGrid::ConstPtr& msg, 104 | MapSubscription& map); 105 | void partialMapUpdate(const map_msgs::OccupancyGridUpdate::ConstPtr& msg, 106 | MapSubscription& map); 107 | 108 | public: 109 | MapMerge(); 110 | 111 | void spin(); 112 | void executetopicSubscribing(); 113 | void executemapMerging(); 114 | void executeposeEstimation(); 115 | 116 | void topicSubscribing(); 117 | void mapMerging(); 118 | /** 119 | * @brief Estimates initial positions of grids 120 | * @details Relevant only if initial poses are not known 121 | */ 122 | void poseEstimation(); 123 | }; 124 | 125 | } // namespace map_merge 126 | 127 | #endif /* MAP_MERGE_H_ */ 128 | -------------------------------------------------------------------------------- /map_merge/launch/experiments/3-robots.ttt.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /map_merge/launch/experiments/demo-scene-1.ttt.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /map_merge/launch/experiments/demo-scene-orientation-exp.ttt.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /map_merge/launch/map_merge.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /map_merge/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | multirobot_map_merge 4 | 2.1.1 5 | 6 | Merging multiple maps without knowledge of initial 7 | positions of robots. 8 | 9 | Jiri Horner 10 | Jiri Horner 11 | BSD 12 | http://wiki.ros.org/multirobot_map_merge 13 | 14 | catkin 15 | message_generation 16 | roscpp 17 | geometry_msgs 18 | nav_msgs 19 | map_msgs 20 | tf2_geometry_msgs 21 | 22 | image_geometry 23 | message_runtime 24 | 25 | roslaunch 26 | rosunit 27 | 28 | -------------------------------------------------------------------------------- /map_merge/script/msg_remap.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | #coding=utf-8 3 | import rospy 4 | #导入自定义的数据类型 5 | from cartographer_ros_msgs.msg import SubmapList 6 | 7 | class Msg_Remaper: 8 | 9 | def __init__(self): 10 | self.pub = rospy.Publisher("msg_out", SubmapList, queue_size=1) 11 | #Subscriber函数第一个参数是topic的名称，第二个参数是接受的数据类型 第三个参数是回调函数的名称 12 | self.sub = rospy.Subscriber("msg_in", SubmapList, self.callback) 13 | 14 | #回调函数输入的应该是msg 15 | def callback(self, data): 16 | self.pub.publish(data) 17 | 18 | 19 | def main(): 20 | rospy.init_node('msg_remap', anonymous=True) 21 | mr = Msg_Remaper() 22 | rospy.spin() 23 | 24 | if __name__ == '__main__': 25 | main() -------------------------------------------------------------------------------- /map_merge/src/combine_grids/grid_compositor.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015-2016, Jiri Horner. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Jiri Horner nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | *********************************************************************/ 36 | 37 | #include 38 | 39 | #include 40 | 41 | #include 42 | 43 | namespace combine_grids 44 | { 45 | namespace internal 46 | { 47 | void merge_mat(const cv::Mat &src1, const cv::Mat &src2, cv::Mat &dst){ 48 | for(int i=0;i(i,j)>=0 && src2.at(i,j)>=0){ 51 | dst.at(i,j) = cv::min(cv::max((src1.at(i,j)-50) + (src2.at(i,j)-50) + 50, 0), 100); 52 | // if(src1.at(i,j)<50 && src2.at(i,j)<50){ 53 | // dst.at(i,j) = cv::min(src1.at(i,j), src2.at(i,j)); 54 | // } 55 | // else{ 56 | // dst.at(i,j) = cv::max(src1.at(i,j), src2.at(i,j)); 57 | // } 58 | } 59 | else if(src1.at(i,j)>=0){ 60 | dst.at(i,j) = src1.at(i,j); 61 | } 62 | else if(src2.at(i,j)>=0){ 63 | dst.at(i,j) = src2.at(i,j); 64 | } 65 | } 66 | } 67 | } 68 | 69 | nav_msgs::OccupancyGrid::Ptr GridCompositor::compose( 70 | const std::vector& grids, const std::vector& rois) 71 | { 72 | ROS_ASSERT(grids.size() == rois.size()); 73 | 74 | nav_msgs::OccupancyGrid::Ptr result_grid(new nav_msgs::OccupancyGrid()); 75 | 76 | std::vector corners; 77 | corners.reserve(grids.size()); 78 | std::vector sizes; 79 | sizes.reserve(grids.size()); 80 | for (auto& roi : rois) { 81 | corners.push_back(roi.tl()); 82 | sizes.push_back(roi.size()); 83 | } 84 | cv::Rect dst_roi = cv::detail::resultRoi(corners, sizes); 85 | 86 | result_grid->info.width = static_cast(dst_roi.width); 87 | result_grid->info.height = static_cast(dst_roi.height); 88 | result_grid->data.resize(static_cast(dst_roi.area()), -1); 89 | // create view for opencv pointing to newly allocated grid 90 | cv::Mat result(dst_roi.size(), CV_8S, result_grid->data.data()); 91 | 92 | for (size_t i = 0; i < grids.size(); ++i) { 93 | // we need to compensate global offset 94 | cv::Rect roi = cv::Rect(corners[i] - dst_roi.tl(), sizes[i]); 95 | cv::Mat result_roi(result, roi); 96 | // reinterpret warped matrix as signed 97 | // we will not change this matrix, but opencv does not support const matrices 98 | cv::Mat warped_signed (grids[i].size(), CV_8S, const_cast(grids[i].ptr())); 99 | // compose img into result matrix 100 | // cv::max(result_roi, warped_signed, result_roi); 101 | merge_mat(result_roi, warped_signed, result_roi); 102 | } 103 | 104 | return result_grid; 105 | } 106 | 107 | } // namespace internal 108 | 109 | } // namespace combine_grids 110 | -------------------------------------------------------------------------------- /map_merge/src/combine_grids/grid_warper.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015-2016, Jiri Horner. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Jiri Horner nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | *********************************************************************/ 36 | 37 | #include 38 | 39 | #include 40 | 41 | #include 42 | 43 | namespace combine_grids 44 | { 45 | namespace internal 46 | { 47 | cv::Rect GridWarper::warp(const cv::Mat& grid, const cv::Mat& transform, 48 | cv::Mat& warped_grid) 49 | { 50 | ROS_ASSERT(transform.type() == CV_64F); 51 | cv::Mat H; 52 | invertAffineTransform(transform.rowRange(0, 2), H); 53 | cv::Rect roi = warpRoi(grid, H); 54 | // shift top left corner for warp affine (otherwise the image is cropped) 55 | H.at(0, 2) -= roi.tl().x; 56 | H.at(1, 2) -= roi.tl().y; 57 | warpAffine(grid, warped_grid, H, roi.size(), cv::INTER_NEAREST, 58 | cv::BORDER_CONSTANT, 59 | cv::Scalar::all(255) /* this is -1 for signed char */); 60 | ROS_ASSERT(roi.size() == warped_grid.size()); 61 | 62 | return roi; 63 | } 64 | 65 | cv::Rect GridWarper::warpRoi(const cv::Mat& grid, const cv::Mat& transform) 66 | { 67 | cv::Ptr warper = 68 | cv::makePtr(); 69 | cv::Mat H; 70 | transform.convertTo(H, CV_32F); 71 | 72 | // separate rotation and translation for plane warper 73 | // 3D translation 74 | cv::Mat T = cv::Mat::zeros(3, 1, CV_32F); 75 | H.colRange(2, 3).rowRange(0, 2).copyTo(T.rowRange(0, 2)); 76 | // 3D rotation 77 | cv::Mat R = cv::Mat::eye(3, 3, CV_32F); 78 | H.colRange(0, 2).copyTo(R.rowRange(0, 2).colRange(0, 2)); 79 | 80 | return warper->warpRoi(grid.size(), cv::Mat::eye(3, 3, CV_32F), R, T); 81 | } 82 | 83 | } // namespace internal 84 | 85 | } // namespace combine_grids 86 | -------------------------------------------------------------------------------- /map_merge/src/map_register.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | namespace map_register 6 | { 7 | 8 | class MapRegister 9 | { 10 | private: 11 | ros::NodeHandle node_; 12 | 13 | /* parameters */ 14 | // double confidence_threshold_; 15 | 16 | ros::ServiceServer service_; 17 | combine_grids::MergingPipeline pipeline_; 18 | 19 | public: 20 | MapRegister(); 21 | bool handle_function(multirobot_map_merge::mapPair2tf::Request &req, multirobot_map_merge::mapPair2tf::Response &res); 22 | 23 | }; 24 | 25 | MapRegister::MapRegister() 26 | { 27 | ros::NodeHandle private_nh("~"); 28 | // private_nh.param("estimation_confidence", confidence_threshold_, 0.5); 29 | 30 | service_ = node_.advertiseService("GetMapTransform", &MapRegister::handle_function, this); 31 | } 32 | 33 | bool MapRegister::handle_function(multirobot_map_merge::mapPair2tf::Request &req, multirobot_map_merge::mapPair2tf::Response &res) 34 | { 35 | cv::Mat transform; 36 | double confidence; 37 | if(!pipeline_.estimateTransform(req.grid1, req.grid2, transform, combine_grids::FeatureType::AKAZE, confidence)){ 38 | res.confidence = 0; 39 | return true; 40 | } 41 | 42 | ROS_ASSERT(transform.type() == CV_64F); 43 | ROS_ASSERT((req.grid1.info.resolution-req.grid2.info.resolution)/req.grid1.info.resolution < 0.01); 44 | transform.at(0, 2) = transform.at(0, 2) * req.grid1.info.resolution; 45 | transform.at(1, 2) = transform.at(1, 2) * req.grid1.info.resolution; 46 | double a = transform.at(0, 0); 47 | double b = transform.at(1, 0); 48 | double scale = std::sqrt(a*a+b*b); 49 | confidence = confidence * std::min(scale, 1/scale); 50 | // transform = transform/scale; 51 | transform.at(0, 0) = transform.at(0, 0)/scale; 52 | transform.at(1, 0) = transform.at(1, 0)/scale; 53 | transform.at(0, 1) = transform.at(0, 1)/scale; 54 | transform.at(1, 1) = transform.at(1, 1)/scale; 55 | std::cout << "transform: " << transform << std::endl; 56 | 57 | cv::Mat map1_to_origin1 = cv::Mat::eye(3,3,CV_64F); 58 | map1_to_origin1.at(0, 2) = req.grid1.info.origin.position.x; 59 | map1_to_origin1.at(1, 2) = req.grid1.info.origin.position.y; 60 | cv::Mat map2_to_origin2 = cv::Mat::eye(3,3,CV_64F); 61 | map2_to_origin2.at(0, 2) = req.grid2.info.origin.position.x; 62 | map2_to_origin2.at(1, 2) = req.grid2.info.origin.position.y; 63 | 64 | cv::Mat origin1_to_map1; 65 | cv::invert(map1_to_origin1, origin1_to_map1); 66 | transform = map2_to_origin2 * transform * origin1_to_map1; 67 | std::cout << "map2_to_origin2: " << map2_to_origin2 << std::endl; 68 | std::cout << "origin1_to_map1: " << origin1_to_map1 << std::endl; 69 | std::cout << "transform_new: " << transform << std::endl; 70 | 71 | res.transform.translation.x = transform.at(0, 2); 72 | res.transform.translation.y = transform.at(1, 2); 73 | res.transform.translation.z = 0.; 74 | 75 | // our rotation is in fact only 2D, thus quaternion can be simplified 76 | a = transform.at(0, 0); 77 | b = transform.at(1, 0); 78 | res.transform.rotation.w = std::sqrt(2. + 2. * a) * 0.5; 79 | res.transform.rotation.x = 0.; 80 | res.transform.rotation.y = 0.; 81 | res.transform.rotation.z = std::copysign(std::sqrt(2. - 2. * a) * 0.5, b); 82 | res.confidence = confidence; 83 | std::cout << "confidence: " << confidence << std::endl; 84 | 85 | return true; 86 | } 87 | 88 | } 89 | 90 | int main(int argc, char** argv) 91 | { 92 | ros::init(argc, argv, "map_register"); 93 | // this package is still in development -- start wil debugging enabled 94 | if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, 95 | ros::console::levels::Debug)) { 96 | ros::console::notifyLoggerLevelsChanged(); 97 | } 98 | map_register::MapRegister map_registration; 99 | ros::spin(); 100 | return 0; 101 | } -------------------------------------------------------------------------------- /map_merge/srv/mapPair2tf.srv: -------------------------------------------------------------------------------- 1 | nav_msgs/OccupancyGrid grid1 2 | nav_msgs/OccupancyGrid grid2 3 | --- 4 | geometry_msgs/Transform transform 5 | float64 confidence -------------------------------------------------------------------------------- /onpolicy/.gitignore: -------------------------------------------------------------------------------- 1 | __pycache__ 2 | results 3 | *.pyc 4 | /gifs/ 5 | build 6 | win_rate 7 | *.so 8 | *.csv 9 | RODE 10 | *.egg-info 11 | wandb 12 | .vscode 13 | api 14 | logs 15 | .nfs* 16 | *.so 17 | *.out 18 | png 19 | 20 | gibson/assets/dataset 21 | docker 22 | dataset 23 | data 24 | pretrained_models 25 | 26 | -------------------------------------------------------------------------------- /onpolicy/.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "onpolicy/envs/iGibson"] 2 | path = onpolicy/envs/iGibson 3 | url = https://github.com/zoeyuchao/iGibson.git 4 | [submodule "onpolicy/envs/habitat/habitat-sim"] 5 | path = onpolicy/envs/habitat/habitat-sim 6 | url = https://github.com/zoeyuchao/habitat-sim.git 7 | [submodule "onpolicy/envs/habitat/habitat-lab"] 8 | path = onpolicy/envs/habitat/habitat-lab 9 | url = https://github.com/zoeyuchao/habitat-lab.git 10 | -------------------------------------------------------------------------------- /onpolicy/.idea/.gitignore: -------------------------------------------------------------------------------- 1 | 2 | # Default ignored files 3 | /workspace.xml -------------------------------------------------------------------------------- /onpolicy/.idea/inspectionProfiles/profiles_settings.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | -------------------------------------------------------------------------------- /onpolicy/.idea/misc.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | -------------------------------------------------------------------------------- /onpolicy/.idea/modules.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /onpolicy/.idea/onpolicy.iml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 14 | 15 | 16 | 19 | -------------------------------------------------------------------------------- /onpolicy/.idea/vcs.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /onpolicy/onpolicy/__init__.py: -------------------------------------------------------------------------------- 1 | from onpolicy import algorithms, envs, runner, scripts, utils, config 2 | 3 | 4 | __version__ = "0.1.0" 5 | 6 | __all__ = [ 7 | "algorithms", 8 | "envs", 9 | "runner", 10 | "scripts", 11 | "utils", 12 | "config", 13 | ] -------------------------------------------------------------------------------- /onpolicy/onpolicy/algorithms/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/onpolicy/onpolicy/algorithms/__init__.py -------------------------------------------------------------------------------- /onpolicy/onpolicy/algorithms/r_mappg/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/onpolicy/onpolicy/algorithms/r_mappg/__init__.py -------------------------------------------------------------------------------- /onpolicy/onpolicy/algorithms/r_mappg/algorithm/rMAPPGPolicy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import torch 3 | from onpolicy.algorithms.r_mappg.algorithm.r_actor_critic import R_Actor, R_Critic 4 | from onpolicy.utils.util import update_linear_schedule 5 | 6 | 7 | class R_MAPPGPolicy: 8 | def __init__(self, args, obs_space, share_obs_space, act_space, device=torch.device("cpu")): 9 | 10 | self.device = device 11 | self.lr = args.lr 12 | self.critic_lr = args.critic_lr 13 | self.opti_eps = args.opti_eps 14 | self.weight_decay = args.weight_decay 15 | 16 | self.obs_space = obs_space 17 | self.share_obs_space = share_obs_space 18 | self.act_space = act_space 19 | 20 | self.actor = R_Actor(args, self.obs_space, self.act_space, self.device) 21 | self.critic = R_Critic(args, self.share_obs_space, self.device) 22 | 23 | self.actor_optimizer = torch.optim.Adam(self.actor.parameters(), lr=self.lr, eps=self.opti_eps, weight_decay=self.weight_decay) 24 | self.critic_optimizer = torch.optim.Adam(self.critic.parameters(), lr=self.critic_lr, eps=self.opti_eps, weight_decay=self.weight_decay) 25 | 26 | def lr_decay(self, episode, episodes): 27 | update_linear_schedule(self.actor_optimizer, episode, episodes, self.lr) 28 | update_linear_schedule(self.critic_optimizer, episode, episodes, self.lr) 29 | 30 | def get_actions(self, share_obs, obs, rnn_states_actor, rnn_states_critic, masks, available_actions=None, deterministic=False): 31 | actions, action_log_probs, rnn_states_actor = self.actor(obs, rnn_states_actor, masks, available_actions, deterministic) 32 | values, rnn_states_critic = self.critic(share_obs, rnn_states_critic, masks) 33 | return values, actions, action_log_probs, rnn_states_actor, rnn_states_critic 34 | 35 | def get_policy_values_and_probs(self, obs, rnn_states_actor, masks, available_actions=None): 36 | values, action_probs = self.actor.get_values_and_probs(obs, rnn_states_actor, masks, available_actions) 37 | return values, action_probs 38 | 39 | def get_probs(self, obs, rnn_states_actor, masks, available_actions=None): 40 | action_probs = self.actor.get_probs(obs, rnn_states_actor, masks, available_actions) 41 | return action_probs 42 | 43 | def get_values(self, share_obs, rnn_states_critic, masks): 44 | values, _ = self.critic(share_obs, rnn_states_critic, masks) 45 | return values 46 | 47 | def evaluate_actions(self, obs, rnn_states_actor, action, masks, available_actions=None, active_masks=None): 48 | action_log_probs, dist_entropy, _ = self.actor.evaluate_actions(obs, rnn_states_actor, action, masks, available_actions, active_masks) 49 | return action_log_probs, dist_entropy 50 | 51 | def act(self, obs, rnn_states_actor, masks, available_actions=None, deterministic=False): 52 | actions, _, rnn_states_actor = self.actor(obs, rnn_states_actor, masks, available_actions, deterministic) 53 | return actions, rnn_states_actor 54 | 55 | -------------------------------------------------------------------------------- /onpolicy/onpolicy/algorithms/r_mappg_single/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/onpolicy/onpolicy/algorithms/r_mappg_single/__init__.py -------------------------------------------------------------------------------- /onpolicy/onpolicy/algorithms/r_mappg_single/algorithm/rMAPPGPolicy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import torch 3 | from onpolicy.algorithms.r_mappg_single.algorithm.r_actor_critic import R_Model 4 | from onpolicy.utils.util import update_linear_schedule 5 | 6 | 7 | class R_MAPPGPolicy: 8 | def __init__(self, args, obs_space, share_obs_space, action_space, device=torch.device("cpu")): 9 | 10 | self.device = device 11 | self.lr = args.lr 12 | self.critic_lr = args.critic_lr 13 | self.opti_eps = args.opti_eps 14 | self.weight_decay = args.weight_decay 15 | 16 | self.obs_space = obs_space 17 | self.share_obs_space = share_obs_space 18 | self.act_space = action_space 19 | 20 | self.model = R_Model(args, self.obs_space, self.share_obs_space, self.act_space, self.device) 21 | 22 | self.optimizer = torch.optim.Adam(self.model.parameters(), lr=self.lr, eps=self.opti_eps, weight_decay=self.weight_decay) 23 | 24 | def lr_decay(self, episode, episodes): 25 | update_linear_schedule(self.optimizer, episode, episodes, self.lr) 26 | 27 | def get_actions(self, share_obs, obs, rnn_states_actor, rnn_states_critic, masks, available_actions=None, deterministic=False): 28 | actions, action_log_probs, rnn_states_actor = self.model.get_actions(obs, rnn_states_actor, masks, available_actions, deterministic) 29 | values, rnn_states_critic = self.model.get_values(share_obs, rnn_states_critic, masks) 30 | return values, actions, action_log_probs, rnn_states_actor, rnn_states_critic 31 | 32 | def get_logprobs(self, obs, rnn_states_actor, masks, available_actions=None, deterministic=False): 33 | actions, action_log_probs, rnn_states_actor = self.model.get_actions(obs, rnn_states_actor, masks, available_actions, deterministic) 34 | return action_log_probs 35 | 36 | def get_values_and_probs(self, share_obs, obs, rnn_states_actor, rnn_states_critic, masks, available_actions=None): 37 | action_probs = self.model.get_probs(obs, rnn_states_actor, masks, available_actions) 38 | values, _ = self.model.get_values(share_obs, rnn_states_critic, masks) 39 | return values, action_probs 40 | 41 | def get_values(self, share_obs, rnn_states_critic, masks): 42 | values, _ = self.model.get_values(share_obs, rnn_states_critic, masks) 43 | return values 44 | 45 | def evaluate_actions(self, obs, rnn_states_actor, action, masks, available_actions=None, active_masks=None): 46 | action_log_probs, dist_entropy = self.model.evaluate_actions(obs, rnn_states_actor, action, masks, available_actions, active_masks) 47 | 48 | return action_log_probs, dist_entropy 49 | 50 | def act(self, obs, rnn_states_actor, masks, available_actions=None, deterministic=False): 51 | actions, _, rnn_states_actor = self.model.get_actions(obs, rnn_states_actor, masks, available_actions, deterministic) 52 | return actions, rnn_states_actor 53 | -------------------------------------------------------------------------------- /onpolicy/onpolicy/algorithms/r_mappo/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/onpolicy/onpolicy/algorithms/r_mappo/__init__.py -------------------------------------------------------------------------------- /onpolicy/onpolicy/algorithms/r_mappo/algorithm/rMAPPOPolicy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import torch 3 | from onpolicy.algorithms.r_mappo.algorithm.r_actor_critic import R_Actor, R_Critic 4 | from onpolicy.utils.util import update_linear_schedule 5 | 6 | 7 | class R_MAPPOPolicy: 8 | def __init__(self, args, obs_space, share_obs_space, act_space, device=torch.device("cpu")): 9 | 10 | self.device = device 11 | self.lr = args.lr 12 | self.critic_lr = args.critic_lr 13 | self.opti_eps = args.opti_eps 14 | self.weight_decay = args.weight_decay 15 | 16 | self.obs_space = obs_space 17 | self.share_obs_space = share_obs_space 18 | self.act_space = act_space 19 | 20 | self.actor = R_Actor(args, self.obs_space, self.act_space, self.device) 21 | self.critic = R_Critic(args, self.share_obs_space, self.device) 22 | 23 | self.actor_optimizer = torch.optim.Adam(self.actor.parameters(), lr=self.lr, eps=self.opti_eps, weight_decay=self.weight_decay) 24 | self.critic_optimizer = torch.optim.Adam(self.critic.parameters(), lr=self.critic_lr, eps=self.opti_eps, weight_decay=self.weight_decay) 25 | 26 | def lr_decay(self, episode, episodes): 27 | update_linear_schedule(self.actor_optimizer, episode, episodes, self.lr) 28 | update_linear_schedule(self.critic_optimizer, episode, episodes, self.critic_lr) 29 | 30 | def get_actions(self, share_obs, obs, rnn_states_actor, rnn_states_critic, masks, available_actions=None, deterministic=False): 31 | actions, action_log_probs, rnn_states_actor = self.actor(obs, rnn_states_actor, masks, available_actions, deterministic) 32 | values, rnn_states_critic = self.critic(share_obs, rnn_states_critic, masks) 33 | return values, actions, action_log_probs, rnn_states_actor, rnn_states_critic 34 | 35 | def get_values(self, share_obs, rnn_states_critic, masks): 36 | values, _ = self.critic(share_obs, rnn_states_critic, masks) 37 | return values 38 | 39 | def evaluate_actions(self, share_obs, obs, rnn_states_actor, rnn_states_critic, action, masks, available_actions=None, active_masks=None): 40 | action_log_probs, dist_entropy, policy_values = self.actor.evaluate_actions(obs, rnn_states_actor, action, masks, available_actions, active_masks) 41 | values, _ = self.critic(share_obs, rnn_states_critic, masks) 42 | return values, action_log_probs, dist_entropy, policy_values 43 | 44 | def act(self, obs, rnn_states_actor, masks, available_actions=None, deterministic=False): 45 | actions, _, rnn_states_actor = self.actor(obs, rnn_states_actor, masks, available_actions, deterministic) 46 | return actions, rnn_states_actor 47 | -------------------------------------------------------------------------------- /onpolicy/onpolicy/algorithms/r_mappo_single/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/onpolicy/onpolicy/algorithms/r_mappo_single/__init__.py -------------------------------------------------------------------------------- /onpolicy/onpolicy/algorithms/r_mappo_single/algorithm/rMAPPOPolicy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import torch 3 | from onpolicy.algorithms.r_mappo_single.algorithm.r_actor_critic import R_Model 4 | from onpolicy.utils.util import update_linear_schedule 5 | 6 | 7 | class R_MAPPOPolicy: 8 | def __init__(self, args, obs_space, share_obs_space, action_space, device=torch.device("cpu")): 9 | 10 | self.device = device 11 | self.lr = args.lr 12 | self.critic_lr = args.critic_lr 13 | self.opti_eps = args.opti_eps 14 | self.weight_decay = args.weight_decay 15 | 16 | self.obs_space = obs_space 17 | self.share_obs_space = share_obs_space 18 | self.act_space = action_space 19 | 20 | self.model = R_Model(args, self.obs_space, self.share_obs_space, self.act_space, self.device) 21 | 22 | self.optimizer = torch.optim.Adam(self.model.parameters(), lr=self.lr, eps=self.opti_eps, weight_decay=self.weight_decay) 23 | 24 | def lr_decay(self, episode, episodes): 25 | update_linear_schedule(self.optimizer, episode, episodes, self.lr) 26 | 27 | def get_actions(self, share_obs, obs, rnn_states_actor, rnn_states_critic, masks, available_actions=None, deterministic=False): 28 | actions, action_log_probs, rnn_states_actor = self.model.get_actions( 29 | obs, rnn_states_actor, masks, available_actions, deterministic) 30 | values, rnn_states_critic = self.model.get_values( 31 | share_obs, rnn_states_critic, masks) 32 | return values, actions, action_log_probs, rnn_states_actor, rnn_states_critic 33 | 34 | def get_values(self, share_obs, rnn_states_critic, masks): 35 | values, _ = self.model.get_values(share_obs, rnn_states_critic, masks) 36 | return values 37 | 38 | def evaluate_actions(self, obs, rnn_states_actor, action, masks, available_actions=None, active_masks=None): 39 | action_log_probs, dist_entropy = self.model.evaluate_actions( 40 | obs, rnn_states_actor, action, masks, available_actions, active_masks) 41 | 42 | return action_log_probs, dist_entropy 43 | 44 | def act(self, obs, rnn_states_actor, masks, available_actions=None, deterministic=False): 45 | actions, _, rnn_states_actor = self.model.get_actions(obs, rnn_states_actor, masks, available_actions, deterministic) 46 | return actions, rnn_states_actor 47 | -------------------------------------------------------------------------------- /onpolicy/onpolicy/algorithms/utils/cnn.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | import torch 4 | import torch.nn as nn 5 | import torch.nn.functional as F 6 | 7 | from .util import init 8 | 9 | class Flatten(nn.Module): 10 | def forward(self, x): 11 | return x.view(x.size(0), -1) 12 | 13 | class CNNLayer(nn.Module): 14 | def __init__(self, obs_shape, hidden_size, use_orthogonal, activation_id, kernel_size=3, stride=1): 15 | super(CNNLayer, self).__init__() 16 | 17 | active_func = [nn.Tanh(), nn.ReLU(), nn.LeakyReLU(), nn.ELU()][activation_id] 18 | init_method = [nn.init.xavier_uniform_, nn.init.orthogonal_][use_orthogonal] 19 | gain = nn.init.calculate_gain(['tanh', 'relu', 'leaky_relu', 'leaky_relu'][activation_id]) 20 | 21 | def init_(m): 22 | return init(m, init_method, lambda x: nn.init.constant_(x, 0), gain=gain) 23 | 24 | input_channel = obs_shape[0] 25 | input_width = obs_shape[1] 26 | input_height = obs_shape[2] 27 | 28 | self.cnn = nn.Sequential( 29 | init_(nn.Conv2d(in_channels=input_channel, out_channels=hidden_size//2, kernel_size=kernel_size, stride=stride)), active_func, 30 | Flatten(), 31 | init_(nn.Linear(hidden_size//2 * (input_width-kernel_size+stride) * (input_height-kernel_size+stride), hidden_size)), active_func, 32 | init_(nn.Linear(hidden_size, hidden_size)), active_func) 33 | 34 | def forward(self, x): 35 | x = x / 255.0 36 | x = self.cnn(x) 37 | 38 | return x 39 | 40 | class CNNBase(nn.Module): 41 | def __init__(self, args, obs_shape): 42 | super(CNNBase, self).__init__() 43 | 44 | self._use_orthogonal = args.use_orthogonal 45 | self._activation_id = args.activation_id 46 | self.hidden_size = args.hidden_size 47 | 48 | self.cnn = CNNLayer(obs_shape, self.hidden_size, self._use_orthogonal, self._activation_id) 49 | 50 | def forward(self, x): 51 | x = self.cnn(x) 52 | return x 53 | 54 | @property 55 | def output_size(self): 56 | return self.hidden_size 57 | -------------------------------------------------------------------------------- /onpolicy/onpolicy/algorithms/utils/distributions.py: -------------------------------------------------------------------------------- 1 | import math 2 | 3 | import torch 4 | import torch.nn as nn 5 | import torch.nn.functional as F 6 | 7 | from .util import init 8 | 9 | """ 10 | Modify standard PyTorch distributions so they are compatible with this code. 11 | """ 12 | 13 | # 14 | # Standardize distribution interfaces 15 | # 16 | 17 | # Categorical 18 | class FixedCategorical(torch.distributions.Categorical): 19 | def sample(self): 20 | return super().sample().unsqueeze(-1) 21 | 22 | def log_probs(self, actions): 23 | return ( 24 | super() 25 | .log_prob(actions.squeeze(-1)) 26 | .view(actions.size(0), -1) 27 | .sum(-1) 28 | .unsqueeze(-1) 29 | ) 30 | 31 | def mode(self): 32 | return self.probs.argmax(dim=-1, keepdim=True) 33 | 34 | 35 | # Normal 36 | class FixedNormal(torch.distributions.Normal): 37 | def log_probs(self, actions): 38 | return super().log_prob(actions).sum(-1, keepdim=True) 39 | 40 | def entrop(self): 41 | return super.entropy().sum(-1) 42 | 43 | def mode(self): 44 | return self.mean 45 | 46 | 47 | # Bernoulli 48 | class FixedBernoulli(torch.distributions.Bernoulli): 49 | def log_probs(self, actions): 50 | return super.log_prob(actions).view(actions.size(0), -1).sum(-1).unsqueeze(-1) 51 | 52 | def entropy(self): 53 | return super().entropy().sum(-1) 54 | 55 | def mode(self): 56 | return torch.gt(self.probs, 0.5).float() 57 | 58 | 59 | class Categorical(nn.Module): 60 | def __init__(self, num_inputs, num_outputs, use_orthogonal=True, gain=0.01): 61 | super(Categorical, self).__init__() 62 | init_method = [nn.init.xavier_uniform_, nn.init.orthogonal_][use_orthogonal] 63 | def init_(m): 64 | return init(m, init_method, lambda x: nn.init.constant_(x, 0), gain) 65 | 66 | self.linear = init_(nn.Linear(num_inputs, num_outputs)) 67 | 68 | def forward(self, x, available_actions=None): 69 | x = self.linear(x) 70 | if available_actions is not None: 71 | x[available_actions == 0] = -1e10 72 | return FixedCategorical(logits=x) 73 | 74 | 75 | class DiagGaussian(nn.Module): 76 | def __init__(self, num_inputs, num_outputs, use_orthogonal=True, gain=0.01): 77 | super(DiagGaussian, self).__init__() 78 | 79 | init_method = [nn.init.xavier_uniform_, nn.init.orthogonal_][use_orthogonal] 80 | def init_(m): 81 | return init(m, init_method, lambda x: nn.init.constant_(x, 0), gain) 82 | 83 | self.fc_mean = init_(nn.Linear(num_inputs, num_outputs)) 84 | self.logstd = AddBias(torch.zeros(num_outputs)) 85 | 86 | def forward(self, x): 87 | action_mean = self.fc_mean(x) 88 | 89 | # An ugly hack for my KFAC implementation. 90 | zeros = torch.zeros(action_mean.size()) 91 | if x.is_cuda: 92 | zeros = zeros.cuda() 93 | 94 | action_logstd = self.logstd(zeros) 95 | return FixedNormal(action_mean, action_logstd.exp()) 96 | 97 | 98 | class Bernoulli(nn.Module): 99 | def __init__(self, num_inputs, num_outputs, use_orthogonal=True, gain=0.01): 100 | super(Bernoulli, self).__init__() 101 | init_method = [nn.init.xavier_uniform_, nn.init.orthogonal_][use_orthogonal] 102 | def init_(m): 103 | return init(m, init_method, lambda x: nn.init.constant_(x, 0), gain) 104 | 105 | self.linear = init_(nn.Linear(num_inputs, num_outputs)) 106 | 107 | def forward(self, x): 108 | x = self.linear(x) 109 | return FixedBernoulli(logits=x) 110 | 111 | class AddBias(nn.Module): 112 | def __init__(self, bias): 113 | super(AddBias, self).__init__() 114 | self._bias = nn.Parameter(bias.unsqueeze(1)) 115 | 116 | def forward(self, x): 117 | if x.dim() == 2: 118 | bias = self._bias.t().view(1, -1) 119 | else: 120 | bias = self._bias.t().view(1, -1, 1, 1) 121 | 122 | return x + bias 123 | -------------------------------------------------------------------------------- /onpolicy/onpolicy/algorithms/utils/popart.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | import torch 4 | import torch.nn as nn 5 | import torch.nn.functional as F 6 | 7 | class PopArt(torch.nn.Module): 8 | 9 | def __init__(self, input_shape, output_shape, norm_axes=1, beta=0.99999, epsilon=1e-5, device=torch.device("cpu")): 10 | 11 | super(PopArt, self).__init__() 12 | 13 | self.beta = beta 14 | self.epsilon = epsilon 15 | self.norm_axes = norm_axes 16 | self.tpdv = dict(dtype=torch.float32, device=device) 17 | 18 | self.input_shape = input_shape 19 | self.output_shape = output_shape 20 | 21 | self.weight = nn.Parameter(torch.Tensor(output_shape, input_shape)).to(**self.tpdv) 22 | self.bias = nn.Parameter(torch.Tensor(output_shape)).to(**self.tpdv) 23 | 24 | self.stddev = nn.Parameter(torch.ones(output_shape), requires_grad=False).to(**self.tpdv) 25 | self.mean = nn.Parameter(torch.zeros(output_shape), requires_grad=False).to(**self.tpdv) 26 | self.mean_sq = nn.Parameter(torch.zeros(output_shape), requires_grad=False).to(**self.tpdv) 27 | self.debiasing_term = nn.Parameter(torch.tensor(0.0), requires_grad=False).to(**self.tpdv) 28 | 29 | self.reset_parameters() 30 | 31 | def reset_parameters(self): 32 | torch.nn.init.kaiming_uniform_(self.weight, a=math.sqrt(5)) 33 | if self.bias is not None: 34 | fan_in, _ = torch.nn.init._calculate_fan_in_and_fan_out(self.weight) 35 | bound = 1 / math.sqrt(fan_in) 36 | torch.nn.init.uniform_(self.bias, -bound, bound) 37 | self.mean.zero_() 38 | self.mean_sq.zero_() 39 | self.debiasing_term.zero_() 40 | 41 | def forward(self, input_vector): 42 | if type(input_vector) == np.ndarray: 43 | input_vector = torch.from_numpy(input_vector) 44 | input_vector = input_vector.to(**self.tpdv) 45 | 46 | return F.linear(input_vector, self.weight, self.bias) 47 | 48 | @torch.no_grad() 49 | def update(self, input_vector): 50 | if type(input_vector) == np.ndarray: 51 | input_vector = torch.from_numpy(input_vector) 52 | input_vector = input_vector.to(**self.tpdv) 53 | 54 | old_mean, old_stddev = self.mean, self.stddev 55 | 56 | batch_mean = input_vector.mean(dim=tuple(range(self.norm_axes))) 57 | batch_sq_mean = (input_vector ** 2).mean(dim=tuple(range(self.norm_axes))) 58 | 59 | self.mean.mul_(self.beta).add_(batch_mean * (1.0 - self.beta)) 60 | self.mean_sq.mul_(self.beta).add_(batch_sq_mean * (1.0 - self.beta)) 61 | self.debiasing_term.mul_(self.beta).add_(1.0 * (1.0 - self.beta)) 62 | 63 | self.stddev = (self.mean_sq - self.mean ** 2).sqrt().clamp(min=1e-4) 64 | 65 | self.weight = self.weight * old_stddev / self.stddev 66 | self.bias = (old_stddev * self.bias + old_mean - self.mean) / self.stddev 67 | 68 | def debiased_mean_var(self): 69 | debiased_mean = self.mean / self.debiasing_term.clamp(min=self.epsilon) 70 | debiased_mean_sq = self.mean_sq / self.debiasing_term.clamp(min=self.epsilon) 71 | debiased_var = (debiased_mean_sq - debiased_mean ** 2).clamp(min=1e-2) 72 | return debiased_mean, debiased_var 73 | 74 | def normalize(self, input_vector): 75 | if type(input_vector) == np.ndarray: 76 | input_vector = torch.from_numpy(input_vector) 77 | input_vector = input_vector.to(**self.tpdv) 78 | 79 | mean, var = self.debiased_mean_var() 80 | out = (input_vector - mean[(None,) * self.norm_axes]) / torch.sqrt(var)[(None,) * self.norm_axes] 81 | 82 | return out 83 | 84 | def denormalize(self, input_vector): 85 | if type(input_vector) == np.ndarray: 86 | input_vector = torch.from_numpy(input_vector) 87 | input_vector = input_vector.to(**self.tpdv) 88 | 89 | mean, var = self.debiased_mean_var() 90 | out = input_vector * torch.sqrt(var)[(None,) * self.norm_axes] + mean[(None,) * self.norm_axes] 91 | 92 | out = out.cpu().numpy() 93 | 94 | return out 95 | -------------------------------------------------------------------------------- /onpolicy/onpolicy/algorithms/utils/rnn.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | 4 | import torch 5 | import torch.nn as nn 6 | import torch.nn.functional as F 7 | 8 | class RNNLayer(nn.Module): 9 | def __init__(self, inputs_dim, outputs_dim, recurrent_N, use_orthogonal): 10 | super(RNNLayer, self).__init__() 11 | self._recurrent_N = recurrent_N 12 | self._use_orthogonal = use_orthogonal 13 | 14 | self.rnn = nn.GRU(inputs_dim, outputs_dim, num_layers=self._recurrent_N) 15 | for name, param in self.rnn.named_parameters(): 16 | if 'bias' in name: 17 | nn.init.constant_(param, 0) 18 | elif 'weight' in name: 19 | if self._use_orthogonal: 20 | nn.init.orthogonal_(param) 21 | else: 22 | nn.init.xavier_uniform_(param) 23 | self.norm = nn.LayerNorm(outputs_dim) 24 | 25 | def forward(self, x, hxs, masks): 26 | if x.size(0) == hxs.size(0): 27 | x, hxs = self.rnn(x.unsqueeze(0), (hxs * masks.repeat(1, self._recurrent_N).unsqueeze(-1)).transpose(0, 1).contiguous()) 28 | #x= self.gru(x.unsqueeze(0)) 29 | x = x.squeeze(0) 30 | hxs = hxs.transpose(0, 1) 31 | else: 32 | # x is a (T, N, -1) tensor that has been flatten to (T * N, -1) 33 | N = hxs.size(0) 34 | T = int(x.size(0) / N) 35 | 36 | # unflatten 37 | x = x.view(T, N, x.size(1)) 38 | 39 | # Same deal with masks 40 | masks = masks.view(T, N) 41 | 42 | # Let's figure out which steps in the sequence have a zero for any agent 43 | # We will always assume t=0 has a zero in it as that makes the logic cleaner 44 | has_zeros = ((masks[1:] == 0.0) 45 | .any(dim=-1) 46 | .nonzero() 47 | .squeeze() 48 | .cpu()) 49 | 50 | # +1 to correct the masks[1:] 51 | if has_zeros.dim() == 0: 52 | # Deal with scalar 53 | has_zeros = [has_zeros.item() + 1] 54 | else: 55 | has_zeros = (has_zeros + 1).numpy().tolist() 56 | 57 | # add t=0 and t=T to the list 58 | has_zeros = [0] + has_zeros + [T] 59 | 60 | hxs = hxs.transpose(0, 1) 61 | 62 | outputs = [] 63 | for i in range(len(has_zeros) - 1): 64 | # We can now process steps that don't have any zeros in masks together! 65 | # This is much faster 66 | start_idx = has_zeros[i] 67 | end_idx = has_zeros[i + 1] 68 | temp = (hxs * masks[start_idx].view(1, -1, 1).repeat(self._recurrent_N, 1, 1)).contiguous() 69 | rnn_scores, hxs = self.rnn(x[start_idx:end_idx], temp) 70 | outputs.append(rnn_scores) 71 | 72 | # assert len(outputs) == T 73 | # x is a (T, N, -1) tensor 74 | x = torch.cat(outputs, dim=0) 75 | 76 | # flatten 77 | x = x.reshape(T * N, -1) 78 | hxs = hxs.transpose(0, 1) 79 | 80 | x = self.norm(x) 81 | return x, hxs 82 | -------------------------------------------------------------------------------- /onpolicy/onpolicy/algorithms/utils/util.py: -------------------------------------------------------------------------------- 1 | import glob 2 | import os 3 | import copy 4 | import numpy as np 5 | 6 | import torch 7 | import torch.nn as nn 8 | 9 | def init(module, weight_init, bias_init, gain=1): 10 | weight_init(module.weight.data, gain=gain) 11 | bias_init(module.bias.data) 12 | return module 13 | 14 | def get_clones(module, N): 15 | return nn.ModuleList([copy.deepcopy(module) for i in range(N)]) 16 | 17 | def check(input): 18 | output = torch.from_numpy(input) if type(input) == np.ndarray else input 19 | return output 20 | -------------------------------------------------------------------------------- /onpolicy/onpolicy/docs/Makefile: -------------------------------------------------------------------------------- 1 | # Minimal makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line. 5 | SPHINXOPTS = 6 | SPHINXBUILD = sphinx-build 7 | SPHINXPROJ = onpolicy 8 | SOURCEDIR = source 9 | BUILDDIR = build 10 | 11 | # Put it first so that "make" without argument is like "make help". 12 | help: 13 | @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 14 | 15 | .PHONY: help Makefile 16 | 17 | # Catch-all target: route all unknown targets to Sphinx using the new 18 | # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). 19 | %: Makefile 20 | @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) -------------------------------------------------------------------------------- /onpolicy/onpolicy/docs/make.bat: -------------------------------------------------------------------------------- 1 | @ECHO OFF 2 | 3 | pushd %~dp0 4 | 5 | REM Command file for Sphinx documentation 6 | 7 | if "%SPHINXBUILD%" == "" ( 8 | set SPHINXBUILD=sphinx-build 9 | ) 10 | set SOURCEDIR=source 11 | set BUILDDIR=build 12 | set SPHINXPROJ=onpolicy 13 | 14 | if "%1" == "" goto help 15 | 16 | %SPHINXBUILD% >NUL 2>NUL 17 | if errorlevel 9009 ( 18 | echo. 19 | echo.The 'sphinx-build' command was not found. Make sure you have Sphinx 20 | echo.installed, then set the SPHINXBUILD environment variable to point 21 | echo.to the full path of the 'sphinx-build' executable. Alternatively you 22 | echo.may add the Sphinx directory to PATH. 23 | echo. 24 | echo.If you don't have Sphinx installed, grab it from 25 | echo.http://sphinx-doc.org/ 26 | exit /b 1 27 | ) 28 | 29 | %SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% 30 | goto end 31 | 32 | :help 33 | %SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% 34 | 35 | :end 36 | popd 37 | -------------------------------------------------------------------------------- /onpolicy/onpolicy/docs/source/_templates/module.rst_t: -------------------------------------------------------------------------------- 1 | {%- if show_headings %} 2 | {{- [basename, "module"] | join(' ') | e | heading }} 3 | 4 | {% endif -%} 5 | .. automodule:: {{ qualname }} 6 | :members: 7 | :undoc-members: -------------------------------------------------------------------------------- /onpolicy/onpolicy/docs/source/_templates/package.rst_t: -------------------------------------------------------------------------------- 1 | {%- macro automodule(modname, options) -%} 2 | .. automodule:: {{ modname }} 3 | {%- for option in options %} 4 | :{{ option }}: 5 | {%- endfor %} 6 | {%- endmacro %} 7 | 8 | {%- macro toctree(docnames) -%} 9 | .. toctree:: 10 | :maxdepth: {{ maxdepth }} 11 | {% for docname in docnames %} 12 | {{ docname }} 13 | {%- endfor %} 14 | {%- endmacro %} 15 | 16 | {%- if is_namespace %} 17 | {{- [pkgname, "namespace"] | join(" ") | e | heading }} 18 | {% else %} 19 | {{- [pkgname, "package"] | join(" ") | e | heading }} 20 | {% endif %} 21 | 22 | {%- if modulefirst and not is_namespace %} 23 | {{ automodule(pkgname, automodule_options) }} 24 | {% endif %} 25 | 26 | {%- if subpackages %} 27 | {{ toctree(subpackages) }} 28 | {% endif %} 29 | 30 | {%- if submodules %} 31 | {% if separatemodules %} 32 | {{ toctree(submodules) }} 33 | {%- else %} 34 | {%- for submodule in submodules %} 35 | {% if show_headings %} 36 | {{- [submodule, "module"] | join(" ") | e | heading(2) }} 37 | {% endif %} 38 | {{ automodule(submodule, automodule_options) }} 39 | {% endfor %} 40 | {%- endif %} 41 | {% endif %} 42 | 43 | {%- if not modulefirst and not is_namespace %} 44 | Module contents 45 | --------------- 46 | 47 | {{ automodule(pkgname, automodule_options) }} 48 | {% endif %} -------------------------------------------------------------------------------- /onpolicy/onpolicy/docs/source/_templates/toc.rst_t: -------------------------------------------------------------------------------- 1 | {{ header | heading }} 2 | 3 | .. toctree:: 4 | :maxdepth: {{ maxdepth }} 5 | {% for docname in docnames %} 6 | {{ docname }} 7 | {%- endfor %} -------------------------------------------------------------------------------- /onpolicy/onpolicy/docs/source/full.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/onpolicy/onpolicy/docs/source/full.gif -------------------------------------------------------------------------------- /onpolicy/onpolicy/docs/source/index.rst: -------------------------------------------------------------------------------- 1 | .. onpolicy documentation master file, created by 2 | sphinx-quickstart on Sun Dec 6 23:54:05 2020. 3 | You can adapt this file completely to your liking, but it should at least 4 | contain the root `toctree` directive. 5 | 6 | Welcome to onpolicy's documentation! 7 | ==================================== 8 | 9 | .. image:: full.gif 10 | 11 | .. include:: setup.rst 12 | 13 | .. include:: quickstart.rst 14 | 15 | .. toctree:: 16 | :maxdepth: 2 17 | :caption: API Reference: 18 | 19 | api/modules.rst 20 | 21 | 22 | 23 | Indices and tables 24 | ================== 25 | 26 | * :ref:`genindex` 27 | * :ref:`modindex` 28 | * :ref:`search` 29 | -------------------------------------------------------------------------------- /onpolicy/onpolicy/docs/source/quickstart.rst: -------------------------------------------------------------------------------- 1 | Quickstart: 2 | ========================================= 3 | 4 | 5 | Training the Highway Environment with MAPPO Algorithm 6 | -------------------- 7 | 8 | One line to start training the agent in Highway Environment with MAPPO Algorithm: 9 | 1. change the root path into `scripts/train/` 10 | 2. run the following code 11 | 12 | .. code-block:: bash 13 | 14 | ./train_highway.sh 15 | 16 | 17 | 18 | Hyperparameters 19 | -------------------- 20 | 21 | Within `train_highway.sh` file, `train_highway.py` under `scripts/train/` is called with part hyperparameters specified. 22 | 23 | .. code-block:: bash 24 | 25 | CUDA_VISIBLE_DEVICES=0 python train/train_highway.py --env_name ${env} --algorithm_name ${algo} --experiment_name ${exp} --scenario_name ${scenario} --task_type ${task} --n_attackers ${n_attackers} --n_defenders ${n_defenders} --n_dummies ${n_dummies} --seed ${seed} --n_training_threads 2 --n_rollout_threads 2 --n_eval_rollout_threads 2 --horizon 40 --episode_length 40 --log_interval 1 --use_wandb 26 | 27 | 28 | Hyperparameters contain two types 29 | 30 | - common hyperparameters used in all environments. Such parameters are parserd in ./config.py 31 | - private hyperparameters used in specified environment itself, which are parsered by ./scripts/train/train_.python 32 | 33 | Take highway env as an example, 34 | - the common hyperparameters are following: 35 | 36 | .. automodule:: config 37 | :members: 38 | 39 | - the private hyperparameters are following: 40 | 41 | Take highway environment as an example: 42 | 43 | .. automodule:: scripts.train.train_highway.make_train_env 44 | :members: 45 | 46 | Hierarchical structure 47 | -------------------- -------------------------------------------------------------------------------- /onpolicy/onpolicy/docs/source/setup.rst: -------------------------------------------------------------------------------- 1 | Quickstart: 2 | ========================================= 3 | .. code-block:: bash 4 | 5 | python3.7 -m venv ~/venv/python37_onpolicy 6 | source ~/venv/python37_onpolicy/bin/activate 7 | python3 -m pip install --upgrade pip 8 | pip install setuptools==51.0.0 9 | pip install wheel 10 | pip install torch==1.5.1+cu101 torchvision==0.6.1+cu101 -f https://download.pytorch.org/whl/torch_stable.html 11 | pip install wandb setproctitle absl-py gym matplotlib pandas pygame imageio tensorboardX numba 12 | # change to the root directory of the onpolicy library 13 | pip install -e . 14 | 15 | -------------------------------------------------------------------------------- /onpolicy/onpolicy/envs/GridEnv/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/onpolicy/onpolicy/envs/GridEnv/__init__.py -------------------------------------------------------------------------------- /onpolicy/onpolicy/envs/GridEnv/datasets/corner.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/onpolicy/onpolicy/envs/GridEnv/datasets/corner.pgm -------------------------------------------------------------------------------- /onpolicy/onpolicy/envs/GridEnv/datasets/corridor.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/onpolicy/onpolicy/envs/GridEnv/datasets/corridor.pgm -------------------------------------------------------------------------------- /onpolicy/onpolicy/envs/GridEnv/datasets/corridor_asym.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/onpolicy/onpolicy/envs/GridEnv/datasets/corridor_asym.pgm -------------------------------------------------------------------------------- /onpolicy/onpolicy/envs/GridEnv/datasets/corridor_sym.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/onpolicy/onpolicy/envs/GridEnv/datasets/corridor_sym.pgm -------------------------------------------------------------------------------- /onpolicy/onpolicy/envs/GridEnv/datasets/loop_with_corridor_sym.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/onpolicy/onpolicy/envs/GridEnv/datasets/loop_with_corridor_sym.pgm -------------------------------------------------------------------------------- /onpolicy/onpolicy/envs/GridEnv/datasets/room1_modified.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/onpolicy/onpolicy/envs/GridEnv/datasets/room1_modified.pgm -------------------------------------------------------------------------------- /onpolicy/onpolicy/envs/GridEnv/datasets/room_1.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/onpolicy/onpolicy/envs/GridEnv/datasets/room_1.pgm -------------------------------------------------------------------------------- /onpolicy/onpolicy/envs/GridEnv/datasets/room_2.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/onpolicy/onpolicy/envs/GridEnv/datasets/room_2.pgm -------------------------------------------------------------------------------- /onpolicy/onpolicy/envs/GridEnv/datasets/room_3.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/onpolicy/onpolicy/envs/GridEnv/datasets/room_3.pgm -------------------------------------------------------------------------------- /onpolicy/onpolicy/envs/GridEnv/datasets/room_with_corner.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/onpolicy/onpolicy/envs/GridEnv/datasets/room_with_corner.pgm -------------------------------------------------------------------------------- /onpolicy/onpolicy/envs/GridEnv/datasets/room_with_corridor.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/onpolicy/onpolicy/envs/GridEnv/datasets/room_with_corridor.pgm -------------------------------------------------------------------------------- /onpolicy/onpolicy/envs/GridEnv/datasets/square_loop.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/onpolicy/onpolicy/envs/GridEnv/datasets/square_loop.pgm -------------------------------------------------------------------------------- /onpolicy/onpolicy/envs/GridEnv/window.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import numpy as np 3 | 4 | # Only ask users to install matplotlib if they actually need it 5 | try: 6 | import matplotlib.pyplot as plt 7 | except: 8 | print('To display the environment in a window, please install matplotlib, eg:') 9 | print('pip3 install --user matplotlib') 10 | sys.exit(-1) 11 | 12 | class Window: 13 | """ 14 | Window to draw a gridworld instance using Matplotlib 15 | """ 16 | 17 | def __init__(self, title): 18 | self.fig = None 19 | 20 | self.imshow_obj = None 21 | 22 | # Create the figure and axes 23 | self.fig, self.ax = plt.subplots() 24 | 25 | # Show the env name in the window title 26 | self.fig.canvas.set_window_title(title) 27 | 28 | # Turn off x/y axis numbering/ticks 29 | self.ax.xaxis.set_ticks_position('none') 30 | self.ax.yaxis.set_ticks_position('none') 31 | _ = self.ax.set_xticklabels([]) 32 | _ = self.ax.set_yticklabels([]) 33 | 34 | # Flag indicating the window was closed 35 | self.closed = False 36 | 37 | def close_handler(evt): 38 | self.closed = True 39 | 40 | self.fig.canvas.mpl_connect('close_event', close_handler) 41 | 42 | def show_img(self, img): 43 | """ 44 | Show an image or update the image being shown 45 | """ 46 | 47 | # Show the first image of the environment 48 | if self.imshow_obj is None: 49 | self.imshow_obj = self.ax.imshow(img, interpolation='bilinear') 50 | 51 | self.imshow_obj.set_data(img) 52 | self.fig.canvas.draw() 53 | 54 | # Let matplotlib process UI events 55 | # This is needed for interactive mode to work properly 56 | plt.pause(0.001) 57 | 58 | def set_caption(self, text): 59 | """ 60 | Set/update the caption text below the image 61 | """ 62 | 63 | plt.xlabel(text) 64 | 65 | def reg_key_handler(self, key_handler): 66 | """ 67 | Register a keyboard event handler 68 | """ 69 | 70 | # Keyboard handler 71 | self.fig.canvas.mpl_connect('key_press_event', key_handler) 72 | 73 | def show(self, block=True): 74 | """ 75 | Show the window, and start an event loop 76 | """ 77 | 78 | # If not blocking, trigger interactive mode 79 | if not block: 80 | plt.ion() 81 | 82 | # Show the plot 83 | # In non-interative mode, this enters the matplotlib event loop 84 | # In interactive mode, this call does not block 85 | plt.show() 86 | 87 | def close(self): 88 | """ 89 | Close the window 90 | """ 91 | 92 | plt.close() 93 | self.closed = True -------------------------------------------------------------------------------- /onpolicy/onpolicy/envs/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | import socket 3 | from absl import flags 4 | FLAGS = flags.FLAGS 5 | FLAGS(['train_sc.py']) 6 | 7 | 8 | -------------------------------------------------------------------------------- /onpolicy/onpolicy/scripts/.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/onpolicy/onpolicy/scripts/.png -------------------------------------------------------------------------------- /onpolicy/onpolicy/scripts/train/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/onpolicy/onpolicy/scripts/train/__init__.py -------------------------------------------------------------------------------- /onpolicy/onpolicy/scripts/train_grid.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | env="GridEnv" 3 | num_agents=2 4 | num_obstacles=0 5 | algo="mappo" 6 | exp="grid_training" 7 | seed_max=1 8 | 9 | echo "env is ${env}, scenario is ${scenario}, algo is ${algo}, exp is ${exp}, max seed is ${seed_max}" 10 | for seed in `seq ${seed_max}`; 11 | do 12 | echo "seed is ${seed}:" 13 | CUDA_VISIBLE_DEVICES=0 python train/train_grid.py \ 14 | --env_name ${env} --algorithm_name ${algo} --experiment_name ${exp} \ 15 | --log_interval 1 --use_wandb --wandb_name "mapping" --user_name "xyf" --num_agents ${num_agents}\ 16 | --num_obstacles ${num_obstacles} --cnn_layers_params '16,3,1,1 32,3,1,1 16,3,1,1' --hidden_size 64 --seed 1 --n_training_threads 1 \ 17 | --n_rollout_threads 40 --num_mini_batch 1 --num_env_steps 50000000 --ppo_epoch 3 --gain 0.01 \ 18 | --lr 1e-3 --critic_lr 1e-3 --max_steps 20 --use_complete_reward --use_intrinsic_reward --use_eval --n_eval_rollout_threads 1 \ 19 | --agent_view_size 7 --local_step_num 1 --use_random_pos --use_same_location --use_merge --use_recurrent_policy 20 | done 21 | # --model_dir "/home/nics/workspace/onpolicy/onpolicy/scripts/results/Gazebo/simple_spread/mappo/new_same_loc_step100_envs50_ppo3_lr5e-4_random_grid_scene_short_goal_asyn_num1_intrinsic/run55/models" 22 | # --model_dir "/home/nics/workspace/onpolicy/onpolicy/scripts/results/Gazebo/simple_spread/mappo/new_same_loc_step100_envs50_ppo3_lr5e-4_random_grid_scene_short_goal_asyn_num1_intrinsic/run137/models" 23 | # --model_dir "/home/nics/workspace/onpolicy/onpolicy/scripts/results/Gazebo/simple_spread/mappo/new_same_loc_step100_envs50_ppo3_lr5e-4_random_grid_scene_short_goal_asyn_num1_intrinsic/run143/models" -------------------------------------------------------------------------------- /onpolicy/onpolicy/utils/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/onpolicy/onpolicy/utils/__init__.py -------------------------------------------------------------------------------- /onpolicy/onpolicy/utils/multi_discrete.py: -------------------------------------------------------------------------------- 1 | import gym 2 | import numpy as np 3 | 4 | # An old version of OpenAI Gym's multi_discrete.py. (Was getting affected by Gym updates) 5 | # (https://github.com/openai/gym/blob/1fb81d4e3fb780ccf77fec731287ba07da35eb84/gym/spaces/multi_discrete.py) 6 | class MultiDiscrete(gym.Space): 7 | """ 8 | - The multi-discrete action space consists of a series of discrete action spaces with different parameters 9 | - It can be adapted to both a Discrete action space or a continuous (Box) action space 10 | - It is useful to represent game controllers or keyboards where each key can be represented as a discrete action space 11 | - It is parametrized by passing an array of arrays containing [min, max] for each discrete action space where the discrete action space can take any integers from `min` to `max` (both inclusive) 12 | Note: A value of 0 always need to represent the NOOP action. 13 | e.g. Nintendo Game Controller 14 | - Can be conceptualized as 3 discrete action spaces: 15 | 1) Arrow Keys: Discrete 5 - NOOP[0], UP[1], RIGHT[2], DOWN[3], LEFT[4] - params: min: 0, max: 4 16 | 2) Button A: Discrete 2 - NOOP[0], Pressed[1] - params: min: 0, max: 1 17 | 3) Button B: Discrete 2 - NOOP[0], Pressed[1] - params: min: 0, max: 1 18 | - Can be initialized as 19 | MultiDiscrete([ [0,4], [0,1], [0,1] ]) 20 | """ 21 | 22 | def __init__(self, array_of_param_array): 23 | self.low = np.array([x[0] for x in array_of_param_array]) 24 | self.high = np.array([x[1] for x in array_of_param_array]) 25 | self.num_discrete_space = self.low.shape[0] 26 | self.n = np.sum(self.high) + 2 27 | 28 | def sample(self): 29 | """ Returns a array with one sample from each discrete action space """ 30 | # For each row: round(random .* (max - min) + min, 0) 31 | random_array = np.random.rand(self.num_discrete_space) 32 | return [int(x) for x in np.floor(np.multiply((self.high - self.low + 1.), random_array) + self.low)] 33 | 34 | def contains(self, x): 35 | return len(x) == self.num_discrete_space and (np.array(x) >= self.low).all() and (np.array(x) <= self.high).all() 36 | 37 | @property 38 | def shape(self): 39 | return self.num_discrete_space 40 | 41 | def __repr__(self): 42 | return "MultiDiscrete" + str(self.num_discrete_space) 43 | 44 | def __eq__(self, other): 45 | return np.array_equal(self.low, other.low) and np.array_equal(self.high, other.high) 46 | -------------------------------------------------------------------------------- /onpolicy/onpolicy/utils/util.py: -------------------------------------------------------------------------------- 1 | import glob 2 | import os 3 | import numpy as np 4 | import math 5 | import torch 6 | import torch.nn as nn 7 | 8 | def check(input): 9 | if type(input) == np.ndarray: 10 | return torch.from_numpy(input) 11 | 12 | def get_gard_norm(it): 13 | sum_grad = 0 14 | for x in it: 15 | if x.grad is None: 16 | continue 17 | sum_grad += x.grad.norm() ** 2 18 | return math.sqrt(sum_grad) 19 | 20 | def update_linear_schedule(optimizer, epoch, total_num_epochs, initial_lr): 21 | """Decreases the learning rate linearly""" 22 | lr = initial_lr - (initial_lr * (epoch / float(total_num_epochs))) 23 | for param_group in optimizer.param_groups: 24 | param_group['lr'] = lr 25 | 26 | def huber_loss(e, d): 27 | a = (abs(e) <= d).float() 28 | b = (e > d).float() 29 | return a*e**2/2 + b*d*(abs(e)-d/2) 30 | 31 | def mse_loss(e): 32 | return e**2/2 33 | 34 | def get_shape_from_obs_space(obs_space): 35 | if obs_space.__class__.__name__ == 'Box': 36 | obs_shape = obs_space.shape 37 | elif obs_space.__class__.__name__ == 'list': 38 | obs_shape = obs_space 39 | elif obs_space.__class__.__name__ == 'Dict': 40 | obs_shape = obs_space.spaces 41 | else: 42 | raise NotImplementedError 43 | return obs_shape 44 | 45 | def get_shape_from_act_space(act_space): 46 | if act_space.__class__.__name__ == 'Discrete': 47 | act_shape = 1 48 | elif act_space.__class__.__name__ == "MultiDiscrete": 49 | act_shape = act_space.shape 50 | elif act_space.__class__.__name__ == "Box": 51 | act_shape = act_space.shape[0] 52 | elif act_space.__class__.__name__ == "MultiBinary": 53 | act_shape = act_space.shape[0] 54 | else: # agar 55 | act_shape = act_space[0].shape[0] + 1 56 | return act_shape 57 | 58 | 59 | def tile_images(img_nhwc): 60 | """ 61 | Tile N images into one big PxQ image 62 | (P,Q) are chosen to be as close as possible, and if N 63 | is square, then P=Q. 64 | input: img_nhwc, list or array of images, ndim=4 once turned into array 65 | n = batch index, h = height, w = width, c = channel 66 | returns: 67 | bigim_HWc, ndarray with ndim=3 68 | """ 69 | img_nhwc = np.asarray(img_nhwc) 70 | N, h, w, c = img_nhwc.shape 71 | H = int(np.ceil(np.sqrt(N))) 72 | W = int(np.ceil(float(N)/H)) 73 | img_nhwc = np.array(list(img_nhwc) + [img_nhwc[0]*0 for _ in range(N, H*W)]) 74 | img_HWhwc = img_nhwc.reshape(H, W, h, w, c) 75 | img_HhWwc = img_HWhwc.transpose(0, 2, 1, 3, 4) 76 | img_Hh_Ww_c = img_HhWwc.reshape(H*h, W*w, c) 77 | return img_Hh_Ww_c -------------------------------------------------------------------------------- /onpolicy/onpolicy/utils/valuenorm.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | 4 | import torch 5 | import torch.nn as nn 6 | 7 | 8 | class ValueNorm(nn.Module): 9 | """ Normalize a vector of observations - across the first norm_axes dimensions""" 10 | 11 | def __init__(self, input_shape, norm_axes=1, beta=0.99999, per_element_update=False, epsilon=1e-5, device=torch.device("cpu")): 12 | super(ValueNorm, self).__init__() 13 | 14 | self.input_shape = input_shape 15 | self.norm_axes = norm_axes 16 | self.epsilon = epsilon 17 | self.beta = beta 18 | self.per_element_update = per_element_update 19 | self.tpdv = dict(dtype=torch.float32, device=device) 20 | 21 | self.running_mean = nn.Parameter(torch.zeros(input_shape), requires_grad=False).to(**self.tpdv) 22 | self.running_mean_sq = nn.Parameter(torch.zeros(input_shape), requires_grad=False).to(**self.tpdv) 23 | self.debiasing_term = nn.Parameter(torch.tensor(0.0), requires_grad=False).to(**self.tpdv) 24 | 25 | self.reset_parameters() 26 | 27 | def reset_parameters(self): 28 | self.running_mean.zero_() 29 | self.running_mean_sq.zero_() 30 | self.debiasing_term.zero_() 31 | 32 | def running_mean_var(self): 33 | debiased_mean = self.running_mean / self.debiasing_term.clamp(min=self.epsilon) 34 | debiased_mean_sq = self.running_mean_sq / self.debiasing_term.clamp(min=self.epsilon) 35 | debiased_var = (debiased_mean_sq - debiased_mean ** 2).clamp(min=1e-2) 36 | return debiased_mean, debiased_var 37 | 38 | @torch.no_grad() 39 | def update(self, input_vector): 40 | if type(input_vector) == np.ndarray: 41 | input_vector = torch.from_numpy(input_vector) 42 | input_vector = input_vector.to(**self.tpdv) 43 | 44 | batch_mean = input_vector.mean(dim=tuple(range(self.norm_axes))) 45 | batch_sq_mean = (input_vector ** 2).mean(dim=tuple(range(self.norm_axes))) 46 | 47 | if self.per_element_update: 48 | batch_size = np.prod(input_vector.size()[:self.norm_axes]) 49 | weight = self.beta ** batch_size 50 | else: 51 | weight = self.beta 52 | 53 | self.running_mean.mul_(weight).add_(batch_mean * (1.0 - weight)) 54 | self.running_mean_sq.mul_(weight).add_(batch_sq_mean * (1.0 - weight)) 55 | self.debiasing_term.mul_(weight).add_(1.0 * (1.0 - weight)) 56 | 57 | def normalize(self, input_vector): 58 | # Make sure input is float32 59 | if type(input_vector) == np.ndarray: 60 | input_vector = torch.from_numpy(input_vector) 61 | input_vector = input_vector.to(**self.tpdv) 62 | 63 | mean, var = self.running_mean_var() 64 | out = (input_vector - mean[(None,) * self.norm_axes]) / torch.sqrt(var)[(None,) * self.norm_axes] 65 | 66 | return out 67 | 68 | def denormalize(self, input_vector): 69 | """ Transform normalized data back into original distribution """ 70 | if type(input_vector) == np.ndarray: 71 | input_vector = torch.from_numpy(input_vector) 72 | input_vector = input_vector.to(**self.tpdv) 73 | 74 | mean, var = self.running_mean_var() 75 | out = input_vector * torch.sqrt(var)[(None,) * self.norm_axes] + mean[(None,) * self.norm_axes] 76 | 77 | out = out.cpu().numpy() 78 | 79 | return out 80 | -------------------------------------------------------------------------------- /onpolicy/render_human.sh: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/onpolicy/render_human.sh -------------------------------------------------------------------------------- /onpolicy/requirements.txt: -------------------------------------------------------------------------------- 1 | absl-py==0.9.0 2 | aiohttp==3.6.2 3 | aioredis==1.3.1 4 | astor==0.8.0 5 | astunparse==1.6.3 6 | async-timeout==3.0.1 7 | atari-py==0.2.6 8 | atomicwrites==1.2.1 9 | attrs==18.2.0 10 | beautifulsoup4==4.9.1 11 | blessings==1.7 12 | cachetools==4.1.1 13 | certifi==2020.4.5.2 14 | cffi==1.14.1 15 | chardet==3.0.4 16 | click==7.1.2 17 | cloudpickle==1.3.0 18 | colorama==0.4.3 19 | colorful==0.5.4 20 | configparser==5.0.1 21 | contextvars==2.4 22 | cycler==0.10.0 23 | Cython==0.29.21 24 | deepdiff==4.3.2 25 | dill==0.3.2 26 | docker-pycreds==0.4.0 27 | docopt==0.6.2 28 | fasteners==0.15 29 | filelock==3.0.12 30 | funcsigs==1.0.2 31 | future==0.16.0 32 | gast==0.2.2 33 | gin==0.1.6 34 | gin-config==0.3.0 35 | gitdb==4.0.5 36 | GitPython==3.1.9 37 | glfw==1.12.0 38 | google==3.0.0 39 | google-api-core==1.22.1 40 | google-auth==1.21.0 41 | google-auth-oauthlib==0.4.1 42 | google-pasta==0.2.0 43 | googleapis-common-protos==1.52.0 44 | gpustat==0.6.0 45 | gql==0.2.0 46 | graphql-core==1.1 47 | grpcio==1.31.0 48 | gym==0.17.2 49 | h5py==2.10.0 50 | hiredis==1.1.0 51 | idna==2.7 52 | idna-ssl==1.1.0 53 | imageio==2.4.1 54 | immutables==0.14 55 | importlib-metadata==1.7.0 56 | joblib==0.16.0 57 | jsonnet==0.16.0 58 | jsonpickle==0.9.6 59 | jsonschema==3.2.0 60 | Keras-Applications==1.0.8 61 | Keras-Preprocessing==1.1.2 62 | kiwisolver==1.0.1 63 | lockfile==0.12.2 64 | Markdown==3.1.1 65 | matplotlib==3.0.0 66 | mkl-fft==1.1.0 67 | mkl-random==1.1.1 68 | mkl-service==2.3.0 69 | mock==2.0.0 70 | monotonic==1.5 71 | more-itertools==4.3.0 72 | mpi4py==3.0.3 73 | mpyq==0.2.5 74 | msgpack==1.0.0 75 | mujoco-py==2.0.2.13 76 | multidict==4.7.6 77 | munch==2.3.2 78 | numpy==1.18.5 79 | nvidia-ml-py3==7.352.0 80 | oauthlib==3.1.0 81 | opencensus==0.7.10 82 | opencensus-context==0.1.1 83 | opencv-python==4.2.0.34 84 | opt-einsum==3.1.0 85 | ordered-set==4.0.2 86 | packaging==20.4 87 | pandas==1.1.1 88 | pathlib2==2.3.2 89 | pathtools==0.1.2 90 | pbr==4.3.0 91 | Pillow==5.3.0 92 | pluggy==0.7.1 93 | portpicker==1.2.0 94 | probscale==0.2.3 95 | progressbar2==3.53.1 96 | prometheus-client==0.8.0 97 | promise==2.3 98 | protobuf==3.12.3 99 | psutil==5.7.2 100 | py==1.6.0 101 | py-spy==0.3.3 102 | pyasn1==0.4.8 103 | pyasn1-modules==0.2.8 104 | pycparser==2.20 105 | pygame==1.9.4 106 | pyglet==1.5.0 107 | PyOpenGL==3.1.5 108 | PyOpenGL-accelerate==3.1.5 109 | pyparsing==2.2.2 110 | pyrsistent==0.16.0 111 | PySC2==3.0.0 112 | pytest==3.8.2 113 | python-dateutil==2.7.3 114 | python-utils==2.4.0 115 | pytz==2020.1 116 | PyYAML==3.13 117 | pyzmq==19.0.2 118 | ray==0.8.0 119 | redis==3.4.1 120 | requests==2.24.0 121 | requests-oauthlib==1.3.0 122 | rsa==4.6 123 | s2clientprotocol==4.10.1.75800.0 124 | s2protocol==4.11.4.78285.0 125 | sacred==0.7.2 126 | scipy==1.4.1 127 | seaborn==0.10.1 128 | sentry-sdk==0.18.0 129 | setproctitle==1.1.10 130 | shortuuid==1.0.1 131 | six==1.15.0 132 | sk-video==1.1.10 133 | smmap==3.0.4 134 | snakeviz==1.0.0 135 | soupsieve==2.0.1 136 | subprocess32==3.5.4 137 | tabulate==0.8.7 138 | tensorboard==2.0.2 139 | tensorboard-logger==0.1.0 140 | tensorboard-plugin-wit==1.7.0 141 | tensorboardX==2.0 142 | tensorflow==2.0.0 143 | tensorflow-estimator==2.0.0 144 | termcolor==1.1.0 145 | torch==1.5.1+cu101 146 | torchvision==0.6.1+cu101 147 | tornado==5.1.1 148 | tqdm==4.48.2 149 | typing-extensions==3.7.4.3 150 | urllib3==1.23 151 | wandb==0.10.5 152 | watchdog==0.10.3 153 | websocket-client==0.53.0 154 | Werkzeug==0.16.1 155 | whichcraft==0.5.2 156 | wrapt==1.12.1 157 | xmltodict==0.12.0 158 | yarl==1.5.1 159 | zipp==3.1.0 160 | zmq==0.0.0 161 | -------------------------------------------------------------------------------- /onpolicy/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | import os 5 | from setuptools import setup, find_packages 6 | import setuptools 7 | 8 | def get_version(): 9 | # https://packaging.python.org/guides/single-sourcing-package-version/ 10 | init = open(os.path.join("onpolicy", "__init__.py"), "r").read().split() 11 | return init[init.index("__version__") + 2][1:-1] 12 | 13 | setup( 14 | name="onpolicy", # Replace with your own username 15 | version=get_version(), 16 | description="on-policy algorithms of marlbenchmark", 17 | long_description=open("README.md", encoding="utf8").read(), 18 | long_description_content_type="text/markdown", 19 | author="yuchao", 20 | author_email="zoeyuchao@gmail.com", 21 | packages=setuptools.find_packages(), 22 | classifiers=[ 23 | "Development Status :: 3 - Alpha", 24 | "Intended Audience :: Science/Research", 25 | "Topic :: Scientific/Engineering :: Artificial Intelligence", 26 | "Topic :: Software Development :: Libraries :: Python Modules", 27 | "Programming Language :: Python :: 3", 28 | "License :: OSI Approved :: MIT License", 29 | "Operating System :: OS Independent", 30 | ], 31 | keywords="multi-agent reinforcement learning platform pytorch", 32 | python_requires='>=3.6', 33 | ) 34 | -------------------------------------------------------------------------------- /sim_env/config/turtlebot3_lds_2d.lua: -------------------------------------------------------------------------------- 1 | include "map_builder.lua" 2 | include "trajectory_builder.lua" 3 | 4 | options = { 5 | map_builder = MAP_BUILDER, 6 | trajectory_builder = TRAJECTORY_BUILDER, 7 | map_frame = "map", 8 | tracking_frame = "base_footprint", -- imu_link, If you are using gazebo, use 'base_footprint' (libgazebo_ros_imu's bug) 9 | published_frame = "odom", 10 | odom_frame = "odom", 11 | provide_odom_frame = false, 12 | publish_frame_projected_to_2d = false, 13 | use_odometry = true, 14 | use_nav_sat = false, 15 | use_landmarks = false, 16 | num_laser_scans = 1, 17 | num_multi_echo_laser_scans = 0, 18 | num_subdivisions_per_laser_scan = 1, 19 | num_point_clouds = 0, 20 | lookup_transform_timeout_sec = 0.2, 21 | submap_publish_period_sec = 0.3, 22 | pose_publish_period_sec = 5e-3, 23 | trajectory_publish_period_sec = 30e-3, 24 | rangefinder_sampling_ratio = 1., 25 | odometry_sampling_ratio = 1., 26 | fixed_frame_pose_sampling_ratio = 1., 27 | imu_sampling_ratio = 1., 28 | landmarks_sampling_ratio = 1., 29 | } 30 | 31 | MAP_BUILDER.use_trajectory_builder_2d = true 32 | TRAJECTORY_BUILDER_2D.submaps.num_range_data = 20 33 | 34 | TRAJECTORY_BUILDER_2D.min_range = 0.1 35 | TRAJECTORY_BUILDER_2D.max_range = 7 36 | TRAJECTORY_BUILDER_2D.missing_data_ray_length = 3. 37 | TRAJECTORY_BUILDER_2D.use_imu_data = true 38 | TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true 39 | TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1) 40 | 41 | POSE_GRAPH.constraint_builder.min_score = 0.65 42 | POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7 43 | 44 | return options 45 | -------------------------------------------------------------------------------- /sim_env/config/turtlebot3_robot1.lua: -------------------------------------------------------------------------------- 1 | include "map_builder.lua" 2 | include "trajectory_builder.lua" 3 | 4 | options = { 5 | map_builder = MAP_BUILDER, 6 | trajectory_builder = TRAJECTORY_BUILDER, 7 | map_frame = "robot1/map", 8 | tracking_frame = "robot1/base_footprint", -- imu_link, If you are using gazebo, use 'base_footprint' (libgazebo_ros_imu's bug) 9 | published_frame = "robot1/odom", 10 | odom_frame = "robot1/odom", 11 | provide_odom_frame = false, 12 | publish_frame_projected_to_2d = false, 13 | use_odometry = true, 14 | use_nav_sat = false, 15 | use_landmarks = false, 16 | num_laser_scans = 1, 17 | num_multi_echo_laser_scans = 0, 18 | num_subdivisions_per_laser_scan = 1, 19 | num_point_clouds = 0, 20 | lookup_transform_timeout_sec = 0.2, 21 | submap_publish_period_sec = 0.3, 22 | pose_publish_period_sec = 5e-3, 23 | trajectory_publish_period_sec = 30e-3, 24 | rangefinder_sampling_ratio = 1., 25 | odometry_sampling_ratio = 1., 26 | fixed_frame_pose_sampling_ratio = 1., 27 | imu_sampling_ratio = 1., 28 | landmarks_sampling_ratio = 1., 29 | } 30 | 31 | MAP_BUILDER.use_trajectory_builder_2d = true 32 | TRAJECTORY_BUILDER_2D.submaps.num_range_data = 20 33 | 34 | TRAJECTORY_BUILDER_2D.min_range = 0.1 35 | TRAJECTORY_BUILDER_2D.max_range = 7 36 | TRAJECTORY_BUILDER_2D.missing_data_ray_length = 7. 37 | TRAJECTORY_BUILDER_2D.use_imu_data = true 38 | TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true 39 | TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1) 40 | 41 | POSE_GRAPH.constraint_builder.min_score = 0.8 42 | POSE_GRAPH.constraint_builder.global_localization_min_score = 0.80 43 | 44 | 45 | return options 46 | -------------------------------------------------------------------------------- /sim_env/config/turtlebot3_robot2.lua: -------------------------------------------------------------------------------- 1 | include "map_builder.lua" 2 | include "trajectory_builder.lua" 3 | 4 | options = { 5 | map_builder = MAP_BUILDER, 6 | trajectory_builder = TRAJECTORY_BUILDER, 7 | map_frame = "robot2/map", 8 | tracking_frame = "robot2/base_footprint", -- imu_link, If you are using gazebo, use 'base_footprint' (libgazebo_ros_imu's bug) 9 | published_frame = "robot2/odom", 10 | odom_frame = "robot2/odom", 11 | provide_odom_frame = false, 12 | publish_frame_projected_to_2d = false, 13 | use_odometry = true, 14 | use_nav_sat = false, 15 | use_landmarks = false, 16 | num_laser_scans = 1, 17 | num_multi_echo_laser_scans = 0, 18 | num_subdivisions_per_laser_scan = 1, 19 | num_point_clouds = 0, 20 | lookup_transform_timeout_sec = 0.2, 21 | submap_publish_period_sec = 0.3, 22 | pose_publish_period_sec = 5e-3, 23 | trajectory_publish_period_sec = 30e-3, 24 | rangefinder_sampling_ratio = 1., 25 | odometry_sampling_ratio = 1., 26 | fixed_frame_pose_sampling_ratio = 1., 27 | imu_sampling_ratio = 1., 28 | landmarks_sampling_ratio = 1., 29 | } 30 | 31 | MAP_BUILDER.use_trajectory_builder_2d = true 32 | TRAJECTORY_BUILDER_2D.submaps.num_range_data = 20 33 | 34 | TRAJECTORY_BUILDER_2D.min_range = 0.1 35 | TRAJECTORY_BUILDER_2D.max_range = 7 36 | TRAJECTORY_BUILDER_2D.missing_data_ray_length = 7. 37 | TRAJECTORY_BUILDER_2D.use_imu_data = true 38 | TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true 39 | TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1) 40 | 41 | POSE_GRAPH.constraint_builder.min_score = 0.8 42 | POSE_GRAPH.constraint_builder.global_localization_min_score = 0.8 43 | 44 | 45 | return options 46 | -------------------------------------------------------------------------------- /sim_env/config/turtlebot3_robot3.lua: -------------------------------------------------------------------------------- 1 | include "map_builder.lua" 2 | include "trajectory_builder.lua" 3 | 4 | options = { 5 | map_builder = MAP_BUILDER, 6 | trajectory_builder = TRAJECTORY_BUILDER, 7 | map_frame = "robot3/map", 8 | tracking_frame = "robot3/base_footprint", -- imu_link, If you are using gazebo, use 'base_footprint' (libgazebo_ros_imu's bug) 9 | published_frame = "robot3/odom", 10 | odom_frame = "robot3/odom", 11 | provide_odom_frame = false, 12 | publish_frame_projected_to_2d = false, 13 | use_odometry = true, 14 | use_nav_sat = false, 15 | use_landmarks = false, 16 | num_laser_scans = 1, 17 | num_multi_echo_laser_scans = 0, 18 | num_subdivisions_per_laser_scan = 1, 19 | num_point_clouds = 0, 20 | lookup_transform_timeout_sec = 0.2, 21 | submap_publish_period_sec = 0.3, 22 | pose_publish_period_sec = 5e-3, 23 | trajectory_publish_period_sec = 30e-3, 24 | rangefinder_sampling_ratio = 1., 25 | odometry_sampling_ratio = 1., 26 | fixed_frame_pose_sampling_ratio = 1., 27 | imu_sampling_ratio = 1., 28 | landmarks_sampling_ratio = 1., 29 | } 30 | 31 | MAP_BUILDER.use_trajectory_builder_2d = true 32 | TRAJECTORY_BUILDER_2D.submaps.num_range_data = 20 33 | 34 | TRAJECTORY_BUILDER_2D.min_range = 0.1 35 | TRAJECTORY_BUILDER_2D.max_range = 7 36 | TRAJECTORY_BUILDER_2D.missing_data_ray_length = 7. 37 | TRAJECTORY_BUILDER_2D.use_imu_data = true 38 | TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true 39 | TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1) 40 | 41 | POSE_GRAPH.constraint_builder.min_score = 0.8 42 | POSE_GRAPH.constraint_builder.global_localization_min_score = 0.8 43 | 44 | return options 45 | -------------------------------------------------------------------------------- /sim_env/config/turtlebot3_robot4.lua: -------------------------------------------------------------------------------- 1 | include "map_builder.lua" 2 | include "trajectory_builder.lua" 3 | 4 | options = { 5 | map_builder = MAP_BUILDER, 6 | trajectory_builder = TRAJECTORY_BUILDER, 7 | map_frame = "robot4/map", 8 | tracking_frame = "robot4/base_footprint", -- imu_link, If you are using gazebo, use 'base_footprint' (libgazebo_ros_imu's bug) 9 | published_frame = "robot4/odom", 10 | odom_frame = "robot4/odom", 11 | provide_odom_frame = false, 12 | publish_frame_projected_to_2d = false, 13 | use_odometry = true, 14 | use_nav_sat = false, 15 | use_landmarks = false, 16 | num_laser_scans = 1, 17 | num_multi_echo_laser_scans = 0, 18 | num_subdivisions_per_laser_scan = 1, 19 | num_point_clouds = 0, 20 | lookup_transform_timeout_sec = 0.2, 21 | submap_publish_period_sec = 0.3, 22 | pose_publish_period_sec = 5e-3, 23 | trajectory_publish_period_sec = 30e-3, 24 | rangefinder_sampling_ratio = 1., 25 | odometry_sampling_ratio = 1., 26 | fixed_frame_pose_sampling_ratio = 1., 27 | imu_sampling_ratio = 1., 28 | landmarks_sampling_ratio = 1., 29 | } 30 | 31 | MAP_BUILDER.use_trajectory_builder_2d = true 32 | TRAJECTORY_BUILDER_2D.submaps.num_range_data = 20 33 | 34 | TRAJECTORY_BUILDER_2D.min_range = 0.1 35 | TRAJECTORY_BUILDER_2D.max_range = 7 36 | TRAJECTORY_BUILDER_2D.missing_data_ray_length = 7. 37 | TRAJECTORY_BUILDER_2D.use_imu_data = true 38 | TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true 39 | TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1) 40 | 41 | POSE_GRAPH.constraint_builder.min_score = 0.8 42 | POSE_GRAPH.constraint_builder.global_localization_min_score = 0.8 43 | 44 | return options 45 | -------------------------------------------------------------------------------- /sim_env/config/turtlebot3_robot5.lua: -------------------------------------------------------------------------------- 1 | include "map_builder.lua" 2 | include "trajectory_builder.lua" 3 | 4 | options = { 5 | map_builder = MAP_BUILDER, 6 | trajectory_builder = TRAJECTORY_BUILDER, 7 | map_frame = "robot5/map", 8 | tracking_frame = "robot5/base_footprint", -- imu_link, If you are using gazebo, use 'base_footprint' (libgazebo_ros_imu's bug) 9 | published_frame = "robot5/odom", 10 | odom_frame = "robot5/odom", 11 | provide_odom_frame = false, 12 | publish_frame_projected_to_2d = false, 13 | use_odometry = true, 14 | use_nav_sat = false, 15 | use_landmarks = false, 16 | num_laser_scans = 1, 17 | num_multi_echo_laser_scans = 0, 18 | num_subdivisions_per_laser_scan = 1, 19 | num_point_clouds = 0, 20 | lookup_transform_timeout_sec = 0.2, 21 | submap_publish_period_sec = 0.3, 22 | pose_publish_period_sec = 5e-3, 23 | trajectory_publish_period_sec = 30e-3, 24 | rangefinder_sampling_ratio = 1., 25 | odometry_sampling_ratio = 1., 26 | fixed_frame_pose_sampling_ratio = 1., 27 | imu_sampling_ratio = 1., 28 | landmarks_sampling_ratio = 1., 29 | } 30 | 31 | MAP_BUILDER.use_trajectory_builder_2d = true 32 | TRAJECTORY_BUILDER_2D.submaps.num_range_data = 20 33 | 34 | TRAJECTORY_BUILDER_2D.min_range = 0.1 35 | TRAJECTORY_BUILDER_2D.max_range = 7 36 | TRAJECTORY_BUILDER_2D.missing_data_ray_length = 7. 37 | TRAJECTORY_BUILDER_2D.use_imu_data = true 38 | TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true 39 | TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1) 40 | 41 | POSE_GRAPH.constraint_builder.min_score = 0.8 42 | POSE_GRAPH.constraint_builder.global_localization_min_score = 0.8 43 | 44 | return options 45 | -------------------------------------------------------------------------------- /sim_env/config/turtlebot3_robot6.lua: -------------------------------------------------------------------------------- 1 | include "map_builder.lua" 2 | include "trajectory_builder.lua" 3 | 4 | options = { 5 | map_builder = MAP_BUILDER, 6 | trajectory_builder = TRAJECTORY_BUILDER, 7 | map_frame = "robot6/map", 8 | tracking_frame = "robot6/base_footprint", -- imu_link, If you are using gazebo, use 'base_footprint' (libgazebo_ros_imu's bug) 9 | published_frame = "robot6/odom", 10 | odom_frame = "robot6/odom", 11 | provide_odom_frame = false, 12 | publish_frame_projected_to_2d = false, 13 | use_odometry = true, 14 | use_nav_sat = false, 15 | use_landmarks = false, 16 | num_laser_scans = 1, 17 | num_multi_echo_laser_scans = 0, 18 | num_subdivisions_per_laser_scan = 1, 19 | num_point_clouds = 0, 20 | lookup_transform_timeout_sec = 0.2, 21 | submap_publish_period_sec = 0.3, 22 | pose_publish_period_sec = 5e-3, 23 | trajectory_publish_period_sec = 30e-3, 24 | rangefinder_sampling_ratio = 1., 25 | odometry_sampling_ratio = 1., 26 | fixed_frame_pose_sampling_ratio = 1., 27 | imu_sampling_ratio = 1., 28 | landmarks_sampling_ratio = 1., 29 | } 30 | 31 | MAP_BUILDER.use_trajectory_builder_2d = true 32 | TRAJECTORY_BUILDER_2D.submaps.num_range_data = 20 33 | 34 | TRAJECTORY_BUILDER_2D.min_range = 0.1 35 | TRAJECTORY_BUILDER_2D.max_range = 7 36 | TRAJECTORY_BUILDER_2D.missing_data_ray_length = 7. 37 | TRAJECTORY_BUILDER_2D.use_imu_data = true 38 | TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true 39 | TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1) 40 | 41 | POSE_GRAPH.constraint_builder.min_score = 0.8 42 | POSE_GRAPH.constraint_builder.global_localization_min_score = 0.8 43 | 44 | return options 45 | -------------------------------------------------------------------------------- /sim_env/gazebo_images/main_body.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/sim_env/gazebo_images/main_body.jpg -------------------------------------------------------------------------------- /sim_env/gazebo_images/meshes/images/main_body.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/sim_env/gazebo_images/meshes/images/main_body.jpg -------------------------------------------------------------------------------- /sim_env/gazebo_images/meshes/images/wheel.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/sim_env/gazebo_images/meshes/images/wheel.jpg -------------------------------------------------------------------------------- /sim_env/gazebo_images/wheel.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/efc-robot/Explore-Bench/ce3b98a09bf593b0716b26555dc4cccbd9e83cd3/sim_env/gazebo_images/wheel.jpg -------------------------------------------------------------------------------- /sim_env/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 | -------------------------------------------------------------------------------- /sim_env/launch/includes/cartographer_teb.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 18 | 19 | 20 | 21 | 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 | -------------------------------------------------------------------------------- /sim_env/launch/includes/move_base_robot.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 | -------------------------------------------------------------------------------- /sim_env/launch/includes/robot.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /sim_env/launch/robots_mapping/single_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /sim_env/launch/robots_mapping/two_robots.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /sim_env/launch/simulation_environment/corner_close_env_two_robots.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 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 | -------------------------------------------------------------------------------- /sim_env/launch/simulation_environment/corner_env_single_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /sim_env/launch/simulation_environment/corner_far_env_two_robots.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 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 | -------------------------------------------------------------------------------- /sim_env/launch/simulation_environment/corridor_close_env_two_robots.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 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 | -------------------------------------------------------------------------------- /sim_env/launch/simulation_environment/corridor_env_single_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /sim_env/launch/simulation_environment/corridor_far_env_two_robots.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 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 | -------------------------------------------------------------------------------- /sim_env/launch/simulation_environment/loop_env_close_two_robots.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /sim_env/launch/simulation_environment/loop_env_far_two_robots.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /sim_env/launch/simulation_environment/loop_env_single_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /sim_env/launch/simulation_environment/loop_with_corridor_close_env_two_robots.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 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 | -------------------------------------------------------------------------------- /sim_env/launch/simulation_environment/loop_with_corridor_env_single_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /sim_env/launch/simulation_environment/loop_with_corridor_far_env_two_robots.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 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 | -------------------------------------------------------------------------------- /sim_env/launch/simulation_environment/room_close_env_two_robots.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 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 | -------------------------------------------------------------------------------- /sim_env/launch/simulation_environment/room_env_single_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /sim_env/launch/simulation_environment/room_far_env_two_robots.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 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 | -------------------------------------------------------------------------------- /sim_env/launch/simulation_environment/room_with_corner_close_env_two_robots.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 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 | -------------------------------------------------------------------------------- /sim_env/launch/simulation_environment/room_with_corner_env_single_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /sim_env/launch/simulation_environment/room_with_corner_far_env_two_robots.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 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 | -------------------------------------------------------------------------------- /sim_env/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sim_env 4 | 0.0.0 5 | The simulation package 6 | 7 | 8 | 9 | 10 | nics 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 | geometry_msgs 53 | message_generation 54 | nav_msgs 55 | roscpp 56 | rospy 57 | std_msgs 58 | tf 59 | visualization_msgs 60 | geometry_msgs 61 | nav_msgs 62 | roscpp 63 | rospy 64 | std_msgs 65 | tf 66 | visualization_msgs 67 | geometry_msgs 68 | nav_msgs 69 | roscpp 70 | rospy 71 | std_msgs 72 | tf 73 | visualization_msgs 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | -------------------------------------------------------------------------------- /sim_env/param_teb/costmap_common_params_burger.yaml: -------------------------------------------------------------------------------- 1 | obstacle_range: 3.0 2 | raytrace_range: 3.5 3 | 4 | footprint: [[-0.105, -0.105], [-0.105, 0.105], [0.041, 0.105], [0.041, -0.105]] 5 | #robot_radius: 0.105 6 | 7 | inflation_radius: 0.4 8 | cost_scaling_factor: 1 9 | 10 | map_type: costmap 11 | observation_sources: laser_scan_sensor 12 | laser_scan_sensor: {sensor_frame: base_scan, data_type: LaserScan, topic: scan, marking: true, clearing: true} -------------------------------------------------------------------------------- /sim_env/param_teb/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: map 3 | robot_base_frame: base_footprint 4 | 5 | update_frequency: 10.0 6 | publish_frequency: 10.0 7 | transform_tolerance: 0.5 8 | always_send_full_costmap: true 9 | 10 | static_map: true 11 | 12 | -------------------------------------------------------------------------------- /sim_env/param_teb/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: odom 3 | robot_base_frame: base_footprint 4 | 5 | update_frequency: 10.0 6 | publish_frequency: 10.0 7 | transform_tolerance: 0.5 8 | 9 | static_map: false 10 | rolling_window: true 11 | width: 3 12 | height: 3 13 | resolution: 0.05 14 | 15 | -------------------------------------------------------------------------------- /sim_env/param_teb/move_base_params.yaml: -------------------------------------------------------------------------------- 1 | shutdown_costmaps: false 2 | controller_frequency: 10.0 3 | planner_patience: 5.0 4 | controller_patience: 15.0 5 | conservative_reset_dist: 3.0 6 | planner_frequency: 5.0 7 | oscillation_timeout: 10.0 8 | oscillation_distance: 0.2 9 | 10 | -------------------------------------------------------------------------------- /sim_env/param_teb/teb_local_planner_params_burger.yaml: -------------------------------------------------------------------------------- 1 | TebLocalPlannerROS: 2 | 3 | odom_topic: odom 4 | #odom_topic: /robot_pose_ekf/odom_combined 5 | map_frame: robot1/odom 6 | #map_frame: /odom_combined 7 | 8 | # Trajectory 9 | 10 | teb_autosize: True 11 | dt_ref: 0.45 12 | dt_hysteresis: 0.1 13 | global_plan_overwrite_orientation: True 14 | max_global_plan_lookahead_dist: 3.0 15 | feasibility_check_no_poses: 5 16 | 17 | # Robot 18 | 19 | max_vel_x: 0.5 20 | max_vel_y: 0 #差速导航注释掉此行 21 | max_vel_x_backwards: 0.35 22 | max_vel_theta: 0.4 23 | acc_lim_x: 0.3 24 | acc_lim_y: 0 #差速导航注释掉此行 25 | acc_lim_theta: 0.50 26 | min_turning_radius: 0.0 27 | footprint_model: # types: "point", "circular", "two_circles", "line", "polygon" 28 | #radius: 0.12 # for type "circular" 29 | vertices: [[-0.12, -0.12], [-0.12, 0.12],[0.12, 0.12], [0.12, -0.12]] 30 | #vertices: [[-0.1, -0.26], [-0.1, 0.26],[0.287, 0.26], [0.287, -0.26]] 31 | 32 | # GoalTolerance 33 | 34 | xy_goal_tolerance: 0.2 35 | yaw_goal_tolerance: 0.5 36 | free_goal_vel: False 37 | 38 | # Obstacles 39 | 40 | min_obstacle_dist: 0.15 41 | include_costmap_obstacles: True 42 | costmap_obstacles_behind_robot_dist: 1.0 43 | obstacle_poses_affected: 7 44 | costmap_converter_plugin: "" 45 | costmap_converter_spin_thread: True 46 | costmap_converter_rate: 5 47 | 48 | # Optimization 49 | 50 | no_inner_iterations: 5 51 | no_outer_iterations: 4 52 | optimization_activate: True 53 | optimization_verbose: False 54 | penalty_epsilon: 0.1 55 | weight_max_vel_x: 1 56 | weight_max_vel_y: 0 #差速导航注释掉此行 57 | weight_max_vel_theta: 1 58 | weight_acc_lim_x: 1 59 | weight_acc_lim_y: 0 #差速导航注释掉此行 60 | weight_acc_lim_theta: 0.5 61 | weight_kinematics_nh: 1000 #差速导航将此值改为1000 62 | weight_kinematics_forward_drive: 60 #差速导航将此值改为60 63 | weight_kinematics_turning_radius: 1 64 | weight_optimaltime: 1 65 | weight_obstacle: 50 66 | weight_dynamic_obstacle: 10 # not in use yet 67 | selection_alternative_time_cost: False # not in use yet 68 | 69 | # Homotopy Class Planner 70 | 71 | enable_homotopy_class_planning: False 72 | enable_multithreading: True 73 | simple_exploration: False 74 | max_number_classes: 4 75 | roadmap_graph_no_samples: 15 76 | roadmap_graph_area_width: 5 77 | h_signature_prescaler: 0.5 78 | h_signature_threshold: 0.1 79 | obstacle_keypoint_offset: 0.1 80 | obstacle_heading_threshold: 0.45 81 | visualize_hc_graph: False 82 | -------------------------------------------------------------------------------- /sim_env/script/ImuChange.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ odom_ekf.py - Version 0.1 2012-07-08 4 | Republish the /robot_pose_ekf/odom_combined topic which is of type 5 | geometry_msgs/PoseWithCovarianceStamped as an equivalent message of 6 | type nav_msgs/Odometry so we can view it in RViz. 7 | Created for the Pi Robot Project: http://www.pirobot.org 8 | Copyright (c) 2012 Patrick Goebel. All rights reserved. 9 | This program is free software; you can redistribute it and/or modify 10 | it under the terms of the GNU General Public License as published by 11 | the Free Software Foundation; either version 2 of the License, or 12 | (at your option) any later version.5 13 | 14 | This program is distributed in the hope that it will be useful, 15 | but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | GNU General Public License for more details at: 18 | 19 | http://www.gnu.org/licenses/gpl.html 20 | 21 | """ 22 | 23 | import roslib 24 | #roslib.load_manifest('rbx1_nav') 25 | import rospy 26 | import getopt 27 | import sys 28 | from sensor_msgs.msg import Imu 29 | 30 | class ImuChange(): 31 | def __init__(self, frame_id = 'base_footprint'): 32 | # Give the node a name 33 | rospy.init_node('ImuChangeHeader', anonymous=False) 34 | self.frame_id = frame_id 35 | 36 | # Publisher of type nav_msgs/Odometry 37 | self.Imu_pub = rospy.Publisher('output', Imu,queue_size=10) 38 | 39 | # Wait for the /odom_combined topic to become available 40 | rospy.wait_for_message('input', Imu) 41 | 42 | # Subscribe to the /odom_combined topic 43 | rospy.Subscriber('input', Imu, self.pub_Imu) 44 | 45 | rospy.loginfo("Publishing combined odometry on /ImuChange") 46 | 47 | def pub_Imu(self, msg): 48 | imu_data = msg 49 | imu_data.header.frame_id = self.frame_id 50 | 51 | self.Imu_pub.publish(imu_data) 52 | 53 | if __name__ == '__main__': 54 | # try: 55 | opts, args = getopt.getopt(sys.argv[1:],"c:") 56 | for opt, arg in opts: 57 | if opt in ("-c"): 58 | self_frame_id = str(arg) 59 | ImuChange(self_frame_id) 60 | rospy.spin() 61 | # except: 62 | # pass 63 | 64 | -------------------------------------------------------------------------------- /sim_env/script/publish_tf_for_two_robots.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | import sys 3 | import time 4 | import os 5 | import numpy as np 6 | import rospy 7 | from std_msgs.msg import String, Float32MultiArray 8 | from nav_msgs.msg import OccupancyGrid 9 | from geometry_msgs.msg import Pose, PoseStamped, Transform, TransformStamped, Quaternion 10 | import tf 11 | import pickle 12 | from autolab_core import RigidTransform 13 | 14 | def trans2pose(trans): 15 | pose = Pose() 16 | pose.orientation = trans.rotation 17 | pose.position.x = trans.translation.x 18 | pose.position.y = trans.translation.y 19 | pose.position.z = trans.translation.z 20 | return pose 21 | 22 | def pose2trans(pose): 23 | trans = Transform() 24 | trans.rotation = pose.orientation 25 | trans.translation =pose.position 26 | return trans 27 | 28 | def trans2Rigidtrans(trans, from_frame, to_frame): 29 | rotation_quaternion = np.asarray([trans.rotation.w, trans.rotation.x, trans.rotation.y, trans.rotation.z]) 30 | T_trans = np.asarray([trans.translation.x, trans.translation.y, trans.translation.z]) 31 | T_qua2rota = RigidTransform(rotation_quaternion, T_trans, from_frame = from_frame, to_frame=to_frame) 32 | return T_qua2rota 33 | 34 | def pose2Rigidtrans(pose, from_frame, to_frame): 35 | trans = pose2trans(pose) 36 | T_qua2rota = trans2Rigidtrans(trans, from_frame, to_frame) 37 | return T_qua2rota 38 | 39 | def transstamp2Rigidtrans(trans): 40 | from_frame = trans.child_frame_id 41 | # to_frame = trans.child_frame_id 42 | to_frame = trans.header.frame_id 43 | T_qua2rota = trans2Rigidtrans(trans.transform, from_frame, to_frame) 44 | return T_qua2rota 45 | 46 | def Rigidtrans2transstamped(Rigidtrans): 47 | trans = TransformStamped() 48 | trans.header.stamp = rospy.Time.now() 49 | trans.header.frame_id = Rigidtrans.to_frame 50 | trans.child_frame_id = Rigidtrans.from_frame 51 | trans.transform = pose2trans(Rigidtrans.pose_msg) 52 | return trans 53 | 54 | def tf2Rigidtrans(trans, from_frame, to_frame): 55 | rotation_quaternion = np.asarray([trans[1][3], trans[1][0], trans[1][1], trans[1][2]]) 56 | T_trans = np.asarray([trans[0][0], trans[0][1], trans[0][2]]) 57 | T_qua2rota = RigidTransform(rotation_quaternion, T_trans, from_frame = from_frame, to_frame=to_frame) 58 | return T_qua2rota 59 | 60 | if __name__ == '__main__': 61 | rospy.init_node('test_publish_tf', anonymous=True) 62 | robot1_pub = rospy.Publisher('robot1/robot_pos', Float32MultiArray, queue_size=10) 63 | robot2_pub = rospy.Publisher('robot2/robot_pos', Float32MultiArray, queue_size=10) 64 | br = tf.TransformBroadcaster() 65 | tf_listener = tf.TransformListener() 66 | rate = rospy.Rate(10) 67 | # from, source: child_id to, target: frame_id 68 | while not rospy.is_shutdown(): 69 | try: 70 | #now = rospy.Time.now() 71 | #tf_listener.waitForTransform('robot1/map','robot1/odom', rospy.Time.now(), rospy.Duration(10.0)) 72 | robot1_map_to_odom = tf_listener.lookupTransform('robot1/map', 'robot1/odom', rospy.Time(0)) 73 | robot2_map_to_odom = tf_listener.lookupTransform('robot2/map', 'robot2/odom', rospy.Time(0)) 74 | except: 75 | print('no tf') 76 | rate.sleep() 77 | continue 78 | robot1_map_to_odom_Rigidtrans = tf2Rigidtrans(robot1_map_to_odom, 'robot1/odom', 'robot1/map') 79 | robot2_map_to_odom_Rigidtrans = tf2Rigidtrans(robot2_map_to_odom, 'robot2/odom', 'robot2/map') 80 | robot1_odom_to_robot2_odom_Rigidtrans = tf2Rigidtrans(([0,0,0],[0,0,0,1]), 'robot2/odom', 'robot1/odom') 81 | robot1_map_to_robot2_map_Rigidtrans = robot1_map_to_odom_Rigidtrans*robot1_odom_to_robot2_odom_Rigidtrans*robot2_map_to_odom_Rigidtrans.inverse() 82 | 83 | tf = Rigidtrans2transstamped(robot1_map_to_robot2_map_Rigidtrans) 84 | # print("publish tf: " + tf.header.frame_id + " to " + tf.child_frame_id) 85 | br.sendTransformMessage(tf) 86 | # send robot pos 87 | try: 88 | #now = rospy.Time.now() 89 | #tf_listener.waitForTransform('robot1/map','robot1/odom', rospy.Time.now(), rospy.Duration(10.0)) 90 | robot1_map_to_base = tf_listener.lookupTransform('robot1/map', 'robot1/base_footprint', rospy.Time(0)) 91 | robot2_map_to_base = tf_listener.lookupTransform('robot2/map', 'robot2/base_footprint', rospy.Time(0)) 92 | except: 93 | print('no tf') 94 | rate.sleep() 95 | continue 96 | robot1_pos_msg = Float32MultiArray() 97 | robot1_pos_msg.data.append(robot1_map_to_base[0][0]) 98 | robot1_pos_msg.data.append(robot1_map_to_base[0][1]) 99 | robot2_pos_msg = Float32MultiArray() 100 | robot2_pos_msg.data.append(robot2_map_to_base[0][0]) 101 | robot2_pos_msg.data.append(robot2_map_to_base[0][1]) 102 | robot1_pub.publish(robot1_pos_msg) 103 | robot2_pub.publish(robot2_pos_msg) 104 | time.sleep(1) --------------------------------------------------------------------------------