Merge branch 'unity-simulator' of https://github.com/TheZomb/arena-ro…

…snav into unity-simulator

.rosinstall

@@ -75,8 +75,8 @@
   # Change back to Arena-Rosnav once the PR is merged
 - git:
     local-name: ../planners/rosnav
-    uri: https://github.com/tuananhroman/rosnav-rl.git
-    version: master
+    uri: https://github.com/Arena-Rosnav/rosnav-rl.git
+    version: dev
 
 - git:
     local-name: ../planners/arena-ros

arena_bringup/configs/training/training_config.yaml

@@ -60,7 +60,7 @@ callbacks:
 ### Agent Specs: Training Hyperparameter and Network Architecture
 rl_agent:
   # name of architecture defined in the Policy factory
-  architecture_name: "AGENT_54"
+  architecture_name: "AGENT_57"
   # path to latest checkpoint; if provided the training will be resumed from that checkpoint
   resume: null # "jackal_AGENT_25_RobotSpecificEncoder_2023_01_21__03_01"
 
@@ -82,11 +82,11 @@ rl_agent:
   laser:
     full_range_laser: true  # additional laser covering 360° covering blind spots -> additional collision check
     reduce_num_beams:
-      enabled: true
+      enabled: false
       num_beams: 200
 
   action_space:
-    discrete: false
+    discrete: true
     custom_discretization: # only used if discrete is true, otherwise ignored
       enabled: true
       # number of buckets for each action dimension

arena_bringup/launch/start_arena.launch

@@ -140,6 +140,7 @@
   <include file="$(find pedsim_ros)/launch/pedsim.launch" if="$(eval arg('pedsim'))">
     <arg name="sfm" value="$(arg sfm)"/>
     <arg name="scene_file" value="$(find arena-simulation-setup)/worlds/$(arg world_file)/worlds/$(arg world_file).world" />
+    <arg name="visualize" value="true" />
   </include>
 
   <node type="create_config_file.py" pkg="rviz_utils" name="rviz_config_file_creator" />

arena_bringup/launch/start_training.launch

@@ -1,5 +1,10 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
+  <!-- CONSTANTS -->
+  <param name="SIMULATOR" value="flatland" />
+  <param name="local_planner" value="rosnav" />
+
+
   <arg name="model" default="burger"      doc="model type [burger, jackal, ridgeback, agvota, rto, rto_real]"/>
 
   <!-- Here are the argument that may be frequently changed -->
@@ -21,11 +26,8 @@
 
   <!-- if it's false flatland server will not publish any debug related infomation and you will not able to visuzalize e.g robot, dynamic obstacles  -->
   <!-- <param name="space_encoder" value="UniformEncoder" /> -->
-
+  <rosparam file="$(find arena_bringup)/configs/task_generator.yaml" command="load" ns="configuration"/>
   <rosparam command="load" file="$(find arena-simulation-setup)/robot/$(arg model)/model_params.yaml" /> 
-  <param name="robot_yaml_path" value="$(find arena-simulation-setup)/robot/$(arg model)/$(arg model).model.yaml" />
-  <param name="tmp_model_path" value="/tmp" />
-
   <rosparam command="load" file="$(find arena_bringup)/params/flatland.yaml" />
   <rosparam command="load" file="$(find arena_bringup)/params/drl.yaml" />
   <rosparam command="load" file="$(find arena_bringup)/params/map_generator.yaml" />
@@ -57,6 +59,7 @@
   <env name="ROSCONSOLE_FORMAT" value="[${severity} ${time} ${logger}]: ${message}" />
   <include file="$(find arena_bringup)/launch/training/single_env_training.launch">
     <arg name="ns" value="eval_sim" />
+    <arg name="model" value="$(arg model)" />
   </include>
 
   <include file="$(find arena_bringup)/launch/training/start_envs.launch">

arena_bringup/launch/testing/move_base/bootstrap.launch

@@ -10,7 +10,9 @@
     <arg name="namespace" />
     <arg name="frame" default="" />
 
+    <arg name="train_mode" if="$(eval arg('local_planner') == 'rosnav')" />
     <arg name="agent_name" if="$(eval arg('local_planner') == 'rosnav')" />
+    <arg name="sim_namespace" default="" if="$(eval arg('local_planner') == 'rosnav')"/>
 
     <include file="$(find arena_bringup)/launch/testing/move_base/mbf_$(arg local_planner).launch">
 
@@ -19,10 +21,13 @@
 
         <arg name="agent_name" value="$(arg agent_name)"
             if="$(eval arg('local_planner') == 'rosnav')" />
+        <arg name="train_mode" value="$(arg train_mode)"
+            if="$(eval arg('local_planner') == 'rosnav')" />
+        <arg name="sim_namespace" value="$(arg sim_namespace)"
+            if="$(eval arg('local_planner') == 'rosnav')" />
 
         <arg name="namespace" value="$(arg namespace)" />
         <arg name="frame" value="$(arg frame)" />
-
     </include>
 
 </launch>

arena_bringup/launch/testing/move_base/mbf_nav/costmap_nav.launch

@@ -6,7 +6,7 @@
     <arg name="speed" />
 
     <!-- move_base -->
-    <node pkg="mbf_costmap_nav" type="mbf_costmap_nav" respawn="false" name="move_base_flex" output="log">
+    <node pkg="mbf_costmap_nav" type="mbf_costmap_nav" respawn="false" name="move_base_flex" output="screen">
         <!-- system defaults -->
         <rosparam file="$(find arena_bringup)/params/mbf/move_base_params.yaml"
             command="load" subst_value="True" />

arena_bringup/launch/testing/move_base/mbf_rosnav.launch

@@ -5,8 +5,9 @@
   <arg name="model" default="burger"/>
   <arg name="speed" />
   <arg name="namespace" />
+  <arg name="sim_namespace" default=""/>
   <arg name="frame" />
-
+  <arg name="train_mode" default="false"/>
   <arg name="agent_name" default="rule_05"/>
 
 
@@ -16,16 +17,17 @@
   <rosparam command="load" file="$(find arena_bringup)/params/drl.yaml" />
 
   <!-- <rosparam file="$(find arena_bringup)/launch/plan_fsm_param.yaml" command="load" /> -->
-
-  <node name="rosnav_manager" pkg="testing" type="drl_agent_node.py" args="--namespace $(arg namespace)" output="screen"/>
-
-  <node 
-    name="rosnav" 
-    pkg="rosnav" 
-    type="rosnav_node.py" 
-    args="--namespace $(arg namespace)"
-    output="screen" 
-  />
+  <group if="$(eval arg('train_mode') == false)">
+    <node name="rosnav_manager" pkg="testing" type="drl_agent_node.py" args="--namespace $(arg namespace)" output="screen"/>
+
+    <node 
+      name="rosnav" 
+      pkg="rosnav" 
+      type="rosnav_node.py" 
+      args="--namespace $(arg namespace)"
+      output="screen" 
+    />
+  </group>
 
   <env name="ROSCONSOLE_CONFIG_FILE" value="$(find arena_bringup)/params/rosconsole.conf"/>
 
@@ -48,6 +50,8 @@
   <!-- move_base -->
   <remap from="move_base_flex/TebLocalPlannerROS/global_plan" to="/devnull" /> <!-- optics -->
   <remap from="cmd_vel" to="/devnulll" /> <!-- optics -->
+  <remap from="/clock" to="/$(arg sim_namespace)/clock"/>
+
   <include file="$(find arena_bringup)/launch/testing/move_base/mbf_nav/costmap_nav.launch">
     <arg name="model" value="$(arg model)" />
     <arg name="speed" value="$(arg speed)" />

arena_bringup/launch/testing/robot.launch

@@ -3,13 +3,17 @@
   <!-- <env name="ROSCONSOLE_CONFIG_FILE" value="$(find arena_bringup)/params/rosconsole.conf"/> -->
 
   <!-- ___________ ARGS ___________ -->
+  <arg name="SIMULATOR" default=""/>
   <arg name="local_planner" default="" doc="local planner type [teb, dwa, mpc, rlca, arena, rosnav]" />
 
   <arg name="name" default="" />
   <arg name="model" default="" />
   <arg name="namespace" default="" />
+  <arg name="sim_namespace" default="" />
   <arg name="frame" default="" /> <!-- frame name -->
 
+  <arg name="train_mode" default="false" doc="If false, start the Rosnav Deployment Nodes"
+    unless="$(eval arg('local_planner') != 'rosnav')" />
   <arg name="agent_name" default="$(arg name)" doc="DRL agent name to be deployed"
     unless="$(eval arg('local_planner') != 'rosnav')" />
 
@@ -18,7 +22,10 @@
   <!-- TODO: Add data recorder -->
   <arg name="record_data" default="false" />
 
+  <param name="SIMULATOR" value="$(arg SIMULATOR)" />
   <param name="model" value="$(arg model)" />
+  <param name="train_mode" value="$(arg train_mode)"
+    unless="$(eval arg('local_planner') != 'rosnav')" />
   <param name="agent_name" value="$(arg agent_name)"
     unless="$(eval arg('local_planner') != 'rosnav')" />
 
@@ -46,7 +53,7 @@
     <node pkg="tf2_ros" type="static_transform_publisher" name="transform_broadcaster"
       args="0 0 0 0 0 0 1 map $(arg name)/odom" />
 
-    <group if="$(eval env('SIMULATOR') == 'gazebo')">
+    <group if="$(eval arg('SIMULATOR') == 'gazebo')">
       <include file="$(find arena-simulation-setup)/robot/$(arg model)/launch/control.launch">
         <arg name="robot_namespace" value="$(arg namespace)" />
         <arg name="frame" value="$(arg frame)" />
@@ -69,11 +76,12 @@
       <remap from="odom" to="$(arg namespace)/odom" />
 
       <include file="$(find arena_bringup)/launch/testing/move_base/bootstrap.launch">
-
         <arg name="local_planner" value="$(arg local_planner)" />
 
         <arg name="model" value="$(arg model)" />
+        <arg name="train_mode" value="$(arg train_mode)" if="$(eval arg('local_planner') == 'rosnav')" />
         <arg name="agent_name" value="$(arg agent_name)" if="$(eval arg('local_planner') == 'rosnav')" />
+        <arg name="sim_namespace" value="$(arg sim_namespace)" if="$(eval arg('local_planner') == 'rosnav')" />
 
         <arg name="namespace" value="$(arg namespace)" />
         <arg name="frame" value="$(arg name)/" />

arena_bringup/launch/training/flatland_simulator.launch

@@ -4,13 +4,14 @@
 
   <rosparam command="load" file="$(find arena_bringup)/params/flatland.yaml" />
 
-  <node pkg="tf2_ros" type="static_transform_publisher" name="transform_broadcaster" args="0 0 0 0 0 0 1 map $(arg ns)_odom" />
-
   <!-- launch flatland server -->
-  <node name="$(anon flatland_server)" pkg="flatland_server" type="flatland_server" output="screen">
+  <node name="flatland_server" pkg="flatland_server" type="flatland_server" output="screen">
     <!-- remapping for namespace -->
     <remap from="/clock" to="/$(arg ns)/clock"/>
     <remap from="/step_world" to="/$(arg ns)/step_world" />
     <!-- <remap from="/move_model" to="/$(arg ns)/move_model" /> -->
   </node>
+  <include file="$(find arena_bringup)/launch/utils/rviz.launch">
+    <arg name="show_rviz" value="false" />
+  </include>
 </launch>

arena_bringup/launch/training/single_env_training.launch

@@ -5,44 +5,20 @@
   <arg name="entity_manager" default="pedsim"/>
   <arg name="world_file" default=""/>
 
+  <!-- TODO: make non-constant -->
+  <arg name="sfm" value="passthrough" />
+
   <!-- use fake localization-->
   <group ns="$(arg ns)" >
-    <include file="$(find arena_bringup)/launch/utils/fake_localization.launch">
-      <arg name="ns" value="$(arg ns)" />
-      <arg name="robot_name" value="$(arg model)" />
-
-      <!-- ADJUST ODOM FRAME IN ROBOT MANAGER-->
-      <arg name="odom_frame_id" value="$(arg ns)/$(arg model)/odom" />
-
-      <arg name="global_frame_id" value="map" />
-    </include>
-
-    <include file="$(find arena_bringup)/launch/utils/intermediate_planner.launch">
-      <arg name="ns" value="$(arg ns)" />
-      <arg name="robot_name" value="$(arg model)" />
-    </include>
-
-    <include file="$(find arena_bringup)/launch/utils/plan_manager.launch">
-      <arg name="ns" value="$(arg ns)" />
-      <arg name="robot_name" value="$(arg model)" />
-      <arg name="train_mode" value="true" />
-
-      <arg name="look_ahead_distance" default="2.3" /> <!--meter, double -->
-      <arg name="tolerance_approach" default="0.6" /> <!--meter, double -->
-      <arg name="timeout_goal" default="330." /> <!--sec, double -->
-      <arg name="timeout_subgoal" default="20" /> <!--sec, double -->
-    </include>
-
     <!-- use simulator and rviz-->
     <include file="$(find arena_bringup)/launch/training/flatland_simulator.launch">
       <arg name="ns" value="$(arg ns)" />
     </include>
 
-    <node name="pedsim_simulator" pkg="pedsim_simulator" type="pedsim_simulator" output="screen" if="$(eval arg('entity_manager') == 'pedsim')">
-        <param name="simulation_factor" value="1" type="double"/>
-        <param name="pedsim_update_rate" value="30.0" type="double"/>
-        <param name="scene_file" value="$(find arena-simulation-setup)/worlds/$(arg world_file)/worlds/$(arg world_file).world" type="string"/>
-      </node>
+    <remap from="/clock" to="/$(arg ns)/clock"/>
+    <include file="$(find pedsim_ros)/launch/pedsim.launch" if="$(eval arg('entity_manager')=='pedsim')">
+      <arg name="scene_file" value="$(find arena-simulation-setup)/worlds/$(arg world_file)/worlds/$(arg world_file).world"/>
+    </include>
   </group>
 
 </launch>

arena_bringup/launch/training/start_envs.launch

@@ -6,6 +6,7 @@
 
   <include file="$(find arena_bringup)/launch/training/single_env_training.launch">
     <arg name="ns" value="$(arg ns_prefix)_$(arg num_envs)" />
+    <arg name="model" value="$(arg model)" />
   </include>
 
   <include file="$(find arena_bringup)/launch/training/start_envs.launch" if="$(eval arg('num_envs') > 1)">

arena_bringup/launch/utils/fake_localization.launch

@@ -8,14 +8,14 @@
 
 
   <!-- send fake static transform map_to_odom, which will be used to get localization from odom to map -->
-  <group ns="$(arg robot_name)" >
-    <node 
-      pkg="tf2_ros" 
-      type="static_transform_publisher" 
-      name="map_to_odom_tfpublisher" 
-      args="0 0 0 0 0 0 $(arg global_frame_id) $(arg odom_frame_id)">
-      <!-- <remap from="/tf_static" to="/$(arg ns)/tf_static"/> -->
-      <remap from="/clock" to="/$(arg ns)/clock" if="$(eval arg('ns') != '')" />
-    </node>
-  </group>
+  <!-- <group ns="$(arg robot_name)" > -->
+  <node 
+    pkg="tf2_ros" 
+    type="static_transform_publisher" 
+    name="map_to_odom_tfpublisher" 
+    args="0 0 0 0 0 0 $(arg global_frame_id) $(arg odom_frame_id)">
+    <!-- <remap from="/tf_static" to="/$(arg ns)/tf_static"/> -->
+    <remap from="/clock" to="/$(arg ns)/clock" if="$(eval arg('ns') != '')" />
+  </node>
+  <!-- </group> -->
 </launch>