Skip to content

Commit dfebe71

Browse files
NPU-yuhangNPU-yuhang
committed
solve some bugs and change some metrics name or defination
1 parent c85581e commit dfebe71

File tree

199 files changed

+47218
-174
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

199 files changed

+47218
-174
lines changed

‎src/ThirdP/rotors_simulator/rotors_gazebo/scripts/__pycache__/names.cpython-38.pyc

22 Bytes
Binary file not shown.

‎src/ThirdP/rotors_simulator/rotors_gazebo/scripts/__pycache__/substitution_args.cpython-38.pyc

22 Bytes
Binary file not shown.

‎src/avoidbench/avoid_manage/CMakeLists.txt

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -58,6 +58,20 @@ target_link_libraries(avoid_manage_node
5858
stdc++fs
5959
)
6060

61+
cs_add_library(rl_test
62+
src/rl_test.cpp
63+
)
64+
65+
cs_add_executable(rl_test_node
66+
src/avoid_manage_node.cpp
67+
)
68+
69+
target_link_libraries(rl_test_node
70+
rl_test
71+
${OpenCV_LIBRARIES}
72+
stdc++fs
73+
)
74+
6175
catkin_install_python(PROGRAMS scripts/plot_metrics.py
6276
scripts/avoid_manage.py
6377
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

‎src/avoidbench/avoid_manage/include/avoid_manage.hpp

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22
#pragma once
33

44
#include <memory>
5-
5+
#include <fstream>
66
// ros
77
#include <nav_msgs/Odometry.h>
88
#include <ros/ros.h>
@@ -116,10 +116,12 @@ class AvoidManage {
116116
nav_msgs::Odometry drone_pose;
117117
int mission_id;
118118
int trial_id;
119+
int maps_tried_id;
119120
std::string cfg_;
120121
quadrotor_common::QuadStateEstimate received_state_est_;
121122
std::vector<float> iter_times;
122123
ros::Time mission_start_time;
124+
std::string quad_name;
123125

124126
// Flightmare(Unity3D)
125127
SceneID scene_id_{UnityScene::WAREHOUSE};
@@ -146,6 +148,8 @@ class AvoidManage {
146148
double baseline;
147149
bool perform_sgm_;
148150
std::vector<double> env_range, env_origin;
151+
152+
std::ofstream debug_out;
149153

150154
void getMissionParam(avoidlib::mission_parameter* const m_param, const int &m_id);
151155
void resetGazebo(const Eigen::Vector3d &pos, const double yaw);

‎src/avoidbench/avoid_manage/launch/pilot/iris.launch

Lines changed: 114 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,114 @@
1+
<?xml version="1.0"?>
2+
<launch>
3+
<arg name="quad_name" default="iris"/>
4+
5+
<arg name="mav_name" default="$(arg quad_name)"/>
6+
<arg name="model" value="$(find rotors_description)/urdf/mav_generic_odometry_sensor.gazebo"/>
7+
<arg name="world_name" value="$(find avoid_manage)/resources/worlds/simple.world"/>
8+
9+
<arg name="use_unity_editor" default="false" />
10+
<arg name="paused" value="false"/>
11+
<arg name="gui" value="false"/>
12+
<arg name="use_mpc" default="false"/>
13+
<arg name="use_ground_truth" default="true"/>
14+
<arg name="enable_ground_truth" default="true"/>
15+
<arg name="enable_command_feedthrough" default="true"/>
16+
<arg name="custom_models" default="$(find avoid_manage)/resources"/>
17+
18+
<arg name="enable_logging" default="false"/>
19+
<arg name="log_file" default="$(arg mav_name)"/>
20+
21+
<arg name="x_init" default="0"/>
22+
<arg name="y_init" default="0"/>
23+
24+
<arg name="debug" default="false"/>
25+
<arg name="verbose" default="false"/>
26+
27+
28+
<!-- Gazebo stuff to spawn the world !-->
29+
<env name="GAZEBO_MODEL_PATH"
30+
value="${GAZEBO_MODEL_PATH}:$(find rotors_gazebo)/models:$(arg custom_models)"/>
31+
<env name="GAZEBO_RESOURCE_PATH"
32+
value="${GAZEBO_RESOURCE_PATH}:$(find rotors_gazebo)/models"/>
33+
<include file="$(find gazebo_ros)/launch/empty_world.launch">
34+
<arg name="world_name" value="$(arg world_name)" />
35+
<arg name="debug" value="$(arg debug)" />
36+
<arg name="paused" value="$(arg paused)" />
37+
<arg name="gui" value="$(arg gui)" />
38+
<arg name="verbose" value="$(arg verbose)"/>
39+
</include>
40+
41+
<!-- RotorS stuff to spawn the quadrotor !-->
42+
<group ns="$(arg mav_name)">
43+
<include file="$(find rotors_gazebo)/launch/spawn_mav.launch">
44+
<arg name="mav_name" value="$(arg mav_name)" />
45+
<arg name="model" value="$(arg model)" />
46+
<arg name="enable_logging" value="$(arg enable_logging)" />
47+
<arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
48+
<arg name="log_file" value="$(arg log_file)"/>
49+
<arg name="x" value="$(arg x_init)" />
50+
<arg name="y" value="$(arg y_init)" />
51+
</include>
52+
</group>
53+
54+
<!-- RPG stuff !-->
55+
<group ns="$(arg quad_name)" >
56+
57+
<!-- RPG RotorS interface. -->
58+
<node pkg="rpg_rotors_interface" type="iris_rotors_interface"
59+
name="iris_rotors_interface" output="screen" >
60+
<rosparam file="$(find rpg_rotors_interface)/parameters/iris_rotors_interface.yaml" />
61+
<!-- .. -->
62+
<remap from="odometry" to="ground_truth/odometry" />
63+
<remap from="rpg_rotors_interface/arm" to="bridge/arm" />
64+
</node>
65+
66+
<!-- Autopilot -->
67+
<group unless="$(arg use_mpc)">
68+
<node pkg="autopilot" type="autopilot" name="autopilot" output="screen">
69+
<rosparam file="$(find state_predictor)/parameters/hummingbird.yaml" />
70+
<rosparam file="$(find rpg_rotors_interface)/parameters/position_controller.yaml" />
71+
<rosparam file="$(find rpg_rotors_interface)/parameters/autopilot.yaml" />
72+
73+
<param name="position_controller/use_rate_mode" value="True" />
74+
75+
<param name="velocity_estimate_in_world_frame" value="false" />
76+
<param name="state_estimate_timeout" value="0.1" />
77+
<param name="control_command_delay" value="0.05" />
78+
<param name="enable_command_feedthrough" value="$(arg enable_command_feedthrough)" />
79+
80+
<remap from="autopilot/state_estimate" to="ground_truth/odometry" />
81+
</node>
82+
</group>
83+
84+
<group if="$(arg use_mpc)">
85+
<node pkg="rpg_mpc" type="autopilot_mpc_instance" name="autopilot" output="screen">
86+
<rosparam file="$(find state_predictor)/parameters/hummingbird.yaml" />
87+
<rosparam file="$(find rpg_mpc)/parameters/default.yaml" />
88+
<rosparam file="$(find rpg_rotors_interface)/parameters/autopilot.yaml" />
89+
90+
<param name="velocity_estimate_in_world_frame" value="false" />
91+
<param name="state_estimate_timeout" value="0.1" />
92+
<param name="control_command_delay" value="0.05" />
93+
<param name="enable_command_feedthrough" value="$(arg enable_command_feedthrough)" />
94+
95+
<remap from="autopilot/state_estimate" to="ground_truth/odometry" />
96+
</node>
97+
</group>
98+
99+
<node pkg="avoid_manage" type="avoid_manage_node" name="avoid_manage_node" output="screen">
100+
<rosparam file="$(find avoid_manage)/params/task_indoor.yaml" />
101+
<param name="quad_name" value="$(arg quad_name)" />
102+
<remap from="flight_pilot/state_estimate" to="ground_truth/odometry" />
103+
</node>
104+
105+
<node pkg="unity_scene" type="AvoidBench.x86_64" name="avoidbench_render" unless="$(arg use_unity_editor)">
106+
</node>
107+
108+
<node name="rqt_quad_gui" pkg="rqt_gui" type="rqt_gui"
109+
args="-s rqt_quad_gui.basic_flight.BasicFlight --args
110+
--quad_name $(arg quad_name)" output="screen"/>
111+
112+
</group>
113+
114+
</launch>

‎src/avoidbench/avoid_manage/launch/pilot/rotors_gazebo.launch

Lines changed: 1 addition & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -98,15 +98,12 @@
9898

9999
<node pkg="avoid_manage" type="avoid_manage_node" name="avoid_manage_node" output="screen">
100100
<rosparam file="$(find avoid_manage)/params/task_outdoor.yaml" />
101+
<param name="quad_name" value="$(arg quad_name)" />
101102
<remap from="flight_pilot/state_estimate" to="ground_truth/odometry" />
102103
</node>
103104

104105
<node pkg="unity_scene" type="AvoidBench.x86_64" name="avoidbench_render" unless="$(arg use_unity_editor)">
105106
</node>
106-
107-
<node name="rqt_quad_gui" pkg="rqt_gui" type="rqt_gui"
108-
args="-s rqt_quad_gui.basic_flight.BasicFlight --args
109-
--quad_name $(arg quad_name)" output="screen"/>
110107

111108
</group>
112109

‎src/avoidbench/avoid_manage/launch/pilot/test_rl.launch

Lines changed: 114 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,114 @@
1+
<?xml version="1.0"?>
2+
<launch>
3+
<arg name="quad_name" default="iris"/>
4+
5+
<arg name="mav_name" default="$(arg quad_name)"/>
6+
<arg name="model" value="$(find rotors_description)/urdf/mav_generic_odometry_sensor.gazebo"/>
7+
<arg name="world_name" value="$(find avoid_manage)/resources/worlds/simple.world"/>
8+
9+
<arg name="use_unity_editor" default="true" />
10+
<arg name="paused" value="false"/>
11+
<arg name="gui" value="false"/>
12+
<arg name="use_mpc" default="false"/>
13+
<arg name="use_ground_truth" default="true"/>
14+
<arg name="enable_ground_truth" default="true"/>
15+
<arg name="enable_command_feedthrough" default="true"/>
16+
<arg name="custom_models" default="$(find avoid_manage)/resources"/>
17+
18+
<arg name="enable_logging" default="false"/>
19+
<arg name="log_file" default="$(arg mav_name)"/>
20+
21+
<arg name="x_init" default="0"/>
22+
<arg name="y_init" default="0"/>
23+
24+
<arg name="debug" default="false"/>
25+
<arg name="verbose" default="false"/>
26+
27+
28+
<!-- Gazebo stuff to spawn the world !-->
29+
<env name="GAZEBO_MODEL_PATH"
30+
value="${GAZEBO_MODEL_PATH}:$(find rotors_gazebo)/models:$(arg custom_models)"/>
31+
<env name="GAZEBO_RESOURCE_PATH"
32+
value="${GAZEBO_RESOURCE_PATH}:$(find rotors_gazebo)/models"/>
33+
<include file="$(find gazebo_ros)/launch/empty_world.launch">
34+
<arg name="world_name" value="$(arg world_name)" />
35+
<arg name="debug" value="$(arg debug)" />
36+
<arg name="paused" value="$(arg paused)" />
37+
<arg name="gui" value="$(arg gui)" />
38+
<arg name="verbose" value="$(arg verbose)"/>
39+
</include>
40+
41+
<!-- RotorS stuff to spawn the quadrotor !-->
42+
<group ns="$(arg mav_name)">
43+
<include file="$(find rotors_gazebo)/launch/spawn_mav.launch">
44+
<arg name="mav_name" value="$(arg mav_name)" />
45+
<arg name="model" value="$(arg model)" />
46+
<arg name="enable_logging" value="$(arg enable_logging)" />
47+
<arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
48+
<arg name="log_file" value="$(arg log_file)"/>
49+
<arg name="x" value="$(arg x_init)" />
50+
<arg name="y" value="$(arg y_init)" />
51+
</include>
52+
</group>
53+
54+
<!-- RPG stuff !-->
55+
<group ns="$(arg quad_name)" >
56+
57+
<!-- RPG RotorS interface. -->
58+
<node pkg="rpg_rotors_interface" type="iris_rotors_interface"
59+
name="iris_rotors_interface" output="screen" >
60+
<rosparam file="$(find rpg_rotors_interface)/parameters/iris_rotors_interface.yaml" />
61+
<!-- .. -->
62+
<remap from="odometry" to="ground_truth/odometry" />
63+
<remap from="rpg_rotors_interface/arm" to="bridge/arm" />
64+
</node>
65+
66+
<!-- Autopilot -->
67+
<group unless="$(arg use_mpc)">
68+
<node pkg="autopilot" type="autopilot" name="autopilot" output="screen">
69+
<rosparam file="$(find state_predictor)/parameters/hummingbird.yaml" />
70+
<rosparam file="$(find rpg_rotors_interface)/parameters/position_controller.yaml" />
71+
<rosparam file="$(find rpg_rotors_interface)/parameters/autopilot.yaml" />
72+
73+
<param name="position_controller/use_rate_mode" value="True" />
74+
75+
<param name="velocity_estimate_in_world_frame" value="false" />
76+
<param name="state_estimate_timeout" value="0.1" />
77+
<param name="control_command_delay" value="0.05" />
78+
<param name="enable_command_feedthrough" value="$(arg enable_command_feedthrough)" />
79+
80+
<remap from="autopilot/state_estimate" to="ground_truth/odometry" />
81+
</node>
82+
</group>
83+
84+
<group if="$(arg use_mpc)">
85+
<node pkg="rpg_mpc" type="autopilot_mpc_instance" name="autopilot" output="screen">
86+
<rosparam file="$(find state_predictor)/parameters/hummingbird.yaml" />
87+
<rosparam file="$(find rpg_mpc)/parameters/default.yaml" />
88+
<rosparam file="$(find rpg_rotors_interface)/parameters/autopilot.yaml" />
89+
90+
<param name="velocity_estimate_in_world_frame" value="false" />
91+
<param name="state_estimate_timeout" value="0.1" />
92+
<param name="control_command_delay" value="0.05" />
93+
<param name="enable_command_feedthrough" value="$(arg enable_command_feedthrough)" />
94+
95+
<remap from="autopilot/state_estimate" to="ground_truth/odometry" />
96+
</node>
97+
</group>
98+
99+
<node pkg="avoid_manage" type="rl_test_node" name="rl_test_node" output="screen">
100+
<rosparam file="$(find avoid_manage)/params/test_rl.yaml" />
101+
<param name="quad_name" value="$(arg quad_name)" />
102+
<remap from="flight_pilot/state_estimate" to="ground_truth/odometry" />
103+
</node>
104+
105+
<node pkg="unity_scene" type="AvoidBench.x86_64" name="avoidbench_render" unless="$(arg use_unity_editor)">
106+
</node>
107+
108+
<node name="rqt_quad_gui" pkg="rqt_gui" type="rqt_gui"
109+
args="-s rqt_quad_gui.basic_flight.BasicFlight --args
110+
--quad_name $(arg quad_name)" output="screen"/>
111+
112+
</group>
113+
114+
</launch>

‎src/avoidbench/avoid_manage/params/task_indoor.yaml

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -21,16 +21,16 @@ camera:
2121
height: 480
2222
baseline: 0.12
2323
pitch_angle_deg: 0.0 # camera pitch angle in degrees
24-
perform_sgm: false
24+
perform_sgm: true
2525

2626
drone:
2727
length: 0.5
2828
width: 0.5
2929

3030
flight_number: 30
3131
mission:
32-
seed: 32
33-
trials: 1
32+
seed: 28
33+
trials: 18
3434
start_area: [16, 1.6]
3535
start_origin: [0, -1.8]
3636
radius_area: 3.0

‎src/avoidbench/avoid_manage/params/task_outdoor.yaml

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -29,9 +29,9 @@ drone:
2929

3030
flight_number: 30
3131
mission:
32-
trials: 2
32+
trials: 28
3333
# seed: 8 8 32 64 256 500
34-
seed: 32 #20 32 128 200 50
34+
seed: 128 #20 32 64 100 128 200 50
3535
start_area: [40, 6.0]
3636
start_origin: [0, 3.0]
3737
radius_area: 3.5

‎src/avoidbench/avoid_manage/scripts/avoid_manage.py

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@
44
import sys
55
import math
66
import random
7+
import time
78
import numpy as np
89
from enum import Enum
910
from ruamel.yaml import YAML
@@ -41,6 +42,7 @@ def __init__(self):
4142
self.iter_time = []
4243
self.mission_id = 0
4344
self.trial_id = 0
45+
self.maps_tried_id = 0
4446
self.cv_bridge = CvBridge()
4547
self.loadParams()
4648
self.p_m = mission_parameter()
@@ -279,6 +281,7 @@ def MissionProcess(self, event):
279281
self.force_hover_pub_.publish(hover)
280282
self.mission.cal_time = self.iter_time
281283
self.iter_time = []
284+
self.mission.trial_id = self.trial_id
282285
self.metrics.setMissions(self.mission)
283286
rospy.sleep(1.0)
284287
self.trial_id = self.trial_id + 1
@@ -318,7 +321,6 @@ def getMissionParam(self, m_id):
318321
random.seed(self.seed)
319322
self.get_seed = True
320323
rand = random.random()
321-
print("rand: ", rand)
322324

323325
if self.env_id == 3:
324326
area_id = random.randint(0, 3)
@@ -359,6 +361,11 @@ def getMissionParam(self, m_id):
359361
self.if_update_map = True
360362
return
361363
print("update env")
364+
# make sure every mission has the same complexity map when benchmark different algos
365+
self.maps_tried_id = self.maps_tried_id+1
366+
random.seed(self.seed * self.maps_tried_id)
367+
rand = random.random()
368+
362369
self.p_m.trials = self.trials_
363370
self.p_m.m_radius = self.radius_origin + self.radius_area*rand
364371
self.p_m.m_seed = random.randint(0, 200)

‎src/avoidbench/avoid_manage/src/.Rhistory

Whitespace-only changes.

0 commit comments

Comments
 (0)