From a4e08e52bcccc3798ff6a6c8bda47e06c90c1b0d Mon Sep 17 00:00:00 2001 From: jiaqchen Date: Sat, 23 Apr 2022 02:56:12 +0200 Subject: [PATCH 1/2] new node for setting pose and getting images from python script --- app/tools/parse_config_to_airsim.py | 2 +- .../online_simulator/sensor_timer.h | 1 + .../online_simulator/simulator.h | 2 + src/online_simulator/sensor_timer.cpp | 4 + src/online_simulator/simulator.cpp | 77 ++++++++++++++++++- 5 files changed, 84 insertions(+), 2 deletions(-) diff --git a/app/tools/parse_config_to_airsim.py b/app/tools/parse_config_to_airsim.py index f9996d9..93d69e9 100755 --- a/app/tools/parse_config_to_airsim.py +++ b/app/tools/parse_config_to_airsim.py @@ -273,7 +273,7 @@ def parse_transformation(self, list_in, dict_out): dict_out["Z"] = -list_in[2][3] R = [x[:3] for x in list_in[:3]] # Support python 2 and 3 scipy versions. - if sys.version_info >= (3, 0): + if sys.version_info < (3, 0): R = Rotation.from_matrix(R) else: R = Rotation.from_dcm(R) diff --git a/include/unreal_airsim/online_simulator/sensor_timer.h b/include/unreal_airsim/online_simulator/sensor_timer.h index 349f68c..69e82ec 100644 --- a/include/unreal_airsim/online_simulator/sensor_timer.h +++ b/include/unreal_airsim/online_simulator/sensor_timer.h @@ -24,6 +24,7 @@ class SensorTimer { virtual ~SensorTimer() = default; void timerCallback(const ros::TimerEvent&); + void getImages(); double getRate() const; bool isPrivate() const; diff --git a/include/unreal_airsim/online_simulator/simulator.h b/include/unreal_airsim/online_simulator/simulator.h index 0dbd6b6..d3025ee 100644 --- a/include/unreal_airsim/online_simulator/simulator.h +++ b/include/unreal_airsim/online_simulator/simulator.h @@ -112,6 +112,7 @@ class AirsimSimulator { * set pose should do for most purposes. */ void commandPoseCallback(const geometry_msgs::Pose& msg); + void commandPoseNoOdomCallback(const geometry_msgs::Pose& msg); // added from trajectory caller node void commandTrajectorycallback( @@ -139,6 +140,7 @@ class AirsimSimulator { ros::Publisher sim_is_ready_pub_; ros::Publisher time_pub_; ros::Subscriber command_pose_sub_; + ros::Subscriber command_pose_no_odom_sub_; ros::Subscriber command_trajectory_sub_; tf2_ros::TransformBroadcaster tf_broadcaster_; tf2_ros::StaticTransformBroadcaster static_tf_broadcaster_; diff --git a/src/online_simulator/sensor_timer.cpp b/src/online_simulator/sensor_timer.cpp index d56269d..addb2b8 100644 --- a/src/online_simulator/sensor_timer.cpp +++ b/src/online_simulator/sensor_timer.cpp @@ -44,6 +44,10 @@ void SensorTimer::timerCallback(const ros::TimerEvent&) { processImus(); } +void SensorTimer::getImages() { + processCameras(); +} + void SensorTimer::addSensor(const AirsimSimulator& simulator, int sensor_index) { AirsimSimulator::Config::Sensor* sensor = diff --git a/src/online_simulator/simulator.cpp b/src/online_simulator/simulator.cpp index 666f4c3..4864da3 100644 --- a/src/online_simulator/simulator.cpp +++ b/src/online_simulator/simulator.cpp @@ -291,6 +291,10 @@ bool AirsimSimulator::setupROS() { nh_.subscribe(config_.vehicle_name + "/command/pose", 10, &AirsimSimulator::commandPoseCallback, this); + command_pose_no_odom_sub_ = + nh_.subscribe(config_.vehicle_name + "/command/pose_no_odom", 10, + &AirsimSimulator::commandPoseNoOdomCallback, this); + command_trajectory_sub_ = nh_.subscribe(config_.vehicle_name + "/command/trajectory", 10, &AirsimSimulator::commandTrajectorycallback, this); @@ -468,7 +472,6 @@ void AirsimSimulator::commandPoseCallback(const geometry_msgs::Pose& msg) { // Use position + yaw as setpoint auto command_pos = T_gt_command.getPosition(); - auto command_ori = T_gt_command.getEigenQuaternion(); std::cout << "Pose Orientation In ROS Frame: " << command_ori.x() << " " << command_ori.y() << " " << command_ori.z() << " " @@ -508,6 +511,78 @@ void AirsimSimulator::commandPoseCallback(const geometry_msgs::Pose& msg) { } } +void AirsimSimulator::commandPoseNoOdomCallback(const geometry_msgs::Pose& msg) { + if (!is_running_) { + std::cout << "not running" << std::endl; + return; + } + + // // Input pose is in drifting odom frame, we therefore + // // first convert it back into Unreal GT frame + // OdometryDriftSimulator::Transformation T_drift_command; + // tf::poseMsgToKindr(msg, &T_drift_command); + // const OdometryDriftSimulator::Transformation T_gt_command = + // odometry_drift_simulator_.convertDriftedToGroundTruthPose( + // T_drift_command); + OdometryDriftSimulator::Transformation::Position t_gt_current_position = + odometry_drift_simulator_.getGroundTruthPose().getPosition(); + + // Use position + yaw as setpoint + auto command_pos = msg.position; + auto command_ori = msg.orientation; + std::cout << "Pose Orientation In ROS Frame: " << command_ori.x << " " + << command_ori.y << " " << command_ori.z << " " + << command_ori.w << std::endl; + frame_converter_.rosToAirsim(&command_ori); + std::cout << "Pose Orientation In Airsim Frame: " << command_ori.x << " " + << command_ori.y << " " << command_ori.z << " " + << command_ori.w << std::endl; + double yaw = tf2::getYaw( + tf2::Quaternion(command_ori.x, command_ori.y, command_ori.z, + command_ori.w)); // Eigen's eulerAngles apparently + // messes up some wrap arounds or direcions and gets the wrong yaws in some + // cases + // config_.velocity =0.2f; + yaw = yaw / M_PI * 180.0; + constexpr double kMinMovingDistance = 0.1; // m + airsim_move_client_.cancelLastTask(); + + Eigen::Vector3f command_pos_eigen(command_pos.x, command_pos.y, command_pos.z); + Eigen::Vector3f gt_curr_pos_eigen(t_gt_current_position[0], + t_gt_current_position[1], + t_gt_current_position[2]); + Eigen::Quaternion command_ori_eigen(float(command_ori.w), + float(command_ori.x), + float(command_ori.y), + float(command_ori.z)); + std::cout << "Old Pose: " << command_pos_eigen << std::endl; + std::cout << "New Pose: " << gt_curr_pos_eigen << std::endl; + std::cout << "Norm: " << (command_pos_eigen - gt_curr_pos_eigen).norm() << std::endl; + std::cout << "kMinMovingDistance: " << kMinMovingDistance << std::endl; + + if ((command_pos_eigen - gt_curr_pos_eigen).norm() >= kMinMovingDistance) { + frame_converter_.rosToAirsim(&command_pos); + auto yaw_mode = msr::airlib::YawMode(false, yaw); + + airsim_move_client_.simSetVehiclePose(Pose(command_pos_eigen, command_ori_eigen), + true, + "airsim_drone"); + std::cout << "Moving to target position with pose " << command_pos_eigen + << " and orientation " << command_ori_eigen + << std::endl; + } else { + // This second command catches the case if the total distance is too small, + // where the moveToPosition command returns without satisfying the yaw. If + // this is always run then apparently sometimes the move command is + // overwritten. + airsim_move_client_.rotateToYawAsync(yaw, 3600, 5, config_.vehicle_name); + std::cout << "No moving, only rotating" << std::endl; + } + + // Trigger image collection + sensor_timers_.front()->getImages(); // which sensor timer ends up getting chosen here? +} + void AirsimSimulator::startupCallback(const ros::TimerEvent&) { // Startup the drone, this should set the MAV hovering at 'PlayerStart' in // unreal From e4c6494f07ab6b6bb456163e1130876e4f39a003 Mon Sep 17 00:00:00 2001 From: jiaqchen Date: Fri, 16 Sep 2022 08:07:50 +0200 Subject: [PATCH 2/2] collect images, no odometry in simulator.cpp --- src/online_simulator/simulator.cpp | 65 +++++++++++------------------- 1 file changed, 23 insertions(+), 42 deletions(-) diff --git a/src/online_simulator/simulator.cpp b/src/online_simulator/simulator.cpp index 4864da3..866b7e7 100644 --- a/src/online_simulator/simulator.cpp +++ b/src/online_simulator/simulator.cpp @@ -292,7 +292,7 @@ bool AirsimSimulator::setupROS() { &AirsimSimulator::commandPoseCallback, this); command_pose_no_odom_sub_ = - nh_.subscribe(config_.vehicle_name + "/command/pose_no_odom", 10, + nh_.subscribe(config_.vehicle_name + "/command/pose_no_odom", 1, &AirsimSimulator::commandPoseNoOdomCallback, this); command_trajectory_sub_ = @@ -513,74 +513,55 @@ void AirsimSimulator::commandPoseCallback(const geometry_msgs::Pose& msg) { void AirsimSimulator::commandPoseNoOdomCallback(const geometry_msgs::Pose& msg) { if (!is_running_) { - std::cout << "not running" << std::endl; + std::cout << "Not running." << std::endl; return; } // // Input pose is in drifting odom frame, we therefore // // first convert it back into Unreal GT frame - // OdometryDriftSimulator::Transformation T_drift_command; - // tf::poseMsgToKindr(msg, &T_drift_command); - // const OdometryDriftSimulator::Transformation T_gt_command = - // odometry_drift_simulator_.convertDriftedToGroundTruthPose( - // T_drift_command); + OdometryDriftSimulator::Transformation T_drift_command; + tf::poseMsgToKindr(msg, &T_drift_command); + const OdometryDriftSimulator::Transformation T_gt_command = + odometry_drift_simulator_.convertDriftedToGroundTruthPose( + T_drift_command); OdometryDriftSimulator::Transformation::Position t_gt_current_position = odometry_drift_simulator_.getGroundTruthPose().getPosition(); // Use position + yaw as setpoint auto command_pos = msg.position; auto command_ori = msg.orientation; + std::cout << "Pose Position In ROS Frame: " << command_pos.x << " " + << command_pos.y << " " << command_pos.z << std::endl; std::cout << "Pose Orientation In ROS Frame: " << command_ori.x << " " << command_ori.y << " " << command_ori.z << " " << command_ori.w << std::endl; + frame_converter_.rosToAirsim(&command_pos); frame_converter_.rosToAirsim(&command_ori); + std::cout << "Pose Position In Airsim Frame: " << command_pos.x << " " + << command_pos.y << " " << command_pos.z << std::endl; std::cout << "Pose Orientation In Airsim Frame: " << command_ori.x << " " << command_ori.y << " " << command_ori.z << " " << command_ori.w << std::endl; - double yaw = tf2::getYaw( - tf2::Quaternion(command_ori.x, command_ori.y, command_ori.z, - command_ori.w)); // Eigen's eulerAngles apparently - // messes up some wrap arounds or direcions and gets the wrong yaws in some - // cases - // config_.velocity =0.2f; - yaw = yaw / M_PI * 180.0; - constexpr double kMinMovingDistance = 0.1; // m - airsim_move_client_.cancelLastTask(); + + // double yaw = tf2::getYaw( + // tf2::Quaternion(command_ori.x, command_ori.y, command_ori.z, + // command_ori.w)); // Eigen's eulerAngles apparently + // // messes up some wrap arounds or direcions and gets the wrong yaws in some + // // cases Eigen::Vector3f command_pos_eigen(command_pos.x, command_pos.y, command_pos.z); - Eigen::Vector3f gt_curr_pos_eigen(t_gt_current_position[0], - t_gt_current_position[1], - t_gt_current_position[2]); Eigen::Quaternion command_ori_eigen(float(command_ori.w), float(command_ori.x), float(command_ori.y), float(command_ori.z)); - std::cout << "Old Pose: " << command_pos_eigen << std::endl; - std::cout << "New Pose: " << gt_curr_pos_eigen << std::endl; - std::cout << "Norm: " << (command_pos_eigen - gt_curr_pos_eigen).norm() << std::endl; - std::cout << "kMinMovingDistance: " << kMinMovingDistance << std::endl; - if ((command_pos_eigen - gt_curr_pos_eigen).norm() >= kMinMovingDistance) { - frame_converter_.rosToAirsim(&command_pos); - auto yaw_mode = msr::airlib::YawMode(false, yaw); - - airsim_move_client_.simSetVehiclePose(Pose(command_pos_eigen, command_ori_eigen), - true, - "airsim_drone"); - std::cout << "Moving to target position with pose " << command_pos_eigen - << " and orientation " << command_ori_eigen - << std::endl; - } else { - // This second command catches the case if the total distance is too small, - // where the moveToPosition command returns without satisfying the yaw. If - // this is always run then apparently sometimes the move command is - // overwritten. - airsim_move_client_.rotateToYawAsync(yaw, 3600, 5, config_.vehicle_name); - std::cout << "No moving, only rotating" << std::endl; - } + airsim_move_client_.cancelLastTask(); + airsim_move_client_.simSetVehiclePose(Pose(command_pos_eigen, command_ori_eigen), + true, + "airsim_drone"); // Trigger image collection - sensor_timers_.front()->getImages(); // which sensor timer ends up getting chosen here? + sensor_timers_.front()->getImages(); } void AirsimSimulator::startupCallback(const ros::TimerEvent&) {