solve some bugs and change some metrics name or defination

tudelft · Mar 1, 2023 · dfebe71 · dfebe71
1 parent c85581e
commit dfebe71
Show file tree

Hide file tree

Showing 199 changed files with 47,218 additions and 174 deletions.
diff --git a/src/ThirdP/rotors_simulator/rotors_gazebo/scripts/__pycache__/names.cpython-38.pyc b/src/ThirdP/rotors_simulator/rotors_gazebo/scripts/__pycache__/names.cpython-38.pyc
diff --git a/...hirdP/rotors_simulator/rotors_gazebo/scripts/__pycache__/substitution_args.cpython-38.pyc b/...hirdP/rotors_simulator/rotors_gazebo/scripts/__pycache__/substitution_args.cpython-38.pyc
diff --git a/src/avoidbench/avoid_manage/CMakeLists.txt b/src/avoidbench/avoid_manage/CMakeLists.txt
@@ -58,6 +58,20 @@ target_link_libraries(avoid_manage_node
   stdc++fs
 )
 
+cs_add_library(rl_test
+    src/rl_test.cpp
+)
+
+cs_add_executable(rl_test_node
+   src/avoid_manage_node.cpp
+)
+
+target_link_libraries(rl_test_node
+  rl_test
+  ${OpenCV_LIBRARIES}
+  stdc++fs
+)
+
 catkin_install_python(PROGRAMS scripts/plot_metrics.py
 				scripts/avoid_manage.py
                       DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

diff --git a/src/avoidbench/avoid_manage/include/avoid_manage.hpp b/src/avoidbench/avoid_manage/include/avoid_manage.hpp
@@ -2,7 +2,7 @@
 #pragma once
 
 #include <memory>
-
+#include <fstream>
 // ros
 #include <nav_msgs/Odometry.h>
 #include <ros/ros.h>
@@ -116,10 +116,12 @@ class AvoidManage {
   nav_msgs::Odometry drone_pose;
   int mission_id;
   int trial_id;
+  int maps_tried_id;
   std::string cfg_;
   quadrotor_common::QuadStateEstimate received_state_est_;
   std::vector<float> iter_times;
   ros::Time mission_start_time;
+  std::string quad_name;
 
   // Flightmare(Unity3D)
   SceneID scene_id_{UnityScene::WAREHOUSE};
@@ -146,6 +148,8 @@ class AvoidManage {
   double baseline;
   bool perform_sgm_;
   std::vector<double> env_range, env_origin;
+
+  std::ofstream debug_out;
 
   void getMissionParam(avoidlib::mission_parameter* const m_param, const int &m_id);
   void resetGazebo(const Eigen::Vector3d &pos, const double yaw);

diff --git a/src/avoidbench/avoid_manage/launch/pilot/iris.launch b/src/avoidbench/avoid_manage/launch/pilot/iris.launch
@@ -0,0 +1,114 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="quad_name" default="iris"/>
+
+  <arg name="mav_name" default="$(arg quad_name)"/>
+  <arg name="model" value="$(find rotors_description)/urdf/mav_generic_odometry_sensor.gazebo"/>
+    <arg name="world_name" value="$(find avoid_manage)/resources/worlds/simple.world"/>
+
+  <arg name="use_unity_editor" default="false" />
+  <arg name="paused" value="false"/>
+  <arg name="gui" value="false"/>
+  <arg name="use_mpc" default="false"/>
+  <arg name="use_ground_truth" default="true"/>
+  <arg name="enable_ground_truth" default="true"/>
+  <arg name="enable_command_feedthrough" default="true"/>
+  <arg name="custom_models" default="$(find avoid_manage)/resources"/>
+
+  <arg name="enable_logging" default="false"/>
+  <arg name="log_file" default="$(arg mav_name)"/>
+
+  <arg name="x_init" default="0"/>
+  <arg name="y_init" default="0"/>
+
+  <arg name="debug" default="false"/>
+  <arg name="verbose" default="false"/>
+
+
+  <!-- Gazebo stuff to spawn the world !-->
+  <env name="GAZEBO_MODEL_PATH"
+      value="${GAZEBO_MODEL_PATH}:$(find rotors_gazebo)/models:$(arg custom_models)"/>
+  <env name="GAZEBO_RESOURCE_PATH"
+      value="${GAZEBO_RESOURCE_PATH}:$(find rotors_gazebo)/models"/>
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="$(arg world_name)" />
+    <arg name="debug" value="$(arg debug)" />
+    <arg name="paused" value="$(arg paused)" />
+    <arg name="gui" value="$(arg gui)" />
+    <arg name="verbose" value="$(arg verbose)"/>
+  </include>
+
+  <!-- RotorS stuff to spawn the quadrotor !-->
+  <group ns="$(arg mav_name)">
+    <include file="$(find rotors_gazebo)/launch/spawn_mav.launch">
+      <arg name="mav_name" value="$(arg mav_name)" />
+      <arg name="model" value="$(arg model)" />
+      <arg name="enable_logging" value="$(arg enable_logging)" />
+      <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
+      <arg name="log_file" value="$(arg log_file)"/>
+      <arg name="x" value="$(arg x_init)" />
+      <arg name="y" value="$(arg y_init)" />
+    </include>
+  </group>
+
+  <!-- RPG stuff !-->
+  <group ns="$(arg quad_name)" >
+
+    <!-- RPG RotorS interface. -->
+    <node pkg="rpg_rotors_interface" type="iris_rotors_interface"
+        name="iris_rotors_interface" output="screen" >
+      <rosparam file="$(find rpg_rotors_interface)/parameters/iris_rotors_interface.yaml" />
+      <!-- .. -->
+      <remap from="odometry" to="ground_truth/odometry" />
+      <remap from="rpg_rotors_interface/arm" to="bridge/arm" />
+    </node>
+
+    <!-- Autopilot -->
+    <group unless="$(arg use_mpc)">
+    <node pkg="autopilot" type="autopilot" name="autopilot" output="screen">
+      <rosparam file="$(find state_predictor)/parameters/hummingbird.yaml" />
+      <rosparam file="$(find rpg_rotors_interface)/parameters/position_controller.yaml" />
+      <rosparam file="$(find rpg_rotors_interface)/parameters/autopilot.yaml" />
+
+      <param name="position_controller/use_rate_mode" value="True" />
+
+      <param name="velocity_estimate_in_world_frame" value="false" />
+      <param name="state_estimate_timeout" value="0.1" />
+      <param name="control_command_delay" value="0.05" />
+      <param name="enable_command_feedthrough" value="$(arg enable_command_feedthrough)" />
+
+      <remap from="autopilot/state_estimate" to="ground_truth/odometry" />
+    </node>
+    </group>
+
+    <group if="$(arg use_mpc)">
+      <node pkg="rpg_mpc" type="autopilot_mpc_instance" name="autopilot" output="screen">
+        <rosparam file="$(find state_predictor)/parameters/hummingbird.yaml" />
+        <rosparam file="$(find rpg_mpc)/parameters/default.yaml" />
+        <rosparam file="$(find rpg_rotors_interface)/parameters/autopilot.yaml" />
+
+        <param name="velocity_estimate_in_world_frame" value="false" />
+        <param name="state_estimate_timeout" value="0.1" />
+        <param name="control_command_delay" value="0.05" />
+        <param name="enable_command_feedthrough" value="$(arg enable_command_feedthrough)" />
+
+        <remap from="autopilot/state_estimate" to="ground_truth/odometry" />
+      </node>
+    </group>
+
+    <node pkg="avoid_manage" type="avoid_manage_node" name="avoid_manage_node" output="screen">
+      <rosparam file="$(find avoid_manage)/params/task_indoor.yaml" />
+      <param name="quad_name" value="$(arg quad_name)" />
+      <remap from="flight_pilot/state_estimate" to="ground_truth/odometry" />
+    </node>
+
+    <node pkg="unity_scene" type="AvoidBench.x86_64" name="avoidbench_render" unless="$(arg use_unity_editor)">
+    </node>
+
+    <node name="rqt_quad_gui" pkg="rqt_gui" type="rqt_gui"
+        args="-s rqt_quad_gui.basic_flight.BasicFlight --args
+        --quad_name $(arg quad_name)" output="screen"/>
+
+  </group>
+
+</launch>
diff --git a/src/avoidbench/avoid_manage/launch/pilot/rotors_gazebo.launch b/src/avoidbench/avoid_manage/launch/pilot/rotors_gazebo.launch
@@ -98,15 +98,12 @@
 
     <node pkg="avoid_manage" type="avoid_manage_node" name="avoid_manage_node" output="screen">
       <rosparam file="$(find avoid_manage)/params/task_outdoor.yaml" />
+      <param name="quad_name" value="$(arg quad_name)" />
       <remap from="flight_pilot/state_estimate" to="ground_truth/odometry" />
     </node>
 
     <node pkg="unity_scene" type="AvoidBench.x86_64" name="avoidbench_render" unless="$(arg use_unity_editor)">
     </node>
-
-    <node name="rqt_quad_gui" pkg="rqt_gui" type="rqt_gui"
-        args="-s rqt_quad_gui.basic_flight.BasicFlight --args
-        --quad_name $(arg quad_name)" output="screen"/>
 
   </group>
 

diff --git a/src/avoidbench/avoid_manage/launch/pilot/test_rl.launch b/src/avoidbench/avoid_manage/launch/pilot/test_rl.launch
@@ -0,0 +1,114 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="quad_name" default="iris"/>
+
+  <arg name="mav_name" default="$(arg quad_name)"/>
+  <arg name="model" value="$(find rotors_description)/urdf/mav_generic_odometry_sensor.gazebo"/>
+    <arg name="world_name" value="$(find avoid_manage)/resources/worlds/simple.world"/>
+
+  <arg name="use_unity_editor" default="true" />
+  <arg name="paused" value="false"/>
+  <arg name="gui" value="false"/>
+  <arg name="use_mpc" default="false"/>
+  <arg name="use_ground_truth" default="true"/>
+  <arg name="enable_ground_truth" default="true"/>
+  <arg name="enable_command_feedthrough" default="true"/>
+  <arg name="custom_models" default="$(find avoid_manage)/resources"/>
+
+  <arg name="enable_logging" default="false"/>
+  <arg name="log_file" default="$(arg mav_name)"/>
+
+  <arg name="x_init" default="0"/>
+  <arg name="y_init" default="0"/>
+
+  <arg name="debug" default="false"/>
+  <arg name="verbose" default="false"/>
+
+
+  <!-- Gazebo stuff to spawn the world !-->
+  <env name="GAZEBO_MODEL_PATH"
+      value="${GAZEBO_MODEL_PATH}:$(find rotors_gazebo)/models:$(arg custom_models)"/>
+  <env name="GAZEBO_RESOURCE_PATH"
+      value="${GAZEBO_RESOURCE_PATH}:$(find rotors_gazebo)/models"/>
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="$(arg world_name)" />
+    <arg name="debug" value="$(arg debug)" />
+    <arg name="paused" value="$(arg paused)" />
+    <arg name="gui" value="$(arg gui)" />
+    <arg name="verbose" value="$(arg verbose)"/>
+  </include>
+
+  <!-- RotorS stuff to spawn the quadrotor !-->
+  <group ns="$(arg mav_name)">
+    <include file="$(find rotors_gazebo)/launch/spawn_mav.launch">
+      <arg name="mav_name" value="$(arg mav_name)" />
+      <arg name="model" value="$(arg model)" />
+      <arg name="enable_logging" value="$(arg enable_logging)" />
+      <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
+      <arg name="log_file" value="$(arg log_file)"/>
+      <arg name="x" value="$(arg x_init)" />
+      <arg name="y" value="$(arg y_init)" />
+    </include>
+  </group>
+
+  <!-- RPG stuff !-->
+  <group ns="$(arg quad_name)" >
+
+    <!-- RPG RotorS interface. -->
+    <node pkg="rpg_rotors_interface" type="iris_rotors_interface"
+        name="iris_rotors_interface" output="screen" >
+      <rosparam file="$(find rpg_rotors_interface)/parameters/iris_rotors_interface.yaml" />
+      <!-- .. -->
+      <remap from="odometry" to="ground_truth/odometry" />
+      <remap from="rpg_rotors_interface/arm" to="bridge/arm" />
+    </node>
+
+    <!-- Autopilot -->
+    <group unless="$(arg use_mpc)">
+    <node pkg="autopilot" type="autopilot" name="autopilot" output="screen">
+      <rosparam file="$(find state_predictor)/parameters/hummingbird.yaml" />
+      <rosparam file="$(find rpg_rotors_interface)/parameters/position_controller.yaml" />
+      <rosparam file="$(find rpg_rotors_interface)/parameters/autopilot.yaml" />
+
+      <param name="position_controller/use_rate_mode" value="True" />
+
+      <param name="velocity_estimate_in_world_frame" value="false" />
+      <param name="state_estimate_timeout" value="0.1" />
+      <param name="control_command_delay" value="0.05" />
+      <param name="enable_command_feedthrough" value="$(arg enable_command_feedthrough)" />
+
+      <remap from="autopilot/state_estimate" to="ground_truth/odometry" />
+    </node>
+    </group>
+
+    <group if="$(arg use_mpc)">
+      <node pkg="rpg_mpc" type="autopilot_mpc_instance" name="autopilot" output="screen">
+        <rosparam file="$(find state_predictor)/parameters/hummingbird.yaml" />
+        <rosparam file="$(find rpg_mpc)/parameters/default.yaml" />
+        <rosparam file="$(find rpg_rotors_interface)/parameters/autopilot.yaml" />
+
+        <param name="velocity_estimate_in_world_frame" value="false" />
+        <param name="state_estimate_timeout" value="0.1" />
+        <param name="control_command_delay" value="0.05" />
+        <param name="enable_command_feedthrough" value="$(arg enable_command_feedthrough)" />
+
+        <remap from="autopilot/state_estimate" to="ground_truth/odometry" />
+      </node>
+    </group>
+
+    <node pkg="avoid_manage" type="rl_test_node" name="rl_test_node" output="screen">
+      <rosparam file="$(find avoid_manage)/params/test_rl.yaml" />
+      <param name="quad_name" value="$(arg quad_name)" />
+      <remap from="flight_pilot/state_estimate" to="ground_truth/odometry" />
+    </node>
+
+    <node pkg="unity_scene" type="AvoidBench.x86_64" name="avoidbench_render" unless="$(arg use_unity_editor)">
+    </node>
+
+    <node name="rqt_quad_gui" pkg="rqt_gui" type="rqt_gui"
+        args="-s rqt_quad_gui.basic_flight.BasicFlight --args
+        --quad_name $(arg quad_name)" output="screen"/>
+
+  </group>
+
+</launch>
diff --git a/src/avoidbench/avoid_manage/params/task_indoor.yaml b/src/avoidbench/avoid_manage/params/task_indoor.yaml
@@ -21,16 +21,16 @@ camera:
   height: 480
   baseline: 0.12
   pitch_angle_deg: 0.0 # camera pitch angle in degrees
-  perform_sgm: false
+  perform_sgm: true
 
 drone:
   length: 0.5
   width: 0.5
 
 flight_number: 30
 mission:
-  seed: 32
-  trials: 1
+  seed: 28
+  trials: 18
   start_area: [16, 1.6]
   start_origin: [0, -1.8]
   radius_area: 3.0

diff --git a/src/avoidbench/avoid_manage/params/task_outdoor.yaml b/src/avoidbench/avoid_manage/params/task_outdoor.yaml
@@ -29,9 +29,9 @@ drone:
 
 flight_number: 30
 mission:
-  trials: 2
+  trials: 28
   # seed: 8 8 32 64 256 500
-  seed: 32 #20 32 128 200 50
+  seed: 128 #20 32 64 100 128 200 50
   start_area: [40, 6.0]
   start_origin: [0, 3.0]
   radius_area: 3.5

diff --git a/src/avoidbench/avoid_manage/scripts/avoid_manage.py b/src/avoidbench/avoid_manage/scripts/avoid_manage.py
@@ -4,6 +4,7 @@
 import sys
 import math
 import random
+import time
 import numpy as np
 from enum import Enum
 from ruamel.yaml import YAML
@@ -41,6 +42,7 @@ def __init__(self):
         self.iter_time = []
         self.mission_id = 0
         self.trial_id = 0
+        self.maps_tried_id = 0
         self.cv_bridge = CvBridge()
         self.loadParams()
         self.p_m = mission_parameter()
@@ -279,6 +281,7 @@ def MissionProcess(self, event):
             self.force_hover_pub_.publish(hover)
             self.mission.cal_time = self.iter_time
             self.iter_time = []
+            self.mission.trial_id = self.trial_id
             self.metrics.setMissions(self.mission)
             rospy.sleep(1.0)
             self.trial_id = self.trial_id + 1
@@ -318,7 +321,6 @@ def getMissionParam(self, m_id):
             random.seed(self.seed)
             self.get_seed = True
         rand = random.random()
-        print("rand: ", rand)
 
         if self.env_id == 3:
             area_id = random.randint(0, 3)
@@ -359,6 +361,11 @@ def getMissionParam(self, m_id):
             self.if_update_map = True
             return
         print("update env")
+        # make sure every mission has the same complexity map when benchmark different algos
+        self.maps_tried_id = self.maps_tried_id+1
+        random.seed(self.seed * self.maps_tried_id)
+        rand = random.random()
+
         self.p_m.trials = self.trials_
         self.p_m.m_radius = self.radius_origin + self.radius_area*rand
         self.p_m.m_seed = random.randint(0, 200)

diff --git a/src/avoidbench/avoid_manage/src/.Rhistory b/src/avoidbench/avoid_manage/src/.Rhistory