Noise and delay modul

README.md

@@ -53,7 +53,6 @@ python3-empy \
 python3-setuptools \
 ros-melodic-navigation \ 
 ros-melodic-teb-local-planner \
-ros-melodic-mpc-local-planner \
 ```
 
 #### 1.2. Prepare virtual environment & install python packages
@@ -97,11 +96,6 @@ pip install pyyaml catkin_pkg netifaces pathlib
 ```
 pip install stable-baselines3
 ```
-* Install CADRL dependencies (venv always activated!)
-```
-cd /arena-rosnav/arena_navigation/arena_local_planner/model_based/cadrl_ros
-pip install -r requirements_cadrl.txt
-```
 
 
 #### 1.3. Install arena-rosnav repo
@@ -112,8 +106,7 @@ mkdir -p catkin_ws/src && cd catkin_ws/src
 git clone https://github.com/ignc-research/arena-rosnav
 
 cd arena-rosnav && rosws update
-cd ../forks/navigation/mpc_local_planner
-rosdep install mpc_local_planner
+
 source $HOME/.zshrc
 cd ../.. 
 catkin_make -DPYTHON_EXECUTABLE=/usr/bin/python3

README_for_noise_and_delay_modules.md

@@ -0,0 +1,55 @@
+## 1.Goals
+Adding noise and latency to laser scan sensors
+
+
+## 2.Principle
+We renamed the original "scan" topic to "scan_original" topic and added a new node "scan_process " 
+to receive the information from "scan_original", and process the sensor information in the new node 
+to increase the noise and delay. Finally,the processed sensor information is broadcast with the "scan" topic.  
+So no additional files need to be changed to accommodate the noise and delay modules.
+
+
+
+##3. Mode and start-up commands for noise and delay modules
+
+###3.1 Mode for noise and delay modules
+
+#### Mode for noise module
+- The first noise mode is the gaussian noise.It is a noise that fits a Gaussian normal distribution.
+- The second noise mode is the offset noise.It simulate by Random Walk errors.
+- The third noise mode is the angle noise. It simulate angular errors, which are made up of mechanical horizontal and vertical errors.
+- The fourth noise mode is bias noise.It simulate Physical offsets.
+
+#### Mode for delay module
+- Adding fixed time delays to sensor information.
+
+###3.2 Parameters for noise module
+
+```bash
+    "max_value_of_data": Maximum value of sensor information
+    "gauss_mean": Mean value of Gaussian noise
+    "gauss_sigma": Standard deviation range of Gaussian noise
+    "gauss_size": Size of gauss error
+    "bias_noise": Size of bias error
+    "offset_noise": Mean value of random error
+    "angle_noise": Angular error in radians.(mrad)
+```
+The parameters can be modified in the noise_parameter.json file
+
+###3.3 Start-up commands for noise and delay modules
+- In one terminal, start simulation. You can specify the following parameters: 
+
+   * noise_mode:=<0,1,2,3,4>(default 0,means without noise) #Different noises can be selected individually or together.eg.noise_mode:=123 It means that noise mode 1 and 2 and 3 is added at the same time.
+   * delay:=<int> (default 0) # Delay time
+
+```bash
+roslaunch arena_bringup start_arena_flatland.launch train_mode:=false use_viz:=true local_planner:=mpc map_file:=map1 obs_vel:=0.3 noise_mode := 1 delay:= 10
+```
+
+## 4.Location of function and how to merge.
+- The functions are implemented in the realistic_modeling/scan_process folder.  
+- Changed parameters and started newly added sublaunch in start_arena_flatland.launch.  
+- Started new nodes and topics in scan_process.launch.
+- Rename the original topic name in flatland_simulator.launch.
+
+The noise and delay modules can be added to any version of the program by simply making changes to the above sections.

arena_bringup/config/config_movebase/costmap_common_params.yaml

@@ -10,4 +10,4 @@ map_type: costmap
 observation_sources: scan
 #scan: {data_type: LaserScan, topic: /scan, marking: true, clearing: true} #, sensor_frame: laser_link}
 scan: {data_type: LaserScan, topic: /scan, marking: true, clearing: true}
-#static_laser: {clearing: true, data_type: LaserScan, expected_update_rate: 0.0, marking: true, max_obstacle_height: 0.25, min_obstacle_height: -0.1, sensor_frame: static_laser_link, topic: static_laser}
+#static_laser: {clearing: true, data_type: LaserScan, expected_update_rate: 0.0, marking: true, max_obstacle_height: 0.25, min_obstacle_height: -0.1, sensor_frame: static_laser_link, topic: static_laser}

arena_bringup/launch/start_arena_flatland.launch

@@ -1,12 +1,19 @@
 <launch>
 
   <param name="use_sim_time" value="true"/>
-
+
+  <arg name="noise_mode"      default="0"/>
+  <param name="noise_mode" value="$(arg noise_mode)"/>
+
+  <arg name="delay"      default="1"/>
+  <param name="delay" value="$(arg delay)"/>
+
   <arg name="local_planner"   default="teb"/>
   <arg name="rviz_file"       default="nav"/>
   <arg name="train_mode"      default="false"/>
   <param name="train_mode"    value="$(arg train_mode)"/>
 
+
   <!-- Task generator arguments (currently "random" and "manual") -->
   <arg name="task_mode"       default="random"/>
   <param name="task_mode"     value="$(arg task_mode)"/>
@@ -32,7 +39,7 @@
   <arg name="obs_vel"       default="0.3"/>
   <param name="obs_vel"     value="$(arg obs_vel)"/>
 
-  <!-- <arg name="scan_topic" default="scan"/> default laser topic in flatland -->
+  <!--  <arg name="scan" default="scan_original"/> -->
 
  <!-- use simulator and rviz-->
   <include file="$(find arena_bringup)/launch/sublaunch/flatland_simulator.launch">
@@ -76,6 +83,9 @@
     <arg name="local_planner"     value="$(arg local_planner)"/>
   </include>
 
+  <!-- start scan_noise node -->
+  <include file="$(find arena_bringup)/launch/sublaunch/scan_process.launch">
+  </include>
 
 
 </launch>

arena_bringup/launch/sublaunch/amcl.launch

@@ -1,5 +1,5 @@
 <launch>
-    <arg name="scan_topic" default="scan"/>
+    <arg name="scan_topic" default="scan_original"/>
     <arg name="initial_pose_x" default="0.0"/>
     <arg name="initial_pose_y" default="0.0"/>
     <arg name="initial_pose_a" default="0.0"/>

arena_bringup/launch/sublaunch/flatland_simulator.launch

@@ -14,6 +14,7 @@
 
   <!-- launch flatland server -->
   <node name="flatland_server" pkg="flatland_server" type="flatland_server" output="screen">  
+    <remap from="/scan" to="/scan_original"/>
     <param name="world_path" value="$(arg world_path)" />
     <param name="update_rate" value="$(arg update_rate)" />
     <param name="step_size" value="$(arg step_size)" />
@@ -46,4 +47,4 @@
     <node name="flatland_rviz" pkg="rviz" type="rviz" output="screen" args="-d $(find arena_bringup)/rviz/$(arg rviz_file).rviz"/>
   </group>    
 
-</launch>
+</launch>

arena_bringup/launch/sublaunch/scan_process.launch

@@ -0,0 +1,9 @@
+
+<launch>
+
+    <!--  ************** task generator  ***************  -->
+    <node pkg="scan_process" name="scan_process" type="scan_process.py" output="screen"> 
+
+    </node>
+
+</launch>

arena_navigation/arena_local_planner/learning_based/arena_local_planner_drl/agents/CNN_NAVREP_2021_01_15__23_28/hyperparameters.json → arena_navigation/arena_local_planner/learning_based/arena_local_planner_drl/agents/MLP_ARENA2D_2021_02_18__13_13/hyperparameters.json

@@ -1,5 +1,5 @@
 {
-    "agent_name": "CNN_NAVREP_2021_01_15__23_28",
+    "agent_name": "MLP_ARENA2D_2021_02_18__13_13",
     "robot": "myrobot",
     "gamma": 0.99,
     "n_steps": 128,
@@ -12,8 +12,6 @@
     "n_epochs": 4,
     "clip_range": 0.2,
     "reward_fnc": "00",
-    "discrete_action_space": true,
-    "task_mode": "staged",
-    "curr_stage": 1,
+    "discrete_action_space": false,
     "n_timesteps": 0
-}
+}

arena_navigation/arena_local_planner/learning_based/arena_local_planner_drl/configs/default_settings.yaml

@@ -1,7 +1,7 @@
 robot:
   discrete_actions:
     - name: move_forward
-      linear: 2.00
+      linear: 2.75
       angular: 0.0
     - name: move_backward
       linear: -1.5
@@ -22,5 +22,5 @@ robot:
       linear: 0.0
       angular: 0.0
   continuous_actions:
-    linear_range: [0, 3.25]
+    linear_range: [0, 0.3]
     angular_range: [-2.7,2.7]

arena_navigation/arena_local_planner/learning_based/arena_local_planner_drl/plot.py

@@ -0,0 +1,53 @@
+# -*- coding: utf-8 -*-
+"""
+    @name:      plot.py
+    @brief:     This file plot the noise data and original data from sensor
+    @author:    Chang Liu
+    @version:   3.7
+    @date:      2020/12/16
+"""
+import matplotlib.pyplot as plt
+import csv
+
+def get_data(column):
+    Original_data_address = 'Original_data_for_plot.csv'  
+    Noise_data_address = 'Noise_data_for_plot.csv'
+    with open(Original_data_address) as f: 
+        reader=csv.reader(f)  
+        Original_dates = []      
+        for row in reader:  
+            Original_dates.append(float(row[column]))   
+        f.close()
+    with open(Noise_data_address) as f: 
+        reader=csv.reader(f)  
+        Noise_dates = []      
+        for row in reader:  
+            Noise_dates.append(float(row[column]))   
+        f.close()
+    return Original_dates,Noise_dates
+
+def plot(column):
+    plt.figure(dpi=85,figsize=(8,6))  
+    plt.ion()
+    for _ in range(1000):
+        plt.cla()
+        Original_dates,Noise_dates = get_data(column)
+        plt.plot(Original_dates,c='red',label = 'Original_dates',alpha=0.5)
+        plt.plot(Noise_dates,c='blue',label = 'Noise_dates',alpha=0.5)  
+        plt.title('Data from ideal sensor and after adding noise',fontsize=20)  
+        plt.xlabel('Time',fontsize=16)  
+        plt.ylabel('Laser sensor values',fontsize=16)  
+        plt.legend()
+        plt.pause(0.5)
+    plt.ioff()
+    plt.show()
+    return
+
+
+if __name__ == "__main__":
+    column = int(input("Please enter the sensor number you wish to display. (Select from 0-90):"))
+    plot(column)
+
+
+
+

arena_navigation/arena_local_planner/learning_based/arena_local_planner_drl/rl_agent/envs/flatland_gym_env.py

@@ -21,7 +21,7 @@
 class FlatlandEnv(gym.Env):
     """Custom Environment that follows gym interface"""
 
-    def __init__(self, task: ABSTask, robot_yaml_path: str, settings_yaml_path: str, is_action_space_discrete, safe_dist: float = None, goal_radius: float = 0.1, max_steps_per_episode=100):
+    def __init__(self, task: ABSTask, robot_yaml_path: str, settings_yaml_path: str, is_action_space_discrete, safe_dist: float = None, goal_radius: float = 0.1, max_steps_per_episode=100,noise_model = [0]):
         """Default env
         Flatland yaml node check the entries in the yaml file, therefore other robot related parameters cound only be saved in an other file.
         TODO : write an uniform yaml paser node to handel with multiple yaml files.
@@ -44,7 +44,7 @@ def __init__(self, task: ABSTask, robot_yaml_path: str, settings_yaml_path: str,
         self.setup_by_configuration(robot_yaml_path, settings_yaml_path)
         # observation collector
         self.observation_collector = ObservationCollector(
-            self._laser_num_beams, self._laser_max_range)
+            self._laser_num_beams, self._laser_max_range,noise_model = noise_model)
         self.observation_space = self.observation_collector.get_observation_space()
 
         # reward calculator