diff --git a/flightlib/src/ros_nodes/traj_eval_node.cpp b/flightlib/src/ros_nodes/traj_eval_node.cpp index 28280ee..78b7d47 100644 --- a/flightlib/src/ros_nodes/traj_eval_node.cpp +++ b/flightlib/src/ros_nodes/traj_eval_node.cpp @@ -7,17 +7,17 @@ #include #include #include - #include - #include "flightlib/controller/ctrl_ref.h" namespace traj_eval { +// Evaluate the performance of the trajectory from the origin [start_record, y, z] to destination [finish_record, y, z] float start_record = -39; float finish_record = 18; +float ctrl_dt = 0.02; // eval -std::ofstream log_, log_x, ctrl_log; +std::ofstream dist_log, ctrl_log; Eigen::Vector3f pose_last; Eigen::Vector3f acc_last; ros::Time start, end; @@ -67,7 +67,7 @@ void odom_cb(const nav_msgs::Odometry::Ptr odom_msg) { if (!odom_init && odom_msg->pose.pose.position.x > start_record) { odom_init = true; pose_last = Eigen::Vector3f(odom_msg->pose.pose.position.x, odom_msg->pose.pose.position.y, odom_msg->pose.pose.position.z); - start = ros::Time::now(); + start = odom_msg->header.stamp; std::cout << "start!" << std::endl; return; } @@ -77,18 +77,19 @@ void odom_cb(const nav_msgs::Odometry::Ptr odom_msg) { Eigen::Vector3f pose_cur(odom_msg->pose.pose.position.x, odom_msg->pose.pose.position.y, odom_msg->pose.pose.position.z); length_ += (pose_cur - pose_last).norm(); dist_ += distance_at(pose_cur); - log_ << distance_at(pose_cur) << std::endl; - log_x << pose_cur(0) << std::endl; + + dist_log << (odom_msg->header.stamp - start).toSec() << ','; + dist_log << pose_cur(0) << ','; + dist_log << distance_at(pose_cur) << std::endl; pose_last = pose_cur; num_++; if (odom_msg->pose.pose.position.x > finish_record) { odom_finish = true; - end = ros::Time::now(); - std::cout << "finish! \n time:" << (end - start).toSec() << " s,\nlength:" << length_ << " m,\ndist:" << dist_ / num_ + end = odom_msg->header.stamp; + std::cout << "finish! \ntime:" << (end - start).toSec() << " s,\nlength:" << length_ << " m,\ndist:" << dist_ / num_ << " m,\nctrl cost:" << ctrl_cost_ << " m2/s5" << std::endl; - log_.close(); - log_x.close(); + dist_log.close(); ctrl_log.close(); } } @@ -103,9 +104,9 @@ void ctrl_cb(const quad_pos_ctrl::ctrl_ref& ctrl_msg) { } Eigen::Vector3f cur_acc(ctrl_msg.acc_ref[0], -ctrl_msg.acc_ref[1], -ctrl_msg.acc_ref[2]); - Eigen::Vector3f d_acc = (acc_last - cur_acc) / 0.02; + Eigen::Vector3f d_acc = (acc_last - cur_acc) / ctrl_dt; float acc_norm2 = d_acc.dot(d_acc); - ctrl_cost_ += 0.02 * acc_norm2; + ctrl_cost_ += ctrl_dt * acc_norm2; acc_last = cur_acc; ctrl_log << ctrl_msg.pos_ref[0] << ','; @@ -126,16 +127,13 @@ void ctrl_cb(const quad_pos_ctrl::ctrl_ref& ctrl_msg) { using namespace traj_eval; int main(int argc, char** argv) { map_input(); - // Move to the same log file. - std::string log_file = getenv("FLIGHTMARE_PATH") + std::string("/run/utils/dist.csv"); + + std::string log_file = getenv("FLIGHTMARE_PATH") + std::string("/run/utils/dist_log.csv"); std::cout << "log path:" << log_file << std::endl; - log_.open(log_file.c_str(), std::ios::out); + dist_log.open(log_file.c_str(), std::ios::out); - std::string log_file2 = getenv("FLIGHTMARE_PATH") + std::string("/run/utils/dist_x.csv"); - log_x.open(log_file2.c_str(), std::ios::out); - - std::string log_file3 = getenv("FLIGHTMARE_PATH") + std::string("/run/utils/ctrl_log.csv"); - ctrl_log.open(log_file3.c_str(), std::ios::out); + std::string log_file2 = getenv("FLIGHTMARE_PATH") + std::string("/run/utils/ctrl_log.csv"); + ctrl_log.open(log_file2.c_str(), std::ios::out); ros::init(argc, argv, "traj_eval"); ros::NodeHandle nh; diff --git a/run/data_collection_realworld.py b/run/data_collection_realworld.py index 6133a5a..ebeb015 100644 --- a/run/data_collection_realworld.py +++ b/run/data_collection_realworld.py @@ -1,7 +1,8 @@ """ -# 收集实飞数据，记录位置、姿态、图像，用于离线fine-tuning (保存至save_dir) -# 注意： 由于里程计漂移，可能utils/pointcloud_clip需要对地图进行微调，需对无人机位置和yaw, pitch, roll做相同的变换 -# 注意保证地图和里程计处于同一坐标系，同时录包+保存地图 +算法具有有一定的Sim2Real的泛化能力, 如果有条件可用雷达+深度相机收集数据, 合并至仿真数据集中一同训练, 以进一步保证实飞的可靠性 +# (1) 运行雷达里程计以记录无人机状态和地图真值. 注意保证地图和里程计处于同一坐标系，请在一次运行中同时记录图像与里程计的rosbag + 保存地图 +# (可选) 由于里程计漂移，可用utils/pointcloud_clip对地图进行微调和降噪，本文件需对无人机位置translation_no和姿态R_no(yaw, pitch, roll)做相同的变换 +# (2) 播包rosbag, 运行本文件: 记录位置、姿态、图像，保存至save_dir """ import cv2 import numpy as np @@ -12,9 +13,21 @@ from sensor_msgs.msg import Image from nav_msgs.msg import Odometry from scipy.spatial.transform import Rotation -depth_img = np.zeros([270, 480]) -pos = np.array([0, 0, 0]) -quat = np.array([1, 0, 0, 0]) +# IMPORTANT PARAM +save_dir = os.environ["FLIGHTMARE_PATH"] + "/run/yopo_realworld" +label_path = save_dir + "/label.npz" +if not os.path.exists(save_dir): + os.mkdir(save_dir) +# Due to odometry drift, the map is adjusted, and the drone's position is also adjusted accordingly. +R_no = Rotation.from_euler('ZYX', [0.0, 0.0, 0.0], degrees=True) # yaw, pitch, roll +translation_no = np.array([0.0, 0.0, 0.0]) +img_height = 270 +img_width = 480 + +# VARIABLES +depth_img = None +pos = None +quat = None positions = [] quaternions = [] frame_id = 0 @@ -22,13 +35,6 @@ new_depth = False new_odom = False first_frame = True last_time = time.time() -save_dir = os.environ["FLIGHTMARE_PATH"] + "/run/depth_realworld" -label_path = save_dir + "/label.npz" -if not os.path.exists(save_dir): - os.mkdir(save_dir) -# Due to odometry drift, the map is adjusted, and the drone's position is also adjusted accordingly. -R_no = Rotation.from_euler('ZYX', [15, 3, 0.0], degrees=True) # yaw, pitch, roll -translation_no = np.array([0, 0, 2]) def callback_odometry(data): @@ -49,17 +55,17 @@ def callback_odometry(data): def callback_depth(data): - global depth_img, new_depth + global depth_img, new_depth, img_height, img_width max_dis = 20.0 min_dis = 0.03 - height = 270 - width = 480 + height = img_height + width = img_width scale = 0.001 bridge = CvBridge() try: depth_ = bridge.imgmsg_to_cv2(data, "32FC1") except: - print("CV_bridge ERROR: Your ros and python path has something wrong!") + print("CV_bridge ERROR: Possible solutions may be found at https://github.com/TJU-Aerial-Robotics/YOPO/issues/2") if depth_.shape[0] != height or depth_.shape[1] != width: depth_ = cv2.resize(depth_, (width, height), interpolation=cv2.INTER_NEAREST) @@ -106,14 +112,10 @@ def save_data(_timer): frame_id = frame_id + 1 -def main(): +if __name__ == "__main__": rospy.init_node('data_collect', anonymous=False) odom_ref_sub = rospy.Subscriber("/odometry/imu", Odometry, callback_odometry, queue_size=1) depth_sub = rospy.Subscriber("/camera/depth/image_rect_raw", Image, callback_depth, queue_size=1) timer = rospy.Timer(rospy.Duration(0.033), save_data) print("Data Collection Node Ready!") rospy.spin() - - -if __name__ == "__main__": - main() diff --git a/run/utils/log_plot.py b/run/utils/log_plot.py index 2a5b517..a84e928 100755 --- a/run/utils/log_plot.py +++ b/run/utils/log_plot.py @@ -1,18 +1,17 @@ import numpy as np import matplotlib.pyplot as plt +import os if __name__ == '__main__': - file_path = "/home/lu/flightmare/flightmare/run/utils/dist.csv" - temp = np.loadtxt(file_path, dtype=float, delimiter=",") - file_path = "/home/lu/flightmare/flightmare/run/utils/dist_x.csv" - tempX = np.loadtxt(file_path, dtype=float, delimiter=",") - plt.plot(tempX, temp) + file_path = os.environ["FLIGHTMARE_PATH"] + "/run/utils/dist_log.csv" + dist_log = np.loadtxt(file_path, dtype=float, delimiter=",") + plt.plot(dist_log[:, 0], dist_log[:, 2]) plt.show() - print("dist min:", np.min(temp)) - file_path = "/home/lu/flightmare/flightmare/run/utils/ctrl_log.csv" + print("dist min:", np.min(dist_log[:, 2])) + + file_path = os.environ["FLIGHTMARE_PATH"] + "/run/utils/ctrl_log.csv" ctrl_log = np.loadtxt(file_path, dtype=float, delimiter=",") - v_total = np.sqrt( - ctrl_log[:, 3] * ctrl_log[:, 3] + ctrl_log[:, 4] * ctrl_log[:, 4] + ctrl_log[:, 5] * ctrl_log[:, 5]) + v_total = np.sqrt(ctrl_log[:, 3] * ctrl_log[:, 3] + ctrl_log[:, 4] * ctrl_log[:, 4] + ctrl_log[:, 5] * ctrl_log[:, 5]) print("v max: ", np.max(v_total)) plt.plot(ctrl_log[:, 3], label='vx') plt.plot(ctrl_log[:, 4], label='vy') diff --git a/run/utils/pointcloud_clip.py b/run/utils/pointcloud_clip.py index 2d510e7..a8173c4 100644 --- a/run/utils/pointcloud_clip.py +++ b/run/utils/pointcloud_clip.py @@ -1,52 +1,38 @@ -# 实飞数据训练：将全局地图裁剪并保存 -# 1、注意数据收集时，地面尽量平，且需要为z=0 -# 2、收集数据不平时，修改yaw_angle_radians, pitch_angle_radians平移，并与data collection一致 -# 3、bug：需要打开保存的文件，手动把前面几行的double改成float... +""" +算法具有一定的Sim2Real的泛化能力, 如果有条件可用雷达+深度相机收集数据, 合并至仿真数据集中一同训练, 以进一步保证实飞的可靠性 +# (1) 运行雷达里程计以记录无人机状态和地图真值. 注意保证地图和里程计处于同一坐标系，请在一次运行中同时记录图像与里程计的rosbag + 保存地图 +# (可选) 运行本文件对地图进行降噪, 并可修改translation_no和R_no(yaw, pitch, roll)对地图进行变换，修正里程计漂移导致的地图倾斜，注意与data_collection_realworld一致 + (BUG: 打开保存的地图ply文件，手动把前面几行的double改成float) +# (3) 播包rosbag, 运行data_collection_realworld, 记录位置、姿态、图像，保存至save_dir +""" import open3d as o3d import numpy as np +from scipy.spatial.transform import Rotation -# 1. 加载点云数据 -point_cloud = o3d.io.read_point_cloud("1.pcd") # 替换为点云文件的路径 +R_no = Rotation.from_euler('ZYX', [0.0, 0.0, 0.0], degrees=True) # yaw, pitch, roll +translation_no = np.array([0.0, 0.0, 0.0]) +# 0. 加载点云数据 +point_cloud = o3d.io.read_point_cloud("map_original.pcd") # 替换为点云文件的路径 -# # 统计离群点移除滤波 -# cl, ind = cropped_point_cloud.remove_statistical_outlier(nb_neighbors=5, std_ratio=1.0) # 调整参数以控制移除离群点的程度 -# filtered_cloud = cropped_point_cloud.select_by_index(ind) +# 1. 统计离群点移除滤波 +cl, ind = point_cloud.remove_statistical_outlier(nb_neighbors=6, std_ratio=2.0) +point_cloud = point_cloud.select_by_index(ind) -# 2. 定义旋转角度（偏航角和俯仰角） -yaw_angle_degrees = -15 # 偏航角（以度为单位） -pitch_angle_degrees = -3 # 俯仰角（以度为单位） -# 3. 将角度转换为弧度 -yaw_angle_radians = np.radians(yaw_angle_degrees) -pitch_angle_radians = np.radians(pitch_angle_degrees) - -yaw_rotation = np.array([[np.cos(yaw_angle_radians), -np.sin(yaw_angle_radians), 0], - [np.sin(yaw_angle_radians), np.cos(yaw_angle_radians), 0], - [0, 0, 1]]) - -pitch_rotation = np.array([[np.cos(pitch_angle_radians), 0, np.sin(pitch_angle_radians)], - [0, 1, 0], - [-np.sin(pitch_angle_radians), 0, np.cos(pitch_angle_radians)]]) -# 4. 平移2米到Z方向 -translation_no = np.array([0, 0, 2]) # 平移2米到Z方向 - -# 5. 组合旋转矩阵 R old->new -R_on = np.dot(yaw_rotation, pitch_rotation) # 内旋是右乘，先yaw后pitch +# 2. 旋转地图以进行矫正 # P_n = (R_no * P_o.T).T + t_no = P_o * R_on + t_no +R_on = R_no.inv().as_matrix() point_cloud.points = o3d.utility.Vector3dVector(np.dot(np.asarray(point_cloud.points), R_on) + translation_no) # o3d.visualization.draw_geometries([point_cloud]) +# 3. 裁剪点云无关区域 +min_bound = np.array([-50.0, -50.0, -1]) +max_bound = np.array([50.0, 50.0, 6]) -# 2. 定义裁剪范围 -# 例如，裁剪一个立方体范围，这里给出立方体的最小点和最大点坐标 -min_bound = np.array([-5.0, -18.0, 0]) # 最小点坐标 -max_bound = np.array([150.0, 25.0, 6]) # 最大点坐标 - -# 3. 使用crop函数裁剪点云 cropped_point_cloud = point_cloud.crop(o3d.geometry.AxisAlignedBoundingBox(min_bound, max_bound)) -o3d.io.write_point_cloud("realworld.ply", cropped_point_cloud, write_ascii=True) +o3d.io.write_point_cloud("map_processed.ply", cropped_point_cloud, write_ascii=True) o3d.visualization.draw_geometries([cropped_point_cloud]) \ No newline at end of file