Arena2d implementation

.rosinstall

@@ -24,6 +24,11 @@
     local-name: ../utils/jackal
     uri: https://github.com/jackal/jackal.git
 
+- git:
+    local-name: ../utils/arena2d
+    uri: https://github.com/zenghjian/arena2D.git
+    version: integrate
+
 ### ARENA INFRASTRUCTURE
 
 - git:
@@ -41,15 +46,10 @@
     uri: https://github.com/Arena-Rosnav/arena-utils.git
     version: v1.0.0
 
-- git:
-    local-name: ../utils/waypoint-generators
-    uri: https://github.com/Arena-Rosnav/waypoint-generators.git
-    version: v1.0.0
-
 - git:
     local-name: ../utils/task-generator
-    uri: https://github.com/Arena-Rosnav/task-generator.git
-    version: master
+    uri: https://github.com/zenghjian/task-generator.git
+    version: dev
 
 ### PLANNERS
 

README.md

@@ -1,9 +1,35 @@
 ## Simple Installation
 
+Follow [this](https://arena-rosnav-wiki.readthedocs.io/en/latest/user_guides/installation/) until step.7
+
+Install Conda and Poetry 
+```
+// conda
+wget https://repo.anaconda.com/miniconda/Miniconda3-latest-Linux-x86_64.sh 
+bash Miniconda3-latest-Linux-x86_64.sh
+source ~/.bashrc
+conda config --set auto_activate_base false
+conda create -n rosnav python=3.8
+
+// poetry
+curl -sSL https://install.python-poetry.org | python3 -
+echo 'export PATH="$HOME/.local/bin:$PATH"'>> ~/.bashrc
+source ~/.bashrc
+poetry config virtualenvs.create false
+
+//activate conda 
+conda activate rosnav
+```
+
 Clone repo in any catkin ws or create new catkin ws
 
 ```
-git clone [email protected]:Arena-Rosnav/arena-rosnav.git
+mkdir -p ~/catkin_ws/src
+cd ~/catkin_ws/src
+```
+
+```
+git clone https://github.com/zenghjian/arena-rosnav
 ```
 
 Change into dir
@@ -27,17 +53,55 @@ poetry shell && poetry install
 Install stable baselines
 
 ```
-cd ../forks/stable-baselines3 && pip install -e .
+cd ../utils/stable-baselines3 && pip install -e .
+```
+
+Install arena2d requirements
+```
+sudo apt-get install cmake libsdl2-dev libfreetype-dev
 ```
 
 Build catkin
 
 ```
-cd ../../.. && catkin_make
+cd ../../.. && catkin_make -DUSE_ROS=ON
 ```
 
+
 Finished!
 
+## Run Simulator
+
+Open Terminal 1 (Run roslaunch)
+```
+cd ~/catkin_ws
+source devel/setup.bash
+roslaunch arena_bringup start_arena.launch
+// roslaunch arena_bringup arena2d_simulator.launch 
+// only for arena2d
+```
+
+Open Terminal 2 (Run Script)
+```
+cd ~/catkin_ws
+source devel/setup.bash 
+cd src/arena-rosnav/
+conda activate rosnav
+poetry shell
+cd ../utils/arena2d/rl-ros-agents/
+python scripts/training/train_ppo.py
+```
+
 #### TODO
 
 Everything
+
+## Tipps for GPU 30X: m_86 is not compatible with the current PyTorch
+```
+conda rosnav
+poetry shell
+pip uninstall torch
+pip list | grep torch
+conda list | grep torch
+pip3 install torch torchvision torchaudio --extra-index-url https://download.pytorch.org/whl/cu113
+```

arena_bringup/launch/start_arena_flatland.launch

@@ -14,14 +14,18 @@
   <arg name="auto_reset" default="true"/>
 
   <arg name="scenario_file" default="test_scenario.json" />
-  <arg name="map_file" default="ignc" doc="[map1, floor, indoor, indoor_map1, map_small, map_empty]"/>
+  <arg name="map_file" default="map_empty" doc="[map1, floor, indoor, indoor_map1, map_small, map_empty]"/>
   <arg name="map_path" default="$(find arena-simulation-setup)/maps/$(arg map_file)/map.yaml" />
 
+  <arg name="simulator" default="arena2d" doc="[flatland, arena2d]" />
+  <arg name="num_envs" default="4" />
   <!-- ___________ PARAMS ___________ -->
-  <param name="use_sim_time" value="true"/>
-
+  <group if="$(eval arg('simulator') == 'flatland')">
+    <param name="use_sim_time" value="true"/>
+  </group>
+
   <param name="model" value="$(arg model)" />
-
+  <param name="simulator" value="$(arg simulator)" />
   <param name="task_mode" value="$(arg task_mode)" />
   <param name="map_path" value="$(arg map_path)" />
   <param name="train_mode" value="false" />
@@ -55,11 +59,21 @@
   <param name="scenario_file"  value="$(arg scenario_file)"/>
 
   <!-- use simulator-->
-  <include file="$(find arena_bringup)/launch/testing/flatland_simulator.launch">
-    <arg name="visualization" default="$(arg visualization)" />
-    <arg name="rviz_file" value="$(arg rviz_file)" />
-    <arg name="model" value="$(arg model)" />
-  </include>
+
+  <group if="$(eval arg('simulator') == 'flatland')">
+    <include file="$(find arena_bringup)/launch/testing/flatland_simulator.launch">
+      <arg name="visualization" default="$(arg visualization)" />
+      <arg name="rviz_file" value="$(arg rviz_file)" />
+      <arg name="model" value="$(arg model)" />
+    </include>
+  </group>
+
+  <group if="$(eval arg('simulator') == 'arena2d')">
+    <include file="$(find arena_bringup)/launch/training/arena2d_simulator.launch">
+      <arg name="robot_model" value="$(arg model)" />
+      <arg name="num_envs" value="$(arg num_envs)" />
+    </include>
+  </group>
 
   <!-- map server-->
   <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_path)">
@@ -77,20 +91,21 @@
     <arg name="auto_reset" value="$(arg auto_reset)"/>
   </include>
 
-  <include file="$(find arena_bringup)/launch/testing/sensor_sim.launch" />
+  <!-- <include file="$(find arena_bringup)/launch/testing/sensor_sim.launch" /> -->
 
   <!-- move_base plan manager: which provide basic global planner and cost map -->
-  <include file="$(find arena_bringup)/launch/testing/move_base/move_base_$(arg local_planner).launch">
-    <arg name="model" value="$(arg model)"/>
-    <arg name="agent_name" value="$(arg agent_name)" if="$(eval arg('local_planner') == 'rosnav')" />
-  </include>
-
-  <node name="pedsim_simulator" pkg="pedsim_simulator" type="pedsim_simulator" output="screen">
-    <param name="simulation_factor" value="1" type="double"/>
-    <param name="pedsim_update_rate" value="30.0" type="double"/>
-    <param name="scene_file" value="$(find task-generator)/scenarios/empty.xml" type="string"/>
-  </node>
-
+  <group if="$(eval arg('simulator') == 'flatland')">
+    <include file="$(find arena_bringup)/launch/testing/move_base/move_base_$(arg local_planner).launch">
+      <arg name="model" value="$(arg model)"/>
+      <arg name="agent_name" value="$(arg agent_name)" if="$(eval arg('local_planner') == 'rosnav')" />
+    </include>
+
+    <node name="pedsim_simulator" pkg="pedsim_simulator" type="pedsim_simulator" output="screen">
+      <param name="simulation_factor" value="1" type="double"/>
+      <param name="pedsim_update_rate" value="30.0" type="double"/>
+      <param name="scene_file" value="$(find task-generator)/scenarios/empty.xml" type="string"/>
+    </node>
+  </group>
   <!-- launch map generator if training with random map-->
   <group if="$(eval arg('map_file') == 'random_map')">
     <node pkg="arena-simulation-setup" name="map_generator_node" type="map_generator_node.py" />

arena_bringup/launch/start_training.launch

@@ -1,10 +1,13 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
+
+
+  <arg name="simulator" default="arena2d" doc="[flatland, arena2d]" />
   <arg name="model" default="burger"      doc="model type [burger, jackal, ridgeback, agvota, rto, rto_real]"/>
 
   <!-- Here are the argument that may be frequently changed -->
   <arg name="ns_prefix" default="sim" />
-  <arg name="num_envs" default="1" />
+  <arg name="num_envs" default="4" />
 
   <param name="single_env" value="false" />
 
@@ -28,7 +31,9 @@
   <rosparam command="load" file="$(find arena_bringup)/params/drl.yaml" />
 
   <!--  PARAMS DON'T CHANGE THEM  -->
-  <param name="use_sim_time" value="true" />
+  <group if="$(eval arg('simulator') == 'flatland')">
+    <param name="use_sim_time" value="true"/>
+  </group>
   <param name="train_mode" value="$(arg train_mode)" />
   <param name="num_envs" value="$(arg num_envs)" />
   <arg name="map_path" default="$(find arena-simulation-setup)/maps/$(arg map_folder_name)/map.yaml" />
@@ -50,13 +55,22 @@
 
   <!-- set the log format -->
   <env name="ROSCONSOLE_FORMAT" value="[${severity} ${time} ${logger}]: ${message}" />
+
+  <group if="$(eval arg('simulator') == 'flatland')">
   <include file="$(find arena_bringup)/launch/training/single_env_training.launch">
     <arg name="ns" value="eval_sim" />
-  </include>
-
+  </include>  
   <include file="$(find arena_bringup)/launch/training/start_envs.launch">
     <arg name="num_envs" value="$(arg num_envs)" />
     <arg name="ns_prefix" value="$(arg ns_prefix)" />
   </include> 
+  </group>
 
+    <group if="$(eval arg('simulator') == 'arena2d')">
+    <include file="$(find arena_bringup)/launch/training/arena2d_simulator.launch">
+      <arg name="robot_model" value="$(arg model)" />
+      <arg name="num_envs" value="$(arg num_envs)" />
+      <arg name="staged_mode" value="true" />
+    </include>
+  </group>
 </launch>

arena_bringup/launch/training/arena2d_simulator.launch

@@ -0,0 +1,86 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+
+    <!-- arguments for simulator setting -->
+    <arg name="level_name" default="scenario" doc = "level name [empty, random, svg, scenario]"/>
+    <param name="level" value="$(arg level_name)"/>
+    <arg name="level_mode" default="--dynamic" doc = "[--dynamic --human, --dynamic, --human,  ]"/>    
+    <arg name="video_mode" default="--enable-video" doc = "video [--disable-video, --enable-video]"/> 
+    <arg name="num_envs" default="1" doc = "number of environment"/>
+    <!-- robot setting -->
+    <arg name="robot_model" default="burger" 
+    doc="model type [burger, jackal, agvota, waffle_pi]"/>
+    <param name="model" value="$(arg robot_model)" />
+
+    <arg name="robot_mode" default="continuous"      doc="mode type [continuous, discrete]"/>
+    <param name="mode" value="$(arg robot_mode)" />
+
+    <rosparam command="load" 
+    file = "$(find arena2d)/configs/robot/$(arg robot_model)/model_params.yaml" />
+
+    <!-- <rosparam command="load" 
+    file = "$(find arena2d)/configs/observation_space/$(arg robot_model).model.yaml" />
+
+    <rosparam command="load" 
+    file= "$(find arena2d)/configs/action_space/default_settings_$(arg robot_model).yaml" /> -->
+
+    <!-- staged task -->
+    <!-- load training curriculum for stage mode + set stage mode -->
+    <arg name="staged_mode" default="false" doc = "[true, false]"/> 
+    <param name="stage/stage" value="$(arg staged_mode)" />
+    <group if="$(eval arg('level_name') == 'scenario')">
+        <group if="$(eval arg('staged_mode') == true)">
+        <arg name="curriculum_file" default="normal" doc="simple, normal, hard, dynamic]"/>
+        <arg name="curriculum_path" default="$(find arena2d)/configs/curriculum/training_curriculum_$(arg curriculum_file).yaml" />    
+            <rosparam command="load" file = "$(arg curriculum_path)" />
+        </group>
+    </group>
+
+    <!-- scenerio task-->
+    <!-- this part only used for setting goal and robot pos manually-->
+
+    <!-- The value needs to be set carefully, 
+    1.) range = [- (width * resolution / 2) , width * resolution / 2]
+    2.) make sure the spawn point without obstacle -->
+    <param name="scenerio/scenerio" value="false" />
+    <!-- <group if="$(eval arg ('level_name') == 'scenario')">
+        <param name="scenerio/scenerio" value="true" />    
+        <param name="scenerio/goal_x" value = "-3.0" />
+        <param name="scenerio/goal_y" value = "4.0" />
+        <param name="scenerio/robot_x" value = "2.0" />
+        <param name="scenerio/robot_y" value = "4.0" />        
+    </group> -->
+
+
+    <!-- Run the map server -->
+    <!-- <arg name="map_file" default="map_empty" doc="[book, ignc, indoor, labor, obs, map1_small, map_empty, map_small]"/>
+    <arg name="map_path" default="$(find arena2d)/scenario/$(arg map_file)/map.yaml" />    
+    <node if="$(eval arg('level_name') == 'scenario')"
+    name="map_server" pkg="map_server" type="map_server" args="$(arg map_path)" /> -->
+
+
+
+
+    <!-- load rviz helper, transfer data to rviz -->    
+    <!-- <node name="rviz_visualize_helper" 
+          pkg="rl-ros-agents" 
+          type="rviz_visualize_helper.py" 
+          output="screen" /> -->
+
+    <!-- load rviz configuration -->
+    <!-- <node if="$(eval arg('level_name') == 'scenario')"
+    name ="rviz" pkg="rviz" type="rviz" 
+    args="-d $(find rl-ros-agents)/launch/rviz_config_new.rviz" required="true" /> -->
+
+    <!-- launch arena2d server -->
+    <env name="ROSCONSOLE_CONFIG_FILE" value="$(find arena2d)/ros_logging_settings.conf" />
+
+    <node name="arena_sim" pkg="arena2d" type="arena2d" output="screen" 
+    args="--use_ros_agent $(arg video_mode) --num_envs $(arg num_envs) --robot $(arg robot_model) --run &quot;level $(arg level_name) $(arg level_mode) &quot;"/>
+
+
+
+
+
+
+</launch>

training/configs/training_curriculums/normal.yaml

@@ -0,0 +1,29 @@
+# Training Curriculum
+# this yaml serves as container of the learning curriculum for the agent
+# by determining the number of static/dynamic obstacles per stage
+# (will carry on training on last stage when threshhold reached)
+
+1:
+  static: 0
+  dynamic: 0
+2:
+  static: 3
+  dynamic: 0
+3:
+  static: 5
+  dynamic: 0
+4:
+  static: 6
+  dynamic: 1
+5:
+  static: 7
+  dynamic: 2
+6:
+  static: 9
+  dynamic: 2
+7:
+  static: 10
+  dynamic: 2
+8:
+  static: 12
+  dynamic: 3

training/scripts/train_agent.py

@@ -120,7 +120,7 @@ def main():
         model = PPO(
             "MlpPolicy",
             env,
-            policy_kwargs=dict(net_arch=args.net_arch, activation_fn=get_act_fn(args.act_fn)),
+            # policy_kwargs=dict(net_arch=args.net_arch, activation_fn=get_act_fn(args.act_fn)),
             gamma=params["gamma"],
             n_steps=params["n_steps"],
             ent_coef=params["ent_coef"],