From c789c5745d4b6b90982d900298afbae71ea2aa42 Mon Sep 17 00:00:00 2001 From: Natalie Rakoski Date: Sat, 30 Jan 2021 18:38:29 -0500 Subject: [PATCH 01/12] added code to create csvs for testing --- .../sensor/ROSAltimeter/ROSAltimeter.h | 5 + .../plugins/sensor/ROSCompass/ROSCompass.h | 5 + missions/quad-airsim-ex1.xml | 155 ++++++++++++++---- .../sensor/ROSAltimeter/ROSAltimeter.cpp | 33 ++++ src/plugins/sensor/ROSCompass/ROSCompass.cpp | 29 ++++ 5 files changed, 192 insertions(+), 35 deletions(-) diff --git a/include/scrimmage/plugins/sensor/ROSAltimeter/ROSAltimeter.h b/include/scrimmage/plugins/sensor/ROSAltimeter/ROSAltimeter.h index 05e88f8a4b..a04eff8b05 100644 --- a/include/scrimmage/plugins/sensor/ROSAltimeter/ROSAltimeter.h +++ b/include/scrimmage/plugins/sensor/ROSAltimeter/ROSAltimeter.h @@ -35,6 +35,8 @@ #include #include #include +#include +#include #include #include @@ -52,6 +54,7 @@ class ROSAltimeter : public scrimmage::Sensor { ROSAltimeter(); void init(std::map ¶ms) override; bool step() override; + void close(double t) override; protected: std::string vehicle_name_ = "none"; @@ -59,6 +62,8 @@ class ROSAltimeter : public scrimmage::Sensor { std::shared_ptr nh_; ros::Publisher altimeter_pub_; float monotonic_ = 0.0; + double prev_time_ = 0.0; + scrimmage::CSV csv; private: }; diff --git a/include/scrimmage/plugins/sensor/ROSCompass/ROSCompass.h b/include/scrimmage/plugins/sensor/ROSCompass/ROSCompass.h index 5363f34fa3..a981cb4bc6 100644 --- a/include/scrimmage/plugins/sensor/ROSCompass/ROSCompass.h +++ b/include/scrimmage/plugins/sensor/ROSCompass/ROSCompass.h @@ -35,6 +35,8 @@ #include #include #include +#include +#include #include #include @@ -50,12 +52,15 @@ class ROSCompass : public scrimmage::Sensor { ROSCompass(); void init(std::map ¶ms) override; bool step() override; + void close(double t) override; protected: std::string vehicle_name_ = "none"; std::string ros_namespace_; std::shared_ptr nh_; ros::Publisher compass_pub_; + double prev_time_ = 0.0; + scrimmage::CSV csv; private: }; diff --git a/missions/quad-airsim-ex1.xml b/missions/quad-airsim-ex1.xml index 9385ff6edc..628c1d5a11 100644 --- a/missions/quad-airsim-ex1.xml +++ b/missions/quad-airsim-ex1.xml @@ -3,11 +3,11 @@ - + start_paused="false"/> false @@ -66,7 +66,7 @@ 0.0 0.0 - 14.0 + 10.0 0 @@ -88,21 +88,12 @@ save_airsim_data="true" get_image_data="true" get_lidar_data="true" - get_imu_data="true" + get_imu_data="false" image_acquisition_period="0.33" lidar_acquisition_period="0.1" imu_acquisition_period="0.1">AirSimSensor - - - - - - - - - @@ -115,19 +106,27 @@ - - - - - - - + ROSAirSim + + + + + ROSAltimeter + ROSCompass + ROSIMUSensor + - Straight + Straight - 4.0 0.0 10.0 @@ -177,19 +175,98 @@ save_airsim_data="true" get_image_data="true" get_lidar_data="true" - get_imu_data="true" + get_imu_data="false" image_acquisition_period="0.33" lidar_acquisition_period="0.1" imu_acquisition_period="0.1">AirSimSensor + + + + + + + + + + + + + ROSAirSim + + - - - + ROSAltimeter + ROSCompass + ROSIMUSensor + + + + + Straight + + DoubleIntegrator + DoubleIntegratorControllerVelYaw + iris + + + + + + 3 + 77 77 255 + 1 + 1 + 1 + + + + + + 8.0 + 0.0 + 10.0 + + 0 + + + + + + + + + + + + + + + + AirSimSensor @@ -204,19 +281,27 @@ - - - - - - - + ROSAirSim + + + + + ROSAltimeter + ROSCompass + ROSIMUSensor + - Straight + Straight ¶ms) { // Create Publisher altimeter_pub_ = nh_->advertise(ros_namespace_ + "/altimeter", 1); + // Open imu_data CSV for append (app) and set column headers + std::string csv_filename = parent_->mp()->log_dir() + "/altimeter_data_robot" + std::to_string(parent_->id().id()) + ".csv"; + if (!csv.open_output(csv_filename, std::ios_base::app)) std::cout << "Couldn't create csv file" << endl; + if (!csv.output_is_open()) cout << "File isn't open. Can't write to CSV" << endl; + + csv.set_column_headers("time, dt, monotonic, amsl, local, relative, terrain, bottom_clearance"); + + // Scrimmage is in East North Up (ENU) // For you to get the ROS data in North East Down (NED): switch x and y, and negate z outside scrimmage. // For more information on the MavROS message format view this page: @@ -92,6 +100,7 @@ void ROSAltimeter::init(std::map ¶ms) { // consistent within a flight. The recommended value for this field is the uncorrected barometric altitude // at boot time. This altitude will also drift and vary between flights. sc::StatePtr &state = parent_->state_truth(); + prev_time_ = time_->t(); double lat_init, lon_init, alt_init; parent_->projection()->Reverse(state->pos()(0), state->pos()(1), state->pos()(2), lat_init, lon_init, alt_init); monotonic_ = static_cast(alt_init); @@ -100,6 +109,9 @@ void ROSAltimeter::init(std::map ¶ms) { bool ROSAltimeter::step() { // Obtain current state information sc::StatePtr &state = parent_->state_truth(); + double time_now = time_->t(); + double dt = time_now - prev_time_; + prev_time_ = time_now; // Scrimmage is in East North Up (ENU) // For you to get the ROS data in North East Down (NED): switch x and y, and negate z outside scrimmage. @@ -158,7 +170,28 @@ bool ROSAltimeter::step() { // Publish Altimeter information altimeter_pub_.publish(alt_msg); + // Write Altimeter data to CSV + // Write the CSV file to the root log directory file name = imu_data.csv + // "time, dt, monotonic, amsl, local, relative, terrain, bottom_clearance" + if (!csv.output_is_open()) { + cout << "File isn't open. Can't append to CSV" << endl; + } + csv.append(sc::CSV::Pairs{ + {"time", time_now}, + {"dt", dt}, + {"monotonic", alt_msg.monotonic}, + {"amsl", alt_msg.amsl}, + {"local", alt_msg.local}, + {"relative", alt_msg.relative}, + {"terrain", alt_msg.terrain}, + {"bottom_clearance", alt_msg.bottom_clearance}}, + true, true); + return true; } + +void ROSAltimeter::close(double t){ + csv.close_output(); +} } // namespace sensor } // namespace scrimmage diff --git a/src/plugins/sensor/ROSCompass/ROSCompass.cpp b/src/plugins/sensor/ROSCompass/ROSCompass.cpp index 9b72d25406..00b733a95f 100644 --- a/src/plugins/sensor/ROSCompass/ROSCompass.cpp +++ b/src/plugins/sensor/ROSCompass/ROSCompass.cpp @@ -75,11 +75,21 @@ void ROSCompass::init(std::map ¶ms) { // Create Publisher compass_pub_ = nh_->advertise(ros_namespace_ + "/compass", 1); + + // Open imu_data CSV for append (app) and set column headers + std::string csv_filename = parent_->mp()->log_dir() + "/compass_data_robot" + std::to_string(parent_->id().id()) + ".csv"; + if (!csv.open_output(csv_filename, std::ios_base::app)) std::cout << "Couldn't create csv file" << endl; + if (!csv.output_is_open()) cout << "File isn't open. Can't write to CSV" << endl; + + csv.set_column_headers("time, dt, mag_field_x, mag_field_y, mag_field_z"); } bool ROSCompass::step() { // Obtain current state information sc::StatePtr &state = parent_->state_truth(); + double time_now = time_->t(); + double dt = time_now - prev_time_; + prev_time_ = time_now; // Scrimmage is in ENU so all outputs are in ENU (East North Up). @@ -102,7 +112,26 @@ bool ROSCompass::step() { // Publish Compass information compass_pub_.publish(compass_msg); + // Write Altimeter data to CSV + // Write the CSV file to the root log directory file name = imu_data.csv + // "time, dt, monotonic, amsl, local, relative, terrain, bottom_clearance" + if (!csv.output_is_open()) { + cout << "File isn't open. Can't append to CSV" << endl; + } + csv.append(sc::CSV::Pairs{ + {"time", time_now}, + {"dt", dt}, + {"mag_field_x", compass_msg.magnetic_field.x}, + {"mag_field_y", compass_msg.magnetic_field.y}, + {"mag_field_z", compass_msg.magnetic_field.z}}, + true, true); + return true; } + +void ROSCompass::close(double t){ + csv.close_output(); +} + } // namespace sensor } // namespace scrimmage From 1e6955816d80b2b13fe1a0ba5520062fd8a07f9d Mon Sep 17 00:00:00 2001 From: Natalie Rakoski Date: Tue, 16 Mar 2021 01:32:46 -0400 Subject: [PATCH 02/12] add VO initialization manuever --- .../sensor/AirSimSensor/AirSimSensor.h | 7 + .../sensor/AirSimSensor/AirSimSensor.xml | 2 + missions/quad-airsim-ex1.xml | 54 +++--- .../sensor/AirSimSensor/AirSimSensor.cpp | 157 ++++++++++++++++-- 4 files changed, 183 insertions(+), 37 deletions(-) diff --git a/include/scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.h b/include/scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.h index 37bce8ec10..93cf660f2a 100644 --- a/include/scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.h +++ b/include/scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.h @@ -33,7 +33,9 @@ #define INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_AIRSIMSENSOR_AIRSIMSENSOR_H_ #include +#include #include +#include #include #include @@ -189,6 +191,11 @@ class AirSimSensor : public scrimmage::Sensor { bool get_image_data_ = true; bool get_lidar_data_ = true; bool get_imu_data_ = true; + bool use_init_maneuver_ = true; + bool init_maneuver_finished_ = false; + std::deque get_init_maneuver_positions(); + std::deque man_positions_; + // period at which the data acquisition is run [seconds] // double data_acquisition_period_ = .1; double image_acquisition_period_ = .1; diff --git a/include/scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.xml b/include/scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.xml index ee968aee95..b703b7f901 100644 --- a/include/scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.xml +++ b/include/scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.xml @@ -66,6 +66,8 @@ + + true true diff --git a/missions/quad-airsim-ex1.xml b/missions/quad-airsim-ex1.xml index c3695235d7..68480e2d84 100644 --- a/missions/quad-airsim-ex1.xml +++ b/missions/quad-airsim-ex1.xml @@ -83,15 +83,18 @@ + + AirSimSensor + imu_acquisition_period="0.1" + use_init_maneuver="true">AirSimSensor @@ -100,9 +103,9 @@ - - - + ROSAltimeter + ROSCompass + ROSIMUSensor @@ -117,13 +120,13 @@ - - - - - - - + ROSAirSim @@ -174,15 +177,18 @@ + + AirSimSensor + imu_acquisition_period="0.1" + use_init_maneuver="true">AirSimSensor @@ -191,9 +197,9 @@ - - - + ROSAltimeter + ROSCompass + ROSIMUSensor @@ -208,13 +214,13 @@ - - - - - - - + ROSAirSim diff --git a/src/plugins/sensor/AirSimSensor/AirSimSensor.cpp b/src/plugins/sensor/AirSimSensor/AirSimSensor.cpp index 607227e4ff..f0333e8c7a 100644 --- a/src/plugins/sensor/AirSimSensor/AirSimSensor.cpp +++ b/src/plugins/sensor/AirSimSensor/AirSimSensor.cpp @@ -41,8 +41,6 @@ #include #include #include -#include -#include #include #include @@ -209,6 +207,102 @@ void AirSimSensor::parse_imu_configs(std::map ¶ms) } } +std::deque AirSimSensor::get_init_maneuver_positions() { + + std::deque man_positions; + + // get the starting position + sc::StatePtr &state = parent_->state_truth(); + State init_state(*state); + State current_state(*state); + man_positions.push_back(current_state); + double delta = 0.1; + + // Rise up to 5 meters above the starting position + for (double i=0.0; i<5.0; i+=delta) { + current_state.pos()(2) = current_state.pos()(2) + delta; + man_positions.push_back(current_state); + } + + // repeat the below right-left movements 3 times + for (int repeat = 0; repeat<3; repeat++){ + // move 2 meters to the right, back to the center + for (double i=0.0; i<2.0; i+=delta) { + current_state.pos()(1) = current_state.pos()(1) + delta;; + man_positions.push_back(current_state); + } + for (double i=0.0; i<2.0; i+=delta) { + current_state.pos()(1) = current_state.pos()(1) - delta; + man_positions.push_back(current_state); + } + // move 2 meters to the left, back to the center + for (double i=0.0; i<2.0; i+=delta) { + current_state.pos()(1) = current_state.pos()(1) - delta; + man_positions.push_back(current_state); + } + for (double i=0.0; i<2.0; i+=delta) { + current_state.pos()(1) = current_state.pos()(1) + delta; + man_positions.push_back(current_state); + } + } + + // repeat the below up-down movements 3 times + for (int repeat = 0; repeat<3; repeat++){ + // move 1 meter up, back to the center + for (double i=0.0; i<1.0; i+=delta) { + current_state.pos()(2) = current_state.pos()(2) + delta; + man_positions.push_back(current_state); + } + for (double i=0.0; i<1.0; i+=delta) { + current_state.pos()(2) = current_state.pos()(2) - delta; + man_positions.push_back(current_state); + } + // move 1 meter down, back to the center + for (double i=0.0; i<1.0; i+=delta) { + current_state.pos()(2) = current_state.pos()(2) - delta; + man_positions.push_back(current_state); + } + for (double i=0.0; i<1.0; i+=delta) { + current_state.pos()(2) = current_state.pos()(2) + delta; + man_positions.push_back(current_state); + } + } + + // move in a 4mx2m rectangle parallel to the ground 3 times + for (int repeat = 0; repeat<3; repeat++){ + // move 1 meter in the +y direction + for (double i=0.0; i<2.0; i+=delta) { + current_state.pos()(1) = current_state.pos()(1) + delta; + man_positions.push_back(current_state); + } + // move 1 meter in the +x direction + for (double i=0.0; i<2.0; i+=delta) { + current_state.pos()(0) = current_state.pos()(0) + delta; + man_positions.push_back(current_state); + } + // move 2 meters in the -y direction + for (double i=0.0; i<4.0; i+=delta) { + current_state.pos()(1) = current_state.pos()(1) - delta; + man_positions.push_back(current_state); + } + // move 1 meters in the -x direction + for (double i=0.0; i<2.0; i+=delta) { + current_state.pos()(0) = current_state.pos()(0) - delta; + man_positions.push_back(current_state); + } + // move 1 meter in the +y direction + for (double i=0.0; i<2.0; i+=delta) { + current_state.pos()(1) = current_state.pos()(1) + delta; + man_positions.push_back(current_state); + } + } + + // place back at beginning for Straight Plugin + man_positions.push_back(init_state); + + return man_positions; +} + void AirSimSensor::init(std::map ¶ms) { // RPC / AirSim.xml file airsim_ip_ = sc::get("airsim_ip", params, "localhost"); @@ -230,6 +324,13 @@ void AirSimSensor::init(std::map ¶ms) { lidar_acquisition_period_ = sc::get("lidar_acquisition_period", params, 0.1); imu_acquisition_period_ = sc::get("imu_acquisition_period", params, 0.1); + // If using an initialization maneuver, then compile the position vector + use_init_maneuver_ = sc::get("use_init_maneuver", params, "true"); + if (use_init_maneuver_) { + man_positions_ = get_init_maneuver_positions(); + cout << "[AirSimSensor] Start VO Initialization Maneuver." << endl; + } + // Open airsim_data CSV for append (app) and set column headers std::string csv_filename = @@ -622,20 +723,50 @@ bool AirSimSensor::step() { // Setup state information for AirSim sc::StatePtr &state = parent_->state_truth(); - // convert from ENU to NED frame - ma::Vector3r pos(state->pos()(1), state->pos()(0), -state->pos()(2)); - enu_to_ned_yaw_.set_angle(ang::rad2deg(state->quat().yaw())); - double airsim_yaw_rad = ang::deg2rad(enu_to_ned_yaw_.angle()); + if (man_positions_.size() == 0){ + init_maneuver_finished_ = true; + } + + if (!init_maneuver_finished_ && use_init_maneuver_) { + + // Pop a state off the front of man_positions_ vector and delete the element + State next_state = man_positions_.front(); + man_positions_.pop_front(); + + // convert from ENU to NED frame + ma::Vector3r pos(next_state.pos()(1), next_state.pos()(0), -next_state.pos()(2)); + + enu_to_ned_yaw_.set_angle(ang::rad2deg(next_state.quat().yaw())); + double airsim_yaw_rad = ang::deg2rad(enu_to_ned_yaw_.angle()); + + // pitch, roll, yaw + // note, the negative pitch and yaw are required because of the wsu coordinate frame + ma::Quaternionr qd = ma::VectorMath::toQuaternion(-next_state.quat().pitch(), + next_state.quat().roll(), + airsim_yaw_rad); - // pitch, roll, yaw - // note, the negative pitch and yaw are required because of the wsu coordinate frame - ma::Quaternionr qd = ma::VectorMath::toQuaternion(-state->quat().pitch(), - state->quat().roll(), - airsim_yaw_rad); + // Send state information to AirSim + sim_client_->simSetVehiclePose(ma::Pose(pos, qd), true, vehicle_name_); - // Send state information to AirSim - sim_client_->simSetVehiclePose(ma::Pose(pos, qd), true, vehicle_name_); + } else { + // Else go through the motions of the autonomy plugin defined in the mission file. + + // convert from ENU to NED frame + ma::Vector3r pos(state->pos()(1), state->pos()(0), -state->pos()(2)); + + enu_to_ned_yaw_.set_angle(ang::rad2deg(state->quat().yaw())); + double airsim_yaw_rad = ang::deg2rad(enu_to_ned_yaw_.angle()); + + // pitch, roll, yaw + // note, the negative pitch and yaw are required because of the wsu coordinate frame + ma::Quaternionr qd = ma::VectorMath::toQuaternion(-state->quat().pitch(), + state->quat().roll(), + airsim_yaw_rad); + + // Send state information to AirSim + sim_client_->simSetVehiclePose(ma::Pose(pos, qd), true, vehicle_name_); + } // Get the camera images from the other thread if (get_image_data_) { From 565fae42d58f8a0a741c32f3d6a41a48e6e2cb73 Mon Sep 17 00:00:00 2001 From: Natalie Rakoski Date: Tue, 16 Mar 2021 04:47:31 -0400 Subject: [PATCH 03/12] init manuever working, but needs autonomy reset --- .../sensor/AirSimSensor/AirSimSensor.h | 4 +- .../sensor/AirSimSensor/AirSimSensor.cpp | 115 ++++++++++++++++-- 2 files changed, 105 insertions(+), 14 deletions(-) diff --git a/include/scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.h b/include/scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.h index 93cf660f2a..c87440277a 100644 --- a/include/scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.h +++ b/include/scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.h @@ -134,7 +134,7 @@ class AirSimSensor : public scrimmage::Sensor { protected: std::string vehicle_name_ = "none"; - bool save_data(MessagePtr>& im_msg, StatePtr& state, int frame_num); + bool save_data(MessagePtr>& im_msg, State state, int frame_num); scrimmage::CSV csv; int airsim_frame_num_ = 0; @@ -194,7 +194,9 @@ class AirSimSensor : public scrimmage::Sensor { bool use_init_maneuver_ = true; bool init_maneuver_finished_ = false; std::deque get_init_maneuver_positions(); + void complete_init_maneuver(); std::deque man_positions_; + StatePtr init_state_; // period at which the data acquisition is run [seconds] // double data_acquisition_period_ = .1; diff --git a/src/plugins/sensor/AirSimSensor/AirSimSensor.cpp b/src/plugins/sensor/AirSimSensor/AirSimSensor.cpp index f0333e8c7a..37f8aea585 100644 --- a/src/plugins/sensor/AirSimSensor/AirSimSensor.cpp +++ b/src/plugins/sensor/AirSimSensor/AirSimSensor.cpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include @@ -216,7 +217,7 @@ std::deque AirSimSensor::get_init_maneuver_positions() { State init_state(*state); State current_state(*state); man_positions.push_back(current_state); - double delta = 0.1; + double delta = 0.01; // Rise up to 5 meters above the starting position for (double i=0.0; i<5.0; i+=delta) { @@ -681,6 +682,62 @@ void AirSimSensor::request_imu() { } } +//void AirSimSensor::complete_init_maneuver(){ +// +// while(man_positions_.size() != 0){ +// // Pop a state off the front of man_positions_ vector and delete the element +// State next_state = man_positions_.front(); +// man_positions_.pop_front(); +// +// // convert from ENU to NED frame +// ma::Vector3r pos(next_state.pos()(1), next_state.pos()(0), -next_state.pos()(2)); +// +// enu_to_ned_yaw_.set_angle(ang::rad2deg(next_state.quat().yaw())); +// double airsim_yaw_rad = ang::deg2rad(enu_to_ned_yaw_.angle()); +// +// // pitch, roll, yaw +// // note, the negative pitch and yaw are required because of the wsu coordinate frame +// ma::Quaternionr qd = ma::VectorMath::toQuaternion(-next_state.quat().pitch(), +// next_state.quat().roll(), +// airsim_yaw_rad); +// +// // Send state information to AirSim +// sim_client_->simSetVehiclePose(ma::Pose(pos, qd), true, vehicle_name_); +// +// // Get the camera images from the other thread +// if (get_image_data_) { +// // sc::MessagePtr> im_msg_step; +// // auto im_msg_step = std::make_shared>>(); // NOLINT +// +// new_image_mutex_.lock(); +// bool new_image = new_image_; +// new_image_ = false; +// new_image_mutex_.unlock(); +// +// img_msg_mutex_.lock(); +// // im_msg_step = img_msg_; +// std::shared_ptr>> im_msg_step = img_msg_; +// // sc::MessagePtr> im_msg_step = img_msg_; +// img_msg_mutex_.unlock(); +// +// // If image is new, publish +// if (new_image) { +// if (save_airsim_data_) { +// AirSimSensor::save_data(im_msg_step, next_state, airsim_frame_num_); +// } +// img_pub_->publish(im_msg_step); +// } +// } +// // increment frame num so we do not save over the images +// airsim_frame_num_++; +// +// // sleep +// std::this_thread::sleep_for(std::chrono::milliseconds(1)); +// } // end while man_positions_ is not empty +// +// init_maneuver_finished_ = true; +//} + bool AirSimSensor::step() { /////////////////////////////////////////////////////////////////////////// /// Client Connection / Disconnection Handling @@ -724,12 +781,29 @@ bool AirSimSensor::step() { // Setup state information for AirSim sc::StatePtr &state = parent_->state_truth(); - if (man_positions_.size() == 0){ + // record init state + if (airsim_frame_num_ == 0){ + init_state_ = parent_->state_truth(); + } + + State next_state(*state); + + if (man_positions_.size() == 0) { init_maneuver_finished_ = true; + // reset to the original state + // parent_->state_truth() = init_state_; + std::vector autos = parent_->autonomies(); + // autos.front()->set_desired_state(init_state_); + for (auto a : autos) { + // (*a).set_desired_state(init_state_); + // autos.front()->set_desired_state(init_state_); + a->set_desired_state(init_state_); + } } if (!init_maneuver_finished_ && use_init_maneuver_) { - + //MissionParsePtr mp_->pause(); + // complete_init_maneuver(); // Pop a state off the front of man_positions_ vector and delete the element State next_state = man_positions_.front(); man_positions_.pop_front(); @@ -750,8 +824,7 @@ bool AirSimSensor::step() { sim_client_->simSetVehiclePose(ma::Pose(pos, qd), true, vehicle_name_); } else { - // Else go through the motions of the autonomy plugin defined in the mission file. - + // Go through the motions of the autonomy plugin defined in the mission file. // convert from ENU to NED frame ma::Vector3r pos(state->pos()(1), state->pos()(0), -state->pos()(2)); @@ -768,6 +841,22 @@ bool AirSimSensor::step() { sim_client_->simSetVehiclePose(ma::Pose(pos, qd), true, vehicle_name_); } +// // Go through the motions of the autonomy plugin defined in the mission file. +// // convert from ENU to NED frame +// ma::Vector3r pos(state->pos()(1), state->pos()(0), -state->pos()(2)); +// +// enu_to_ned_yaw_.set_angle(ang::rad2deg(state->quat().yaw())); +// double airsim_yaw_rad = ang::deg2rad(enu_to_ned_yaw_.angle()); +// +// // pitch, roll, yaw +// // note, the negative pitch and yaw are required because of the wsu coordinate frame +// ma::Quaternionr qd = ma::VectorMath::toQuaternion(-state->quat().pitch(), +// state->quat().roll(), +// airsim_yaw_rad); +// +// // Send state information to AirSim +// sim_client_->simSetVehiclePose(ma::Pose(pos, qd), true, vehicle_name_); + // Get the camera images from the other thread if (get_image_data_) { // sc::MessagePtr> im_msg_step; @@ -787,7 +876,7 @@ bool AirSimSensor::step() { // If image is new, publish if (new_image) { if (save_airsim_data_) { - AirSimSensor::save_data(im_msg_step, state, airsim_frame_num_); + AirSimSensor::save_data(im_msg_step, next_state, airsim_frame_num_); } img_pub_->publish(im_msg_step); } @@ -837,7 +926,7 @@ bool AirSimSensor::step() { return true; } -bool AirSimSensor::save_data(MessagePtr>& im_msg, sc::StatePtr& state, int frame_num) { +bool AirSimSensor::save_data(MessagePtr>& im_msg, State state, int frame_num) { // Get timestamp double time_now = time_->t(); @@ -866,12 +955,12 @@ bool AirSimSensor::save_data(MessagePtr>& im_msg, s csv.append(sc::CSV::Pairs{ {"frame", frame_num}, {"t", time_now}, - {"x", state->pos()(0)}, - {"y", state->pos()(1)}, - {"z", state->pos()(2)}, - {"roll", state->quat().roll()}, - {"pitch", state->quat().pitch()}, - {"yaw", state->quat().yaw()}}, true, true); + {"x", state.pos()(0)}, + {"y", state.pos()(1)}, + {"z", state.pos()(2)}, + {"roll", state.quat().roll()}, + {"pitch", state.quat().pitch()}, + {"yaw", state.quat().yaw()}}, true, true); return true; } From 26f0ac393f4579b7e328bde4b7186e1ed90d8e7d Mon Sep 17 00:00:00 2001 From: Natalie Rakoski Date: Mon, 19 Apr 2021 00:25:45 -0400 Subject: [PATCH 04/12] Added square plugin: square flight pattern works, still working on init_maneuver --- .../plugins/autonomy/Square/Square.h | 105 +++ .../plugins/autonomy/Square/Square.xml | 12 + missions/ALT-PNT-Chain-mission.xml | 676 ++++++++++++++++++ missions/quad-airsim-ex1.xml | 39 +- src/plugins/autonomy/Square/CMakeLists.txt | 42 ++ src/plugins/autonomy/Square/Square.cpp | 473 ++++++++++++ 6 files changed, 1341 insertions(+), 6 deletions(-) create mode 100644 include/scrimmage/plugins/autonomy/Square/Square.h create mode 100644 include/scrimmage/plugins/autonomy/Square/Square.xml create mode 100644 missions/ALT-PNT-Chain-mission.xml create mode 100644 src/plugins/autonomy/Square/CMakeLists.txt create mode 100644 src/plugins/autonomy/Square/Square.cpp diff --git a/include/scrimmage/plugins/autonomy/Square/Square.h b/include/scrimmage/plugins/autonomy/Square/Square.h new file mode 100644 index 0000000000..1be6688832 --- /dev/null +++ b/include/scrimmage/plugins/autonomy/Square/Square.h @@ -0,0 +1,105 @@ +/*! + * @file + * + * @section LICENSE + * + * Copyright (C) 2017 by the Georgia Tech Research Institute (GTRI) + * + * This file is part of SCRIMMAGE. + * + * SCRIMMAGE is free software: you can redistribute it and/or modify it under + * the terms of the GNU Lesser General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * SCRIMMAGE is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public + * License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with SCRIMMAGE. If not, see . + * + * @author Natalie Rakoski + * @author Kevin DeMarco + * @date 10 April 2021 + * @version 0.1.0 + * @brief Brief file description. + * @section DESCRIPTION + * A Long description goes here. + * + */ + +#ifndef INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_SQUARE_SQUARE_H_ +#define INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_SQUARE_SQUARE_H_ +#include +#include +#include + +#include + +#include +#include +#include + +namespace scrimmage { + +//namespace interaction { +//class BoundaryBase; +//} + +namespace autonomy { +class Square : public scrimmage::Autonomy{ + public: + void init(std::map ¶ms) override; + bool step_autonomy(double t, double dt) override; + + protected: + double speed_; + Eigen::Vector3d goal_; + Eigen::Vector3d goal1_; + Eigen::Vector3d goal2_; + Eigen::Vector3d goal3_; + Eigen::Vector3d goal4_; + + int frame_number_; + bool show_camera_images_; + bool save_camera_images_; + +// bool enable_boundary_control_ = false; +// std::shared_ptr boundary_; + + int desired_alt_idx_ = 0; + int desired_speed_idx_ = 0; + int desired_heading_idx_ = 0; + + scrimmage_proto::ShapePtr text_shape_; + scrimmage_proto::ShapePtr sphere_shape_; + + bool noisy_state_set_ = false; + State noisy_state_; + + ContactMap noisy_contacts_; + + double desired_z_ = 0; + +// PublisherPtr pub_gen_ents_; +// bool gen_ents_ = false; +// double prev_gen_time_ = -1.0; + + bool use_init_maneuver_ = true; + bool init_maneuver_finished_ = false; + std::deque get_init_maneuver_positions(); + std::deque man_positions_; + Eigen::Vector3d init_man_goal_pos_; + Quaternion init_man_goal_quat_; + + int square_side_ = 0; + double sq_side_length_m_ = 100; + Eigen::Vector3d init_goal_; + // StatePtr init_state_; + +}; +} // namespace autonomy +} // namespace scrimmage +#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_SQUARE_SQUARE_H_ diff --git a/include/scrimmage/plugins/autonomy/Square/Square.xml b/include/scrimmage/plugins/autonomy/Square/Square.xml new file mode 100644 index 0000000000..414774e7b6 --- /dev/null +++ b/include/scrimmage/plugins/autonomy/Square/Square.xml @@ -0,0 +1,12 @@ + + + + Square_plugin + 21 + false + false + + + false + 100 + diff --git a/missions/ALT-PNT-Chain-mission.xml b/missions/ALT-PNT-Chain-mission.xml new file mode 100644 index 0000000000..681e8bf5fe --- /dev/null +++ b/missions/ALT-PNT-Chain-mission.xml @@ -0,0 +1,676 @@ + + + + + + + false + + 50051 + localhost + + time, all_dead + + 10 + 1000 + + mcmillan + 191 191 191 + 10 + + false + all + false + + SimpleCollisionMetrics + + ~/.scrimmage/logs + + 35.721025 + -120.767925 + 300 + true + 10 + + SimpleCollision + ROSClockServer + + GlobalNetwork + LocalNetwork + + 2147483648 + + + + + + + 1 + 77 77 255 + 1 + 1 + 1 + + + + + + 0.0 + 0.0 + 20.0 + 0 + + + + + + + + + + AirSimSensor + + + + + + + ROSAirSim + + + + Straight + + DoubleIntegrator + DoubleIntegratorControllerVelYaw + iris + + ROSAltimeter + ROSCompass + ROSIMUSensor + + + + + + 2 + 77 77 255 + 1 + 1 + 1 + + + + + + 0.0 + 10.0 + 20.0 + + 0 + + + + + + + + + + AirSimSensor + + + + + + + ROSAirSim + + + + Straight + + DoubleIntegrator + DoubleIntegratorControllerVelYaw + iris + + ROSAltimeter + ROSCompass + ROSIMUSensor + + + + + 3 + 77 77 255 + 1 + 1 + 1 + + + + + + -10.0 + 0.0 + 20.0 + + 0 + + + + + + + + + + AirSimSensor + + + + + + + ROSAirSim + + + + Straight + + DoubleIntegrator + DoubleIntegratorControllerVelYaw + iris + + ROSAltimeter + ROSCompass + ROSIMUSensor + + + + + 4 + 77 77 255 + 1 + 1 + 1 + + + + + + -10.0 + 10.0 + 20.0 + + 0 + + + + + + + + + + AirSimSensor + + + + + + + ROSAirSim + + + + Straight + + DoubleIntegrator + DoubleIntegratorControllerVelYaw + iris + + ROSAltimeter + ROSCompass + ROSIMUSensor + + + + + 5 + 77 77 255 + 1 + 1 + 1 + + + + + + -20.0 + 0.0 + 20.0 + + 0 + + + + + + + + + + AirSimSensor + + + + + + + ROSAirSim + + + + Straight + + DoubleIntegrator + DoubleIntegratorControllerVelYaw + iris + + ROSAltimeter + ROSCompass + ROSIMUSensor + + + + + 6 + 77 77 255 + 1 + 1 + 1 + + + + + + -20.0 + 10.0 + 20.0 + + 0 + + + + + + + + + + AirSimSensor + + + + + + + ROSAirSim + + + + Straight + + DoubleIntegrator + DoubleIntegratorControllerVelYaw + iris + + ROSAltimeter + ROSCompass + ROSIMUSensor + + + + + 7 + 77 77 255 + 1 + 1 + 1 + + + + + + -30.0 + 0.0 + 20.0 + + 0 + + + + + + + + + + AirSimSensor + + + + + + + ROSAirSim + + + + Straight + + DoubleIntegrator + DoubleIntegratorControllerVelYaw + iris + + ROSAltimeter + ROSCompass + ROSIMUSensor + + + + + 8 + 77 77 255 + 1 + 1 + 1 + + + + + + -30.0 + 10.0 + 20.0 + + 0 + + + + + + + + + + AirSimSensor + + + + + + + ROSAirSim + + + + Straight + + DoubleIntegrator + DoubleIntegratorControllerVelYaw + iris + + ROSAltimeter + ROSCompass + ROSIMUSensor + + + + + 9 + 77 77 255 + 1 + 1 + 1 + + + + + + -40.0 + 0.0 + 20.0 + + 0 + + + + + + + + + + AirSimSensor + + + + + + + ROSAirSim + + + + Straight + + DoubleIntegrator + DoubleIntegratorControllerVelYaw + iris + + ROSAltimeter + ROSCompass + ROSIMUSensor + + + + + 10 + 77 77 255 + 1 + 1 + 1 + + + + + + -40.0 + 10.0 + 20.0 + + 0 + + + + + + + + + + AirSimSensor + + + + + + + ROSAirSim + + + + Straight + + DoubleIntegrator + DoubleIntegratorControllerVelYaw + iris + + ROSAltimeter + ROSCompass + ROSIMUSensor + + + + diff --git a/missions/quad-airsim-ex1.xml b/missions/quad-airsim-ex1.xml index 68480e2d84..eae0b19ac9 100644 --- a/missions/quad-airsim-ex1.xml +++ b/missions/quad-airsim-ex1.xml @@ -3,7 +3,7 @@ - true 10 + Boundary + SimpleCollision ROSClockServer @@ -94,7 +102,7 @@ image_acquisition_period="0.33" lidar_acquisition_period="0.1" imu_acquisition_period="0.1" - use_init_maneuver="true">AirSimSensor + use_init_maneuver="false">AirSimSensor @@ -128,11 +136,20 @@ ros_python="false" ros_cartographer="false">ROSAirSim - + + + + + + + - Straight + Square AirSimSensor + use_init_maneuver="false">AirSimSensor @@ -224,9 +241,19 @@ + + + + + + + - Straight + Square . + * + * @author Natalie Rakoski + * @author Kevin DeMarco + * @date 10 April 2021 + * @version 0.1.0 + * @brief Brief file description. + * @section DESCRIPTION + * A Long description goes here. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if ENABLE_OPENCV == 1 +#include +#include +#include +#include +#endif + +#if ENABLE_AIRSIM == 1 +#include +#endif + +#include + +namespace sc = scrimmage; +namespace sp = scrimmage_proto; +namespace sci = scrimmage::interaction; + +#include + +#define BOOST_NO_CXX11_SCOPED_ENUMS +#include +#undef BOOST_NO_CXX11_SCOPED_ENUMS +namespace fs = boost::filesystem; + +REGISTER_PLUGIN(scrimmage::Autonomy, + scrimmage::autonomy::Square, + Square_plugin) + +namespace scrimmage { +namespace autonomy { + +std::deque Square::get_init_maneuver_positions() { + + std::deque man_positions; + + // get the starting position + sc::StatePtr &state = parent_->state_truth(); + State init_state(*state); + State current_state(*state); + man_positions.push_back(current_state); +// double delta = 0.05; + double delta = 1.0; + + // Rise up to 5 meters above the starting position + for (double i=0.0; i<5.0; i+=delta) { + current_state.pos()(2) = current_state.pos()(2) + delta; + man_positions.push_back(current_state); + } + + // repeat the below right-left movements 1 times + for (int repeat = 0; repeat<1; repeat++){ + // move 2 meters to the right, back to the center + for (double i=0.0; i<5.0; i+=delta) { + current_state.pos()(1) = current_state.pos()(1) + delta;; + man_positions.push_back(current_state); + } + for (double i=0.0; i<5.0; i+=delta) { + current_state.pos()(1) = current_state.pos()(1) - delta; + man_positions.push_back(current_state); + } + // move 2 meters to the left, back to the center + for (double i=0.0; i<5.0; i+=delta) { + current_state.pos()(1) = current_state.pos()(1) - delta; + man_positions.push_back(current_state); + } + for (double i=0.0; i<5.0; i+=delta) { + current_state.pos()(1) = current_state.pos()(1) + delta; + man_positions.push_back(current_state); + } + } + + // repeat the below up-down movements 3 times + for (int repeat = 0; repeat<1; repeat++){ + // move 1 meter up, back to the center + for (double i=0.0; i<5.0; i+=delta) { + current_state.pos()(2) = current_state.pos()(2) + delta; + man_positions.push_back(current_state); + } + for (double i=0.0; i<5.0; i+=delta) { + current_state.pos()(2) = current_state.pos()(2) - delta; + man_positions.push_back(current_state); + } + // move 1 meter down, back to the center + for (double i=0.0; i<5.0; i+=delta) { + current_state.pos()(2) = current_state.pos()(2) - delta; + man_positions.push_back(current_state); + } + for (double i=0.0; i<5.0; i+=delta) { + current_state.pos()(2) = current_state.pos()(2) + delta; + man_positions.push_back(current_state); + } + } + +// // move in a 4mx2m rectangle parallel to the ground 3 times +// for (int repeat = 0; repeat<1; repeat++){ +// // move 1 meter in the +y direction +// for (double i=0.0; i<5.0; i+=delta) { +// current_state.pos()(1) = current_state.pos()(1) + delta; +// man_positions.push_back(current_state); +// } +// // move 1 meter in the +x direction +// for (double i=0.0; i<5.0; i+=delta) { +// current_state.pos()(0) = current_state.pos()(0) + delta; +// man_positions.push_back(current_state); +// } +// // move 2 meters in the -y direction +// for (double i=0.0; i<10.0; i+=delta) { +// current_state.pos()(1) = current_state.pos()(1) - delta; +// man_positions.push_back(current_state); +// } +// // move 1 meters in the -x direction +// for (double i=0.0; i<5.0; i+=delta) { +// current_state.pos()(0) = current_state.pos()(0) - delta; +// man_positions.push_back(current_state); +// } +// // move 1 meter in the +y direction +// for (double i=0.0; i<5.0; i+=delta) { +// current_state.pos()(1) = current_state.pos()(1) + delta; +// man_positions.push_back(current_state); +// } +// } + + // place back at beginning for Straight Plugin + man_positions.push_back(init_state); + + return man_positions; +} + +void Square::init(std::map ¶ms) { + speed_ = scrimmage::get("speed", params, 0.0); + show_camera_images_ = scrimmage::get("show_camera_images", params, false); + save_camera_images_ = scrimmage::get("save_camera_images", params, false); + sq_side_length_m_ = scrimmage::get("sq_side_length_m", params, 100); + + // record the initial state: + init_man_goal_pos_ = state_->pos(); + init_man_goal_quat_ = state_->quat(); + + // Project goal in front... +// Eigen::Vector3d rel_pos = Eigen::Vector3d::UnitX()*1e6; +// Eigen::Vector3d unit_vector = rel_pos.normalized(); +// unit_vector = state_->quat().rotate(unit_vector); +// goal_ = state_->pos() + unit_vector * rel_pos.norm(); +// goal_(2) = state_->pos()(2); + + cout << "speed: " << speed_ << endl; + + // initialize the corner locations of the square + // positve x direction + goal1_ = state_->pos(); + goal1_(0) = goal1_(0) + sq_side_length_m_; + cout << "goal1: " << goal1_ << endl; + // positve y direction + goal2_ = goal1_; + goal2_(1) = goal2_(1) + sq_side_length_m_; + cout << "goal2: " << goal2_ << endl; + // negative x direction + goal3_ = goal2_; + goal3_(0) = goal3_(0) - sq_side_length_m_; + cout << "goal3: " << goal3_ << endl; + // negative y direction + goal4_ = goal3_; + goal4_(1) = goal4_(1) - sq_side_length_m_; + cout << "goal4: " << goal4_ << endl; + + goal_ = goal1_; + + // Set the desired_z to our initial position. + // desired_z_ = state_->pos()(2); + init_goal_ = state_->pos(); + cout << "original init_goal_: " << init_goal_ << endl; + + // Register the desired_z parameter with the parameter server + auto param_cb = [&](const double &desired_z) { + std::cout << "desired_z param changed at: " << time_->t() + << ", with value: " << desired_z << endl; + }; + register_param("desired_z", goal_(2), param_cb); + + if (save_camera_images_) { + ///////////////////////////////////////////////////////// + // Remove all img files from previous run + if (fs::exists("./imgs")) { + fs::recursive_directory_iterator it("./imgs"); + fs::recursive_directory_iterator endit; + std::list paths_to_rm; + while (it != endit) { + fs::path path = it->path(); + if (fs::is_regular_file(*it) && path.extension() == ".png") { + paths_to_rm.push_back(path); + } + ++it; + } + for (fs::path p : paths_to_rm) { + fs::remove(p); + } + } else { + fs::create_directories("./imgs"); + } + ///////////////////////////////////////////////////////// + } + + frame_number_ = 0; + + // If using an initialization maneuver, then compile the position vector + use_init_maneuver_ = sc::get("use_init_maneuver", params, "false"); + if (use_init_maneuver_) { + man_positions_ = get_init_maneuver_positions(); + cout << "[Square] Start VO Initialization Maneuver." << endl; + } + +// enable_boundary_control_ = get("enable_boundary_control", params, false); +// +// auto bd_cb = [&](auto &msg) {boundary_ = sci::Boundary::make_boundary(msg->data);}; +// subscribe("GlobalNetwork", "Boundary", bd_cb); + + auto state_cb = [&](auto &msg) { + noisy_state_set_ = true; + noisy_state_ = msg->data; + }; + subscribe("LocalNetwork", "StateWithCovariance", state_cb); + + auto cnt_cb = [&](scrimmage::MessagePtr &msg) { + noisy_contacts_ = msg->data; // Save map of noisy contacts + }; + subscribe("LocalNetwork", "ContactsWithCovariances", cnt_cb); + +#if (ENABLE_OPENCV == 1 && ENABLE_AIRSIM == 1) + auto airsim_cb = [&](auto &msg) { + for (sc::sensor::AirSimImageType a : msg->data) { + if (show_camera_images_) { + // Get Camera Name + std::string window_name = a.vehicle_name + "_" + a.camera_config.cam_name + "_" + a.camera_config.img_type_name; + // for depth images CV imshow expects grayscale image values to be between 0 and 1. + if (a.camera_config.img_type_name == "DepthPerspective" || a.camera_config.img_type_name == "DepthPlanner") { + // Worked before building with ROS + cv::Mat tempImage; + a.img.convertTo(tempImage, CV_32FC1, 1.f/255); + // cv::normalize(a.img, tempImage, 0, 1, cv::NORM_MINMAX); + // cout << tempImage << endl; + cv::imshow(window_name, tempImage); + } else { + // other image types are int 0-255. + if (a.img.channels() == 4) { + cout << "image channels: " << a.img.channels() << endl; + cout << "Warning: Old AirSim Linux Asset Environments have 4 channels. Color images will not display correctly." << endl; + cout << "Warning: Use Asset Environment versions Linux-v1.3.1+." << endl; + cv::Mat tempImage; + cv::cvtColor(a.img , tempImage, CV_RGBA2RGB); + cv::imshow(window_name, tempImage); + } else { + cv::imshow(window_name, a.img); + } + } + cv::waitKey(1); + } + } + }; + subscribe>("LocalNetwork", "AirSimImages", airsim_cb); +#endif + +#if ENABLE_OPENCV == 1 + auto blob_cb = [&](auto &msg) { + if (save_camera_images_) { + std::string img_name = "./imgs/camera_" + + std::to_string(frame_number_++) + ".png"; + cv::imwrite(img_name, msg->data.frame); + } + + if (show_camera_images_) { + cv::imshow("Camera Sensor", msg->data.frame); + cv::waitKey(1); + } + }; + subscribe("LocalNetwork", "ContactBlobCamera", blob_cb); +#endif + +// gen_ents_ = sc::get("generate_entities", params, gen_ents_); +// if (gen_ents_) { +// pub_gen_ents_ = advertise("GlobalNetwork", "GenerateEntity"); +// } + + desired_alt_idx_ = vars_.declare(VariableIO::Type::desired_altitude, VariableIO::Direction::Out); + desired_speed_idx_ = vars_.declare(VariableIO::Type::desired_speed, VariableIO::Direction::Out); + desired_heading_idx_ = vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::Out); +} + +bool Square::step_autonomy(double t, double dt) { + + // Read data from sensors... + if (!noisy_state_set_) { + noisy_state_ = *state_; + } + +// if (boundary_ != nullptr && enable_boundary_control_) { +// if (!boundary_->contains(noisy_state_.pos())) { +// // Project goal through center of boundary +// Eigen::Vector3d center = boundary_->center(); +// center(2) = noisy_state_.pos()(2); // maintain altitude +// Eigen::Vector3d diff = center - noisy_state_.pos(); +// goal_ = noisy_state_.pos() + diff.normalized() * 1e6; +// } +// } + + Eigen::Vector3d diff; + Eigen::Vector3d v; + double altitude = goal_(2); + // double heading = init_man_goal_quat_.yaw(); + + + if (man_positions_.size() == 0 && use_init_maneuver_) { + init_maneuver_finished_ = true; + goal_ = init_goal_; + cout << "init maneuver finished" << endl; + } + + if (!init_maneuver_finished_ && use_init_maneuver_) { + + // Pop a state off the front of man_positions_ vector and delete the element + float distance = sqrt(pow((init_man_goal_pos_(0) - noisy_state_.pos()(0)), 2) + pow((init_man_goal_pos_(1) - noisy_state_.pos()(1)), 2)); + if (distance < 0.05) { + State next_state = man_positions_.front(); + man_positions_.pop_front(); + // cout << "remaining man_pos: " << man_positions_.size() << endl; + init_man_goal_pos_ = next_state.pos(); + init_man_goal_quat_ = next_state.quat(); + } + + cout << "remaining man_pos: " << man_positions_.size() << endl; + + cout << "next_state: " << init_man_goal_pos_ << endl; + // create desired goal and velocity + diff = init_man_goal_pos_ - noisy_state_.pos(); + v = 5.0 * diff.normalized(); + // heading = init_man_goal_quat_.yaw(); + altitude = init_man_goal_pos_(2); + + } else { + + // if init maneuver was used, start the drones at the starting point + if(square_side_ == 0) { + if(use_init_maneuver_) { + float distance = sqrt(pow((goal_(0) - noisy_state_.pos()(0)), 2) + pow((goal_(1) - noisy_state_.pos()(1)), 2)); + if (distance < 5) { + square_side_ += 1; + goal_ = goal1_; + } + } + else{ + square_side_ += 1; + } + } + + // check if we have completed this side + float distance = sqrt(pow((init_goal_(0) - state_->pos()(0)), 2) + pow((init_goal_(1) - state_->pos()(1)), 2)); +// cout << "distance: " << distance << endl; +// cout << "sq_side_length_m_: " << sq_side_length_m_ << endl; +// cout << "square_side_: " << square_side_ << endl; +// cout << "position: " << state_->pos()<< endl; + if (distance >= sq_side_length_m_ && square_side_ > 0){ + // update which side of the square we are on. + square_side_ += 1; + init_goal_ = goal_; + + // rotate the goal + int current_side = square_side_ % 4; + switch(current_side) { + case 1: + { + // cout << '1' << endl; // prints "1" + // Project goal in front... + // Move in positive X direction + goal_ = goal1_; + break; + } + case 2 : + { + // cout << '2' << endl; + // Project goal in front... + // Move in positive Y direction + goal_ = goal2_; + break; + } + case 3: + { + // cout << '3' << endl; + // Project goal in front... + // Move in negative X direction + goal_ = goal3_; + break; + } + case 0: + { + // cout << '4' << endl; + // Project goal in front... + // Move in negative Y direction + goal_ = goal4_; + break; + } + } + cout << "goal: " << goal_ << endl; + } + + // create desired goal and velocity + diff = goal_ - noisy_state_.pos(); + v = speed_ * diff.normalized(); + altitude = goal_(2); + // heading = Angles::angle_2pi(atan2(v(1), v(0))); + } + + /////////////////////////////////////////////////////////////////////////// + // Convert desired velocity to desired speed, heading, and pitch controls + /////////////////////////////////////////////////////////////////////////// + double heading = Angles::angle_2pi(atan2(v(1), v(0))); + vars_.output(desired_alt_idx_, altitude); + vars_.output(desired_speed_idx_, v.norm()); + vars_.output(desired_heading_idx_, heading); + + noisy_state_set_ = false; + return true; +} +} // namespace autonomy +} // namespace scrimmage From b9e335bc89ad004911a67478611148bf09758ff6 Mon Sep 17 00:00:00 2001 From: Natalie Rakoski Date: Thu, 22 Apr 2021 00:12:34 -0400 Subject: [PATCH 05/12] PD controller tuned for square maneuver, not init --- .../plugins/autonomy/Square/Square.h | 3 +- src/plugins/autonomy/Square/Square.cpp | 264 +++++++++++++----- 2 files changed, 190 insertions(+), 77 deletions(-) diff --git a/include/scrimmage/plugins/autonomy/Square/Square.h b/include/scrimmage/plugins/autonomy/Square/Square.h index 1be6688832..443d7b5a44 100644 --- a/include/scrimmage/plugins/autonomy/Square/Square.h +++ b/include/scrimmage/plugins/autonomy/Square/Square.h @@ -94,10 +94,11 @@ class Square : public scrimmage::Autonomy{ Eigen::Vector3d init_man_goal_pos_; Quaternion init_man_goal_quat_; - int square_side_ = 0; + int square_side_ = 1; double sq_side_length_m_ = 100; Eigen::Vector3d init_goal_; // StatePtr init_state_; + Eigen::Vector3d prev_diff_ = {0,0,0}; }; } // namespace autonomy diff --git a/src/plugins/autonomy/Square/Square.cpp b/src/plugins/autonomy/Square/Square.cpp index b7cafaed24..aa3fb5c43c 100644 --- a/src/plugins/autonomy/Square/Square.cpp +++ b/src/plugins/autonomy/Square/Square.cpp @@ -87,59 +87,76 @@ std::deque Square::get_init_maneuver_positions() { sc::StatePtr &state = parent_->state_truth(); State init_state(*state); State current_state(*state); - man_positions.push_back(current_state); + // man_positions.push_back(current_state); // double delta = 0.05; double delta = 1.0; // Rise up to 5 meters above the starting position - for (double i=0.0; i<5.0; i+=delta) { - current_state.pos()(2) = current_state.pos()(2) + delta; - man_positions.push_back(current_state); - } +// for (double i=0.0; i<5.0; i+=delta) { +// current_state.pos()(2) = current_state.pos()(2) + delta; +// man_positions.push_back(current_state); +// } + cout << "starting_state: " << current_state.pos() << endl; + current_state.pos()(2) = current_state.pos()(2) + 5.0; + man_positions.push_back(current_state); - // repeat the below right-left movements 1 times - for (int repeat = 0; repeat<1; repeat++){ - // move 2 meters to the right, back to the center - for (double i=0.0; i<5.0; i+=delta) { - current_state.pos()(1) = current_state.pos()(1) + delta;; - man_positions.push_back(current_state); - } - for (double i=0.0; i<5.0; i+=delta) { - current_state.pos()(1) = current_state.pos()(1) - delta; - man_positions.push_back(current_state); - } - // move 2 meters to the left, back to the center - for (double i=0.0; i<5.0; i+=delta) { - current_state.pos()(1) = current_state.pos()(1) - delta; - man_positions.push_back(current_state); - } - for (double i=0.0; i<5.0; i+=delta) { - current_state.pos()(1) = current_state.pos()(1) + delta; - man_positions.push_back(current_state); - } - } +// // repeat the below right-left movements 1 times +// for (int repeat = 0; repeat<1; repeat++){ +// // move 2 meters to the right, back to the center +// for (double i=0.0; i<5.0; i+=delta) { +// current_state.pos()(1) = current_state.pos()(1) + delta;; +// man_positions.push_back(current_state); +// } +// for (double i=0.0; i<5.0; i+=delta) { +// current_state.pos()(1) = current_state.pos()(1) - delta; +// man_positions.push_back(current_state); +// } +// // move 2 meters to the left, back to the center +// for (double i=0.0; i<5.0; i+=delta) { +// current_state.pos()(1) = current_state.pos()(1) - delta; +// man_positions.push_back(current_state); +// } +// for (double i=0.0; i<5.0; i+=delta) { +// current_state.pos()(1) = current_state.pos()(1) + delta; +// man_positions.push_back(current_state); +// } +// } - // repeat the below up-down movements 3 times - for (int repeat = 0; repeat<1; repeat++){ - // move 1 meter up, back to the center - for (double i=0.0; i<5.0; i+=delta) { - current_state.pos()(2) = current_state.pos()(2) + delta; - man_positions.push_back(current_state); - } - for (double i=0.0; i<5.0; i+=delta) { - current_state.pos()(2) = current_state.pos()(2) - delta; - man_positions.push_back(current_state); - } - // move 1 meter down, back to the center - for (double i=0.0; i<5.0; i+=delta) { - current_state.pos()(2) = current_state.pos()(2) - delta; - man_positions.push_back(current_state); - } - for (double i=0.0; i<5.0; i+=delta) { - current_state.pos()(2) = current_state.pos()(2) + delta; - man_positions.push_back(current_state); - } - } + current_state.pos()(1) = current_state.pos()(1) + 5.0; + man_positions.push_back(current_state); + current_state.pos()(1) = current_state.pos()(1) - 10.0; + man_positions.push_back(current_state); + current_state.pos()(1) = current_state.pos()(1) + 5.0; + man_positions.push_back(current_state); + +// // repeat the below up-down movements 3 times +// for (int repeat = 0; repeat<1; repeat++){ +// // move 1 meter up, back to the center +// for (double i=0.0; i<5.0; i+=delta) { +// current_state.pos()(2) = current_state.pos()(2) + delta; +// man_positions.push_back(current_state); +// } +// for (double i=0.0; i<5.0; i+=delta) { +// current_state.pos()(2) = current_state.pos()(2) - delta; +// man_positions.push_back(current_state); +// } +// // move 1 meter down, back to the center +// for (double i=0.0; i<5.0; i+=delta) { +// current_state.pos()(2) = current_state.pos()(2) - delta; +// man_positions.push_back(current_state); +// } +// for (double i=0.0; i<5.0; i+=delta) { +// current_state.pos()(2) = current_state.pos()(2) + delta; +// man_positions.push_back(current_state); +// } +// } + + current_state.pos()(2) = current_state.pos()(2) + 5.0; + man_positions.push_back(current_state); + current_state.pos()(2) = current_state.pos()(2) - 10.0; + man_positions.push_back(current_state); + current_state.pos()(2) = current_state.pos()(2) + 5.0; + man_positions.push_back(current_state); // // move in a 4mx2m rectangle parallel to the ground 3 times // for (int repeat = 0; repeat<1; repeat++){ @@ -170,8 +187,25 @@ std::deque Square::get_init_maneuver_positions() { // } // } + current_state.pos()(1) = current_state.pos()(1) + 5.0; + man_positions.push_back(current_state); + current_state.pos()(0) = current_state.pos()(0) + 5.0; + man_positions.push_back(current_state); + current_state.pos()(1) = current_state.pos()(1) - 10.0; + man_positions.push_back(current_state); + current_state.pos()(0) = current_state.pos()(0) - 5.0; + man_positions.push_back(current_state); + current_state.pos()(1) = current_state.pos()(1) + 5.0; + man_positions.push_back(current_state); + // place back at beginning for Straight Plugin man_positions.push_back(init_state); + man_positions.push_back(init_state); + man_positions.push_back(init_state); + + for (auto state : man_positions) { + cout << state.pos()(0) << ", " << state.pos()(1) << ", " << state.pos()(2) << "\n" << endl; + } return man_positions; } @@ -182,10 +216,6 @@ void Square::init(std::map ¶ms) { save_camera_images_ = scrimmage::get("save_camera_images", params, false); sq_side_length_m_ = scrimmage::get("sq_side_length_m", params, 100); - // record the initial state: - init_man_goal_pos_ = state_->pos(); - init_man_goal_quat_ = state_->quat(); - // Project goal in front... // Eigen::Vector3d rel_pos = Eigen::Vector3d::UnitX()*1e6; // Eigen::Vector3d unit_vector = rel_pos.normalized(); @@ -213,11 +243,12 @@ void Square::init(std::map ¶ms) { goal4_(1) = goal4_(1) - sq_side_length_m_; cout << "goal4: " << goal4_ << endl; - goal_ = goal1_; + // goal_ = goal1_; - // Set the desired_z to our initial position. + // Save the initial starting position // desired_z_ = state_->pos()(2); init_goal_ = state_->pos(); + noisy_state_ = *state_; cout << "original init_goal_: " << init_goal_ << endl; // Register the desired_z parameter with the parameter server @@ -259,6 +290,16 @@ void Square::init(std::map ¶ms) { cout << "[Square] Start VO Initialization Maneuver." << endl; } + if (use_init_maneuver_) { + State next_state = man_positions_.front(); + man_positions_.pop_front(); + init_man_goal_pos_ = next_state.pos(); + init_man_goal_quat_ = next_state.quat(); + // goal_ = man_positions_; + } else { + goal_ = goal1_; + } + // enable_boundary_control_ = get("enable_boundary_control", params, false); // // auto bd_cb = [&](auto &msg) {boundary_ = sci::Boundary::make_boundary(msg->data);}; @@ -357,49 +398,105 @@ bool Square::step_autonomy(double t, double dt) { double altitude = goal_(2); // double heading = init_man_goal_quat_.yaw(); + // cout << "HERE" << endl; if (man_positions_.size() == 0 && use_init_maneuver_) { init_maneuver_finished_ = true; - goal_ = init_goal_; + // goal_ = init_goal_; + goal_ = goal1_; + prev_diff_ = {0,0,0}; cout << "init maneuver finished" << endl; } if (!init_maneuver_finished_ && use_init_maneuver_) { // Pop a state off the front of man_positions_ vector and delete the element - float distance = sqrt(pow((init_man_goal_pos_(0) - noisy_state_.pos()(0)), 2) + pow((init_man_goal_pos_(1) - noisy_state_.pos()(1)), 2)); - if (distance < 0.05) { + float distance = sqrt(pow((init_man_goal_pos_(0) - noisy_state_.pos()(0)), 2) + pow((init_man_goal_pos_(1) - noisy_state_.pos()(1)), 2) + pow((init_man_goal_pos_(2) - noisy_state_.pos()(2)), 2)); + cout << "DISTANCE: " << distance << endl; + if (distance < 1.0) { State next_state = man_positions_.front(); man_positions_.pop_front(); // cout << "remaining man_pos: " << man_positions_.size() << endl; init_man_goal_pos_ = next_state.pos(); init_man_goal_quat_ = next_state.quat(); + v = {0, 0, 0}; + altitude = init_man_goal_pos_(2); + prev_diff_ = {0, 0, 0}; + } else { + + cout << "remaining man_pos: " << man_positions_.size() << endl; + cout << "next_state: " << init_man_goal_pos_ << endl; + cout << "current_state: " << noisy_state_.pos() << endl; + // create desired goal and velocity + // diff = init_man_goal_pos_ - noisy_state_.pos(); + // v = 5.0 * diff.normalized(); + // // heading = init_man_goal_quat_.yaw(); + // altitude = init_man_goal_pos_(2); + + // create desired goal and velocity + // 1.0, 1.0 - bad overshoot + // 0.5, 50 is good + // 0.4/50 and 0.5/60 and 0.5/55 gives dead stop at corners + double kp = 0.4; + double kd = 50.0; + diff = init_man_goal_pos_ - noisy_state_.pos(); + cout << "diff: " << diff << endl; + // v = speed_ * (diff - (diff-prev_diff); + // v = 1.0 * (kp*diff.normalized() + kd*(diff.normalized() - prev_diff_.normalized())); + v = 3.0 * (kp*diff + kd*(diff - prev_diff_)); + } - cout << "remaining man_pos: " << man_positions_.size() << endl; +// cout << "remaining man_pos: " << man_positions_.size() << endl; +// cout << "next_state: " << init_man_goal_pos_ << endl; +// cout << "current_state: " << noisy_state_.pos() << endl; +// // create desired goal and velocity +//// diff = init_man_goal_pos_ - noisy_state_.pos(); +//// v = 5.0 * diff.normalized(); +//// // heading = init_man_goal_quat_.yaw(); +//// altitude = init_man_goal_pos_(2); +// +// // create desired goal and velocity +// // 1.0, 1.0 - bad overshoot +// // 0.5, 50 is pretty good so is 0.6, 60 - maybe a meter of overshoot, 0.4 40 gives more overshoot +// // 0.4/50 and 0.5/60 and 0.5/55 gives dead stop at corners +// double kp = 0.5; +// double kd = 50.0; +// diff = init_man_goal_pos_ - noisy_state_.pos(); +// cout << "diff: " << diff << endl; +// // v = speed_ * (diff - (diff-prev_diff); +// // v = 1.0 * (kp*diff.normalized() + kd*(diff.normalized() - prev_diff_.normalized())); +// v = 3.0 * (kp*diff + kd*(diff - prev_diff_)); + +//// // solution that is different for individual axis? +// Eigen::Vector3d kp = diff.normalized(); +// Eigen::Vector3d kd = (diff - prev_diff_).normalized(); +// v(0) = speed_ * (kp(0)*diff(0) + kd(0)*(diff(0) - prev_diff_(0))); +// v(1) = speed_ * (kp(1)*diff(1) + kd(1)*(diff(1) - prev_diff_(1))); +// v(2) = speed_ * (kp(2)*diff(2) + kd(2)*(diff(2) - prev_diff_(2))); - cout << "next_state: " << init_man_goal_pos_ << endl; - // create desired goal and velocity - diff = init_man_goal_pos_ - noisy_state_.pos(); - v = 5.0 * diff.normalized(); - // heading = init_man_goal_quat_.yaw(); altitude = init_man_goal_pos_(2); + cout << "V: " << v << endl; + cout << "proportional: " << diff << endl; + cout << "derivative: " << diff - prev_diff_ << endl; + cout << "altitude: " << altitude << endl; + prev_diff_ = diff; } else { - // if init maneuver was used, start the drones at the starting point - if(square_side_ == 0) { - if(use_init_maneuver_) { - float distance = sqrt(pow((goal_(0) - noisy_state_.pos()(0)), 2) + pow((goal_(1) - noisy_state_.pos()(1)), 2)); - if (distance < 5) { - square_side_ += 1; - goal_ = goal1_; - } - } - else{ - square_side_ += 1; - } - } +// // if init maneuver was used, start the drones at the starting point +// if(square_side_ == 0) { +// if(use_init_maneuver_) { +// float distance = sqrt(pow((goal_(0) - noisy_state_.pos()(0)), 2) + pow((goal_(1) - noisy_state_.pos()(1)), 2)); +// if (distance < 5) { +// square_side_ += 1; +// goal_ = goal1_; +// } +// } +// else{ +// square_side_ += 1; +// } +// } // check if we have completed this side float distance = sqrt(pow((init_goal_(0) - state_->pos()(0)), 2) + pow((init_goal_(1) - state_->pos()(1)), 2)); @@ -452,9 +549,24 @@ bool Square::step_autonomy(double t, double dt) { } // create desired goal and velocity + // 1.0, 1.0 - bad overshoot + // 0.5, 50 is pretty good so is 0.6, 60 - maybe a meter of overshoot, 0.4 40 gives more overshoot + // 0.4/50 and 0.5/60 and 0.5/55 gives dead stop at corners + + double kp = 0.6; + double kd = 60; diff = goal_ - noisy_state_.pos(); - v = speed_ * diff.normalized(); + cout << "diff: " << diff << endl; + // v * (diff - (diff-prev_diff) + // v = speed_ * (kp*diff.normalized() + kd*(diff.normalized() - prev_diff_.normalized())); + v = speed_ * (kp*diff + kd*(diff - prev_diff_)); + cout << "V: " << v << endl; + cout << "proportional: " << diff << endl; + cout << "derivative: " << diff - prev_diff_ << endl; + altitude = goal_(2); + + prev_diff_ = diff; // heading = Angles::angle_2pi(atan2(v(1), v(0))); } From 327cf0dafe6c498665a58b237101eb0523858c9b Mon Sep 17 00:00:00 2001 From: Natalie Rakoski Date: Thu, 29 Apr 2021 11:49:58 -0400 Subject: [PATCH 06/12] Added Square autonomy plugin with ability to perform visual odometry initialization manuever. --- .../plugins/autonomy/Square/Square.h | 20 +- .../plugins/autonomy/Square/Square.xml | 2 + .../sensor/AirSimSensor/AirSimSensor.h | 10 +- missions/quad-airsim-ex-square.xml | 280 ++++++++++++++++++ missions/quad-airsim-ex1.xml | 89 ++---- src/plugins/autonomy/Square/Square.cpp | 274 ++++++----------- .../sensor/AirSimSensor/AirSimSensor.cpp | 215 +++----------- 7 files changed, 454 insertions(+), 436 deletions(-) create mode 100644 missions/quad-airsim-ex-square.xml diff --git a/include/scrimmage/plugins/autonomy/Square/Square.h b/include/scrimmage/plugins/autonomy/Square/Square.h index 443d7b5a44..e0131747b1 100644 --- a/include/scrimmage/plugins/autonomy/Square/Square.h +++ b/include/scrimmage/plugins/autonomy/Square/Square.h @@ -49,6 +49,12 @@ namespace scrimmage { //} namespace autonomy { + +class InitManeuverType { + public: + bool active = false; +}; + class Square : public scrimmage::Autonomy{ public: void init(std::map ¶ms) override; @@ -57,6 +63,7 @@ class Square : public scrimmage::Autonomy{ protected: double speed_; Eigen::Vector3d goal_; + Eigen::Vector3d next_goal_; Eigen::Vector3d goal1_; Eigen::Vector3d goal2_; Eigen::Vector3d goal3_; @@ -66,9 +73,6 @@ class Square : public scrimmage::Autonomy{ bool show_camera_images_; bool save_camera_images_; -// bool enable_boundary_control_ = false; -// std::shared_ptr boundary_; - int desired_alt_idx_ = 0; int desired_speed_idx_ = 0; int desired_heading_idx_ = 0; @@ -83,12 +87,9 @@ class Square : public scrimmage::Autonomy{ double desired_z_ = 0; -// PublisherPtr pub_gen_ents_; -// bool gen_ents_ = false; -// double prev_gen_time_ = -1.0; - - bool use_init_maneuver_ = true; - bool init_maneuver_finished_ = false; + PublisherPtr init_maneuver_pub_; + bool use_init_maneuver_ = false; + bool init_maneuver_finished_ = true; std::deque get_init_maneuver_positions(); std::deque man_positions_; Eigen::Vector3d init_man_goal_pos_; @@ -96,6 +97,7 @@ class Square : public scrimmage::Autonomy{ int square_side_ = 1; double sq_side_length_m_ = 100; + double start_corner_turn_ = 10; Eigen::Vector3d init_goal_; // StatePtr init_state_; Eigen::Vector3d prev_diff_ = {0,0,0}; diff --git a/include/scrimmage/plugins/autonomy/Square/Square.xml b/include/scrimmage/plugins/autonomy/Square/Square.xml index 414774e7b6..23810d131c 100644 --- a/include/scrimmage/plugins/autonomy/Square/Square.xml +++ b/include/scrimmage/plugins/autonomy/Square/Square.xml @@ -9,4 +9,6 @@ false 100 + + 10 diff --git a/include/scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.h b/include/scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.h index c87440277a..d85c99e744 100644 --- a/include/scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.h +++ b/include/scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.h @@ -191,12 +191,10 @@ class AirSimSensor : public scrimmage::Sensor { bool get_image_data_ = true; bool get_lidar_data_ = true; bool get_imu_data_ = true; - bool use_init_maneuver_ = true; - bool init_maneuver_finished_ = false; - std::deque get_init_maneuver_positions(); - void complete_init_maneuver(); - std::deque man_positions_; - StatePtr init_state_; + + // Init Maneuver + bool init_maneuver_active_ = false; + Quaternion init_man_orig_quat_; // period at which the data acquisition is run [seconds] // double data_acquisition_period_ = .1; diff --git a/missions/quad-airsim-ex-square.xml b/missions/quad-airsim-ex-square.xml new file mode 100644 index 0000000000..f2586f0955 --- /dev/null +++ b/missions/quad-airsim-ex-square.xml @@ -0,0 +1,280 @@ + + + + + + + false + + 50051 + localhost + + time, all_dead + + 10 + 1000 + + mcmillan + 191 191 191 + 10 + + false + all + false + + SimpleCollisionMetrics + + ~/.scrimmage/logs + + 35.721025 + -120.767925 + 300 + true + 10 + + Boundary + + SimpleCollision + ROSClockServer + + GlobalNetwork + LocalNetwork + + 2147483648 + + + + + + + 1 + 77 77 255 + 1 + 1 + 1 + + + + + + 0.0 + 0.0 + 14.0 + 0 + + + + + + + + + + + + + + + + + + AirSimSensor + + + + + + + + + ROSAltimeter + ROSCompass + ROSIMUSensor + + + + + + + + + + + + + + + ROSAirSim + + + + + + + + + + + + + + + + Square + + DoubleIntegrator + DoubleIntegratorControllerVelYaw + iris + + + + + + 2 + 77 77 255 + 1 + 1 + 1 + + + + + + 4.0 + 0.0 + 10.0 + + 0 + + + + + + + + + + + + + + + + + + AirSimSensor + + + + + + + + + ROSAltimeter + ROSCompass + ROSIMUSensor + + + + + + + + + + + + + + + ROSAirSim + + + + + + + + + + + + + + + + + + Square + + DoubleIntegrator + DoubleIntegratorControllerVelYaw + iris + + + + diff --git a/missions/quad-airsim-ex1.xml b/missions/quad-airsim-ex1.xml index eae0b19ac9..c3695235d7 100644 --- a/missions/quad-airsim-ex1.xml +++ b/missions/quad-airsim-ex1.xml @@ -3,7 +3,7 @@ - true 10 - Boundary - SimpleCollision ROSClockServer @@ -91,18 +83,15 @@ - - AirSimSensor + imu_acquisition_period="0.1">AirSimSensor @@ -111,9 +100,9 @@ - ROSAltimeter - ROSCompass - ROSIMUSensor + + + @@ -128,28 +117,19 @@ - ROSAirSim + + + + + + + - - - - - - - + - Square + Straight - - AirSimSensor + imu_acquisition_period="0.1">AirSimSensor @@ -214,9 +191,9 @@ - ROSAltimeter - ROSCompass - ROSIMUSensor + + + @@ -231,29 +208,19 @@ - ROSAirSim + + + + + + + - - - - - - - - Square + Straight Square::get_init_maneuver_positions() { State init_state(*state); State current_state(*state); // man_positions.push_back(current_state); -// double delta = 0.05; - double delta = 1.0; // Rise up to 5 meters above the starting position // for (double i=0.0; i<5.0; i+=delta) { // current_state.pos()(2) = current_state.pos()(2) + delta; // man_positions.push_back(current_state); // } - cout << "starting_state: " << current_state.pos() << endl; - current_state.pos()(2) = current_state.pos()(2) + 5.0; + // cout << "starting_state: " << current_state.pos() << endl; + // add 5m in altitude + // current_state.pos()(2) = current_state.pos()(2) + 5.0; man_positions.push_back(current_state); // // repeat the below right-left movements 1 times @@ -121,12 +120,12 @@ std::deque Square::get_init_maneuver_positions() { // man_positions.push_back(current_state); // } // } - - current_state.pos()(1) = current_state.pos()(1) + 5.0; + // move left - right + current_state.pos()(1) = current_state.pos()(1) + 4.0; man_positions.push_back(current_state); - current_state.pos()(1) = current_state.pos()(1) - 10.0; + current_state.pos()(1) = current_state.pos()(1) - 8.0; man_positions.push_back(current_state); - current_state.pos()(1) = current_state.pos()(1) + 5.0; + current_state.pos()(1) = current_state.pos()(1) + 4.0; man_positions.push_back(current_state); // // repeat the below up-down movements 3 times @@ -150,12 +149,12 @@ std::deque Square::get_init_maneuver_positions() { // man_positions.push_back(current_state); // } // } - - current_state.pos()(2) = current_state.pos()(2) + 5.0; + // move up-down + current_state.pos()(2) = current_state.pos()(2) + 4.0; man_positions.push_back(current_state); - current_state.pos()(2) = current_state.pos()(2) - 10.0; + current_state.pos()(2) = current_state.pos()(2) - 8.0; man_positions.push_back(current_state); - current_state.pos()(2) = current_state.pos()(2) + 5.0; + current_state.pos()(2) = current_state.pos()(2) + 4.0; man_positions.push_back(current_state); // // move in a 4mx2m rectangle parallel to the ground 3 times @@ -186,16 +185,16 @@ std::deque Square::get_init_maneuver_positions() { // man_positions.push_back(current_state); // } // } - - current_state.pos()(1) = current_state.pos()(1) + 5.0; + // move in a rectangle - right, forward, left, backward, right + current_state.pos()(1) = current_state.pos()(1) + 4.0; man_positions.push_back(current_state); - current_state.pos()(0) = current_state.pos()(0) + 5.0; + current_state.pos()(0) = current_state.pos()(0) + 4.0; man_positions.push_back(current_state); - current_state.pos()(1) = current_state.pos()(1) - 10.0; + current_state.pos()(1) = current_state.pos()(1) - 8.0; man_positions.push_back(current_state); - current_state.pos()(0) = current_state.pos()(0) - 5.0; + current_state.pos()(0) = current_state.pos()(0) - 4.0; man_positions.push_back(current_state); - current_state.pos()(1) = current_state.pos()(1) + 5.0; + current_state.pos()(1) = current_state.pos()(1) + 4.0; man_positions.push_back(current_state); // place back at beginning for Straight Plugin @@ -203,9 +202,9 @@ std::deque Square::get_init_maneuver_positions() { man_positions.push_back(init_state); man_positions.push_back(init_state); - for (auto state : man_positions) { - cout << state.pos()(0) << ", " << state.pos()(1) << ", " << state.pos()(2) << "\n" << endl; - } +// for (auto state : man_positions) { +// cout << state.pos()(0) << ", " << state.pos()(1) << ", " << state.pos()(2) << "\n" << endl; +// } return man_positions; } @@ -215,41 +214,31 @@ void Square::init(std::map ¶ms) { show_camera_images_ = scrimmage::get("show_camera_images", params, false); save_camera_images_ = scrimmage::get("save_camera_images", params, false); sq_side_length_m_ = scrimmage::get("sq_side_length_m", params, 100); + start_corner_turn_ = scrimmage::get("start_corner_turn", params, 10); - // Project goal in front... -// Eigen::Vector3d rel_pos = Eigen::Vector3d::UnitX()*1e6; -// Eigen::Vector3d unit_vector = rel_pos.normalized(); -// unit_vector = state_->quat().rotate(unit_vector); -// goal_ = state_->pos() + unit_vector * rel_pos.norm(); -// goal_(2) = state_->pos()(2); - - cout << "speed: " << speed_ << endl; - + // Save the initial starting position + init_goal_ = state_->pos(); // initialize the corner locations of the square + cout << "[SquareAutonomy] Square Corner Locations:" << endl; // positve x direction goal1_ = state_->pos(); goal1_(0) = goal1_(0) + sq_side_length_m_; - cout << "goal1: " << goal1_ << endl; + cout << "goal1: " << goal1_(0) << ", " << goal1_(1) << ", " << goal1_(2) << endl; // positve y direction goal2_ = goal1_; goal2_(1) = goal2_(1) + sq_side_length_m_; - cout << "goal2: " << goal2_ << endl; + cout << "goal2: " << goal2_(0) << ", " << goal2_(1) << ", " << goal2_(2) << endl; // negative x direction goal3_ = goal2_; goal3_(0) = goal3_(0) - sq_side_length_m_; - cout << "goal3: " << goal3_ << endl; + cout << "goal3: " << goal3_(0) << ", " << goal3_(1) << ", " << goal3_(2) << endl; // negative y direction goal4_ = goal3_; goal4_(1) = goal4_(1) - sq_side_length_m_; - cout << "goal4: " << goal4_ << endl; - - // goal_ = goal1_; + cout << "goal4: " << goal4_(0) << ", " << goal4_(1) << ", " << goal4_(2) << endl; // Save the initial starting position - // desired_z_ = state_->pos()(2); - init_goal_ = state_->pos(); - noisy_state_ = *state_; - cout << "original init_goal_: " << init_goal_ << endl; + // cout << "starting position: " << init_goal_ << endl; // Register the desired_z parameter with the parameter server auto param_cb = [&](const double &desired_z) { @@ -285,26 +274,24 @@ void Square::init(std::map ¶ms) { // If using an initialization maneuver, then compile the position vector use_init_maneuver_ = sc::get("use_init_maneuver", params, "false"); + // create an init maneuver publisher + init_maneuver_pub_ = advertise("LocalNetwork", "InitManeuver"); + init_maneuver_finished_ = true; if (use_init_maneuver_) { man_positions_ = get_init_maneuver_positions(); - cout << "[Square] Start VO Initialization Maneuver." << endl; - } + init_maneuver_finished_ = false; + cout << "[SquareAutonomy] Start VO Initialization Maneuver." << endl; - if (use_init_maneuver_) { State next_state = man_positions_.front(); man_positions_.pop_front(); init_man_goal_pos_ = next_state.pos(); init_man_goal_quat_ = next_state.quat(); - // goal_ = man_positions_; + } else { goal_ = goal1_; + next_goal_ = goal2_; } -// enable_boundary_control_ = get("enable_boundary_control", params, false); -// -// auto bd_cb = [&](auto &msg) {boundary_ = sci::Boundary::make_boundary(msg->data);}; -// subscribe("GlobalNetwork", "Boundary", bd_cb); - auto state_cb = [&](auto &msg) { noisy_state_set_ = true; noisy_state_ = msg->data; @@ -366,11 +353,6 @@ void Square::init(std::map ¶ms) { subscribe("LocalNetwork", "ContactBlobCamera", blob_cb); #endif -// gen_ents_ = sc::get("generate_entities", params, gen_ents_); -// if (gen_ents_) { -// pub_gen_ents_ = advertise("GlobalNetwork", "GenerateEntity"); -// } - desired_alt_idx_ = vars_.declare(VariableIO::Type::desired_altitude, VariableIO::Direction::Out); desired_speed_idx_ = vars_.declare(VariableIO::Type::desired_speed, VariableIO::Direction::Out); desired_heading_idx_ = vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::Out); @@ -383,127 +365,62 @@ bool Square::step_autonomy(double t, double dt) { noisy_state_ = *state_; } -// if (boundary_ != nullptr && enable_boundary_control_) { -// if (!boundary_->contains(noisy_state_.pos())) { -// // Project goal through center of boundary -// Eigen::Vector3d center = boundary_->center(); -// center(2) = noisy_state_.pos()(2); // maintain altitude -// Eigen::Vector3d diff = center - noisy_state_.pos(); -// goal_ = noisy_state_.pos() + diff.normalized() * 1e6; -// } -// } - + Eigen::Vector3d current_goal; Eigen::Vector3d diff; Eigen::Vector3d v; double altitude = goal_(2); - // double heading = init_man_goal_quat_.yaw(); - - // cout << "HERE" << endl; - if (man_positions_.size() == 0 && use_init_maneuver_) { + if (man_positions_.size() == 0 && use_init_maneuver_ && init_maneuver_finished_ == false) { init_maneuver_finished_ = true; - // goal_ = init_goal_; goal_ = goal1_; + next_goal_ = goal2_; prev_diff_ = {0,0,0}; cout << "init maneuver finished" << endl; } - if (!init_maneuver_finished_ && use_init_maneuver_) { + // Publish the Init Maneuver Status to send to AirSimSensor + // This is necessary to prevent the quadcopter from rotating during the init maneuver + // for Visual Odometry init maneuver we need the camera to stay facing forward + auto msg = std::make_shared>(); + msg->data.active = !init_maneuver_finished_; + init_maneuver_pub_->publish(msg); + if (!init_maneuver_finished_ && use_init_maneuver_) { // Pop a state off the front of man_positions_ vector and delete the element float distance = sqrt(pow((init_man_goal_pos_(0) - noisy_state_.pos()(0)), 2) + pow((init_man_goal_pos_(1) - noisy_state_.pos()(1)), 2) + pow((init_man_goal_pos_(2) - noisy_state_.pos()(2)), 2)); - cout << "DISTANCE: " << distance << endl; - if (distance < 1.0) { + if (distance < 0.5) { State next_state = man_positions_.front(); man_positions_.pop_front(); - // cout << "remaining man_pos: " << man_positions_.size() << endl; init_man_goal_pos_ = next_state.pos(); - init_man_goal_quat_ = next_state.quat(); v = {0, 0, 0}; altitude = init_man_goal_pos_(2); prev_diff_ = {0, 0, 0}; - } else { - - cout << "remaining man_pos: " << man_positions_.size() << endl; - cout << "next_state: " << init_man_goal_pos_ << endl; - cout << "current_state: " << noisy_state_.pos() << endl; - // create desired goal and velocity - // diff = init_man_goal_pos_ - noisy_state_.pos(); - // v = 5.0 * diff.normalized(); - // // heading = init_man_goal_quat_.yaw(); - // altitude = init_man_goal_pos_(2); - - // create desired goal and velocity - // 1.0, 1.0 - bad overshoot - // 0.5, 50 is good - // 0.4/50 and 0.5/60 and 0.5/55 gives dead stop at corners - double kp = 0.4; - double kd = 50.0; - diff = init_man_goal_pos_ - noisy_state_.pos(); - cout << "diff: " << diff << endl; - // v = speed_ * (diff - (diff-prev_diff); - // v = 1.0 * (kp*diff.normalized() + kd*(diff.normalized() - prev_diff_.normalized())); - v = 3.0 * (kp*diff + kd*(diff - prev_diff_)); - } -// cout << "remaining man_pos: " << man_positions_.size() << endl; -// cout << "next_state: " << init_man_goal_pos_ << endl; -// cout << "current_state: " << noisy_state_.pos() << endl; -// // create desired goal and velocity -//// diff = init_man_goal_pos_ - noisy_state_.pos(); -//// v = 5.0 * diff.normalized(); -//// // heading = init_man_goal_quat_.yaw(); -//// altitude = init_man_goal_pos_(2); -// -// // create desired goal and velocity -// // 1.0, 1.0 - bad overshoot -// // 0.5, 50 is pretty good so is 0.6, 60 - maybe a meter of overshoot, 0.4 40 gives more overshoot -// // 0.4/50 and 0.5/60 and 0.5/55 gives dead stop at corners -// double kp = 0.5; -// double kd = 50.0; -// diff = init_man_goal_pos_ - noisy_state_.pos(); -// cout << "diff: " << diff << endl; -// // v = speed_ * (diff - (diff-prev_diff); -// // v = 1.0 * (kp*diff.normalized() + kd*(diff.normalized() - prev_diff_.normalized())); -// v = 3.0 * (kp*diff + kd*(diff - prev_diff_)); - -//// // solution that is different for individual axis? -// Eigen::Vector3d kp = diff.normalized(); -// Eigen::Vector3d kd = (diff - prev_diff_).normalized(); -// v(0) = speed_ * (kp(0)*diff(0) + kd(0)*(diff(0) - prev_diff_(0))); -// v(1) = speed_ * (kp(1)*diff(1) + kd(1)*(diff(1) - prev_diff_(1))); -// v(2) = speed_ * (kp(2)*diff(2) + kd(2)*(diff(2) - prev_diff_(2))); - - altitude = init_man_goal_pos_(2); - cout << "V: " << v << endl; - cout << "proportional: " << diff << endl; - cout << "derivative: " << diff - prev_diff_ << endl; - cout << "altitude: " << altitude << endl; + // PD Controller + // create desired goal and velocity + // 1.0, 1.0 - bad overshoot + // 0.5, 50 is good + // 0.4/50 and 0.5/60 and 0.5/55 gives dead stop at corners + double kp = 0.6; + double kd = 60.0; + double init_man_speed = 5.0; + diff = init_man_goal_pos_ - noisy_state_.pos(); + v = init_man_speed * (kp*diff + kd*(diff - prev_diff_)); + // calculate the new desired altitude + altitude = noisy_state_.pos()(2) + v(2)*dt; + // do not count z in the velocity normalization + v(2) = 0.0; + // heading should stay facing the original heading + noisy_state_.set_quat(init_man_goal_quat_); prev_diff_ = diff; } else { - -// // if init maneuver was used, start the drones at the starting point -// if(square_side_ == 0) { -// if(use_init_maneuver_) { -// float distance = sqrt(pow((goal_(0) - noisy_state_.pos()(0)), 2) + pow((goal_(1) - noisy_state_.pos()(1)), 2)); -// if (distance < 5) { -// square_side_ += 1; -// goal_ = goal1_; -// } -// } -// else{ -// square_side_ += 1; -// } -// } + // if not performing init maneuver // check if we have completed this side - float distance = sqrt(pow((init_goal_(0) - state_->pos()(0)), 2) + pow((init_goal_(1) - state_->pos()(1)), 2)); -// cout << "distance: " << distance << endl; -// cout << "sq_side_length_m_: " << sq_side_length_m_ << endl; -// cout << "square_side_: " << square_side_ << endl; -// cout << "position: " << state_->pos()<< endl; + // float distance = sqrt(pow((init_goal_(0) - state_->pos()(0)), 2) + pow((init_goal_(1) - state_->pos()(1)), 2)); + float distance = sqrt(pow((init_goal_(0) - noisy_state_.pos()(0)), 2) + pow((init_goal_(1) - noisy_state_.pos()(1)), 2)); if (distance >= sq_side_length_m_ && square_side_ > 0){ // update which side of the square we are on. square_side_ += 1; @@ -514,60 +431,61 @@ bool Square::step_autonomy(double t, double dt) { switch(current_side) { case 1: { - // cout << '1' << endl; // prints "1" - // Project goal in front... // Move in positive X direction goal_ = goal1_; + next_goal_ = goal2_; break; } case 2 : { - // cout << '2' << endl; - // Project goal in front... // Move in positive Y direction goal_ = goal2_; + next_goal_ = goal3_; break; } case 3: { - // cout << '3' << endl; - // Project goal in front... // Move in negative X direction goal_ = goal3_; + next_goal_ = goal4_; break; } case 0: { - // cout << '4' << endl; - // Project goal in front... // Move in negative Y direction goal_ = goal4_; + next_goal_ = goal1_; break; } } - cout << "goal: " << goal_ << endl; + } // if distance + + // Parametric Path Planning + // heading should turn slowly toward the next corner as it approaches a corner in order to keep the camera on track + current_goal = goal_; + // if distance is less than 10 to the upcoming corner start adding in influence for the next goal + // cout << (sq_side_length_m_ - distance) << endl; + if ((sq_side_length_m_ - distance) < start_corner_turn_) { + // influence (btw 0 and 1) will increase as the quadcopter gets closer to the corner + double influence = (start_corner_turn_ - (sq_side_length_m_ - distance)) / start_corner_turn_; + // average the goal position between the upcoming corner and the next corner as the quadcopter gets closer + current_goal = (current_goal * (1.0 - influence)) + (next_goal_ * influence); } - // create desired goal and velocity - // 1.0, 1.0 - bad overshoot - // 0.5, 50 is pretty good so is 0.6, 60 - maybe a meter of overshoot, 0.4 40 gives more overshoot - // 0.4/50 and 0.5/60 and 0.5/55 gives dead stop at corners - - double kp = 0.6; - double kd = 60; - diff = goal_ - noisy_state_.pos(); - cout << "diff: " << diff << endl; - // v * (diff - (diff-prev_diff) - // v = speed_ * (kp*diff.normalized() + kd*(diff.normalized() - prev_diff_.normalized())); - v = speed_ * (kp*diff + kd*(diff - prev_diff_)); - cout << "V: " << v << endl; - cout << "proportional: " << diff << endl; - cout << "derivative: " << diff - prev_diff_ << endl; - - altitude = goal_(2); - - prev_diff_ = diff; - // heading = Angles::angle_2pi(atan2(v(1), v(0))); + // create desired goal and velocity + // 1.0, 1.0 - bad overshoot + // 0.5, 50 is pretty good so is 0.6, 60 - maybe a meter of overshoot, 0.4 40 gives more overshoot + // 0.4/50 and 0.5/60 and 0.5/55 gives dead stop at corners + double kp = 0.6; + double kd = 60; + diff = current_goal - noisy_state_.pos(); + v = speed_ * (kp*diff + kd*(diff - prev_diff_)); + // calculate the new desired altitude + altitude = noisy_state_.pos()(2) + v(2)*dt; + // do not count z in the velocity normalization + v(2) = 0.0; + + prev_diff_ = diff; } /////////////////////////////////////////////////////////////////////////// diff --git a/src/plugins/sensor/AirSimSensor/AirSimSensor.cpp b/src/plugins/sensor/AirSimSensor/AirSimSensor.cpp index 37f8aea585..d025182d6f 100644 --- a/src/plugins/sensor/AirSimSensor/AirSimSensor.cpp +++ b/src/plugins/sensor/AirSimSensor/AirSimSensor.cpp @@ -42,6 +42,7 @@ #include #include #include +#include #include #include @@ -208,102 +209,6 @@ void AirSimSensor::parse_imu_configs(std::map ¶ms) } } -std::deque AirSimSensor::get_init_maneuver_positions() { - - std::deque man_positions; - - // get the starting position - sc::StatePtr &state = parent_->state_truth(); - State init_state(*state); - State current_state(*state); - man_positions.push_back(current_state); - double delta = 0.01; - - // Rise up to 5 meters above the starting position - for (double i=0.0; i<5.0; i+=delta) { - current_state.pos()(2) = current_state.pos()(2) + delta; - man_positions.push_back(current_state); - } - - // repeat the below right-left movements 3 times - for (int repeat = 0; repeat<3; repeat++){ - // move 2 meters to the right, back to the center - for (double i=0.0; i<2.0; i+=delta) { - current_state.pos()(1) = current_state.pos()(1) + delta;; - man_positions.push_back(current_state); - } - for (double i=0.0; i<2.0; i+=delta) { - current_state.pos()(1) = current_state.pos()(1) - delta; - man_positions.push_back(current_state); - } - // move 2 meters to the left, back to the center - for (double i=0.0; i<2.0; i+=delta) { - current_state.pos()(1) = current_state.pos()(1) - delta; - man_positions.push_back(current_state); - } - for (double i=0.0; i<2.0; i+=delta) { - current_state.pos()(1) = current_state.pos()(1) + delta; - man_positions.push_back(current_state); - } - } - - // repeat the below up-down movements 3 times - for (int repeat = 0; repeat<3; repeat++){ - // move 1 meter up, back to the center - for (double i=0.0; i<1.0; i+=delta) { - current_state.pos()(2) = current_state.pos()(2) + delta; - man_positions.push_back(current_state); - } - for (double i=0.0; i<1.0; i+=delta) { - current_state.pos()(2) = current_state.pos()(2) - delta; - man_positions.push_back(current_state); - } - // move 1 meter down, back to the center - for (double i=0.0; i<1.0; i+=delta) { - current_state.pos()(2) = current_state.pos()(2) - delta; - man_positions.push_back(current_state); - } - for (double i=0.0; i<1.0; i+=delta) { - current_state.pos()(2) = current_state.pos()(2) + delta; - man_positions.push_back(current_state); - } - } - - // move in a 4mx2m rectangle parallel to the ground 3 times - for (int repeat = 0; repeat<3; repeat++){ - // move 1 meter in the +y direction - for (double i=0.0; i<2.0; i+=delta) { - current_state.pos()(1) = current_state.pos()(1) + delta; - man_positions.push_back(current_state); - } - // move 1 meter in the +x direction - for (double i=0.0; i<2.0; i+=delta) { - current_state.pos()(0) = current_state.pos()(0) + delta; - man_positions.push_back(current_state); - } - // move 2 meters in the -y direction - for (double i=0.0; i<4.0; i+=delta) { - current_state.pos()(1) = current_state.pos()(1) - delta; - man_positions.push_back(current_state); - } - // move 1 meters in the -x direction - for (double i=0.0; i<2.0; i+=delta) { - current_state.pos()(0) = current_state.pos()(0) - delta; - man_positions.push_back(current_state); - } - // move 1 meter in the +y direction - for (double i=0.0; i<2.0; i+=delta) { - current_state.pos()(1) = current_state.pos()(1) + delta; - man_positions.push_back(current_state); - } - } - - // place back at beginning for Straight Plugin - man_positions.push_back(init_state); - - return man_positions; -} - void AirSimSensor::init(std::map ¶ms) { // RPC / AirSim.xml file airsim_ip_ = sc::get("airsim_ip", params, "localhost"); @@ -325,14 +230,16 @@ void AirSimSensor::init(std::map ¶ms) { lidar_acquisition_period_ = sc::get("lidar_acquisition_period", params, 0.1); imu_acquisition_period_ = sc::get("imu_acquisition_period", params, 0.1); - // If using an initialization maneuver, then compile the position vector - use_init_maneuver_ = sc::get("use_init_maneuver", params, "true"); - if (use_init_maneuver_) { - man_positions_ = get_init_maneuver_positions(); + ///// Init Maneuver ////// + // Subscribe to initialization maneuver active flag + auto init_man_cb = [&](auto &msg) { + init_maneuver_active_ = msg->data.active; + }; + subscribe("LocalNetwork", "InitManeuver", init_man_cb); + if (init_maneuver_active_) { cout << "[AirSimSensor] Start VO Initialization Maneuver." << endl; } - // Open airsim_data CSV for append (app) and set column headers std::string csv_filename = parent_->mp()->log_dir() + "/airsim_data_robot" + std::to_string(parent_->id().id()) + ".csv"; @@ -682,62 +589,6 @@ void AirSimSensor::request_imu() { } } -//void AirSimSensor::complete_init_maneuver(){ -// -// while(man_positions_.size() != 0){ -// // Pop a state off the front of man_positions_ vector and delete the element -// State next_state = man_positions_.front(); -// man_positions_.pop_front(); -// -// // convert from ENU to NED frame -// ma::Vector3r pos(next_state.pos()(1), next_state.pos()(0), -next_state.pos()(2)); -// -// enu_to_ned_yaw_.set_angle(ang::rad2deg(next_state.quat().yaw())); -// double airsim_yaw_rad = ang::deg2rad(enu_to_ned_yaw_.angle()); -// -// // pitch, roll, yaw -// // note, the negative pitch and yaw are required because of the wsu coordinate frame -// ma::Quaternionr qd = ma::VectorMath::toQuaternion(-next_state.quat().pitch(), -// next_state.quat().roll(), -// airsim_yaw_rad); -// -// // Send state information to AirSim -// sim_client_->simSetVehiclePose(ma::Pose(pos, qd), true, vehicle_name_); -// -// // Get the camera images from the other thread -// if (get_image_data_) { -// // sc::MessagePtr> im_msg_step; -// // auto im_msg_step = std::make_shared>>(); // NOLINT -// -// new_image_mutex_.lock(); -// bool new_image = new_image_; -// new_image_ = false; -// new_image_mutex_.unlock(); -// -// img_msg_mutex_.lock(); -// // im_msg_step = img_msg_; -// std::shared_ptr>> im_msg_step = img_msg_; -// // sc::MessagePtr> im_msg_step = img_msg_; -// img_msg_mutex_.unlock(); -// -// // If image is new, publish -// if (new_image) { -// if (save_airsim_data_) { -// AirSimSensor::save_data(im_msg_step, next_state, airsim_frame_num_); -// } -// img_pub_->publish(im_msg_step); -// } -// } -// // increment frame num so we do not save over the images -// airsim_frame_num_++; -// -// // sleep -// std::this_thread::sleep_for(std::chrono::milliseconds(1)); -// } // end while man_positions_ is not empty -// -// init_maneuver_finished_ = true; -//} - bool AirSimSensor::step() { /////////////////////////////////////////////////////////////////////////// /// Client Connection / Disconnection Handling @@ -783,43 +634,43 @@ bool AirSimSensor::step() { // record init state if (airsim_frame_num_ == 0){ - init_state_ = parent_->state_truth(); + sc::StatePtr &init_state = parent_->state_truth(); + init_man_orig_quat_ = init_state->quat(); } State next_state(*state); - if (man_positions_.size() == 0) { - init_maneuver_finished_ = true; - // reset to the original state - // parent_->state_truth() = init_state_; - std::vector autos = parent_->autonomies(); - // autos.front()->set_desired_state(init_state_); - for (auto a : autos) { - // (*a).set_desired_state(init_state_); - // autos.front()->set_desired_state(init_state_); - a->set_desired_state(init_state_); - } - } - - if (!init_maneuver_finished_ && use_init_maneuver_) { - //MissionParsePtr mp_->pause(); - // complete_init_maneuver(); - // Pop a state off the front of man_positions_ vector and delete the element - State next_state = man_positions_.front(); - man_positions_.pop_front(); +// if (man_positions_.size() == 0) { +// init_maneuver_finished_ = true; +// // reset to the original state +// // parent_->state_truth() = init_state_; +// std::vector autos = parent_->autonomies(); +// // autos.front()->set_desired_state(init_state_); +// for (auto a : autos) { +// // (*a).set_desired_state(init_state_); +// // autos.front()->set_desired_state(init_state_); +// a->set_desired_state(init_state_); +// } +// } + if (init_maneuver_active_) { + // move the quadcopter, but keep its heading facing forward so that the camera can + // perform correctly + // Go through the motions of the autonomy plugin defined in the mission file. // convert from ENU to NED frame - ma::Vector3r pos(next_state.pos()(1), next_state.pos()(0), -next_state.pos()(2)); + ma::Vector3r pos(state->pos()(1), state->pos()(0), -state->pos()(2)); - enu_to_ned_yaw_.set_angle(ang::rad2deg(next_state.quat().yaw())); + enu_to_ned_yaw_.set_angle(ang::rad2deg(init_man_orig_quat_.yaw())); double airsim_yaw_rad = ang::deg2rad(enu_to_ned_yaw_.angle()); // pitch, roll, yaw // note, the negative pitch and yaw are required because of the wsu coordinate frame - ma::Quaternionr qd = ma::VectorMath::toQuaternion(-next_state.quat().pitch(), - next_state.quat().roll(), +// ma::Quaternionr qd = ma::VectorMath::toQuaternion(-state->quat().pitch(), +// state->quat().roll(), +// airsim_yaw_rad); + ma::Quaternionr qd = ma::VectorMath::toQuaternion(-init_man_orig_quat_.pitch(), + init_man_orig_quat_.roll(), airsim_yaw_rad); - // Send state information to AirSim sim_client_->simSetVehiclePose(ma::Pose(pos, qd), true, vehicle_name_); From d56d67109b6588fb74d800b0f2deb4cf6105119b Mon Sep 17 00:00:00 2001 From: Natalie Rakoski Date: Thu, 29 Apr 2021 12:24:33 -0400 Subject: [PATCH 07/12] added init time printout --- missions/quad-airsim-ex-square.xml | 14 +++++++------- src/plugins/autonomy/Square/Square.cpp | 2 +- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/missions/quad-airsim-ex-square.xml b/missions/quad-airsim-ex-square.xml index f2586f0955..901acd1a74 100644 --- a/missions/quad-airsim-ex-square.xml +++ b/missions/quad-airsim-ex-square.xml @@ -3,7 +3,7 @@ - Boundary @@ -152,8 +152,8 @@ Square Square Date: Thu, 29 Apr 2021 14:34:06 -0400 Subject: [PATCH 08/12] remove init_maneuver tags from AirSimSensor.xml --- .../plugins/sensor/AirSimSensor/AirSimSensor.xml | 2 -- missions/quad-airsim-ex-square.xml | 10 ++-------- 2 files changed, 2 insertions(+), 10 deletions(-) diff --git a/include/scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.xml b/include/scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.xml index b703b7f901..ee968aee95 100644 --- a/include/scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.xml +++ b/include/scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.xml @@ -66,8 +66,6 @@ - - true true diff --git a/missions/quad-airsim-ex-square.xml b/missions/quad-airsim-ex-square.xml index 901acd1a74..dc7d643391 100644 --- a/missions/quad-airsim-ex-square.xml +++ b/missions/quad-airsim-ex-square.xml @@ -91,8 +91,6 @@ - - AirSimSensor + imu_acquisition_period="0.1">AirSimSensor @@ -199,8 +196,6 @@ - - AirSimSensor + imu_acquisition_period="0.1">AirSimSensor From 79f418fadd87a7f391ecfa949f33951b91b116a0 Mon Sep 17 00:00:00 2001 From: Natalie Rakoski Date: Thu, 13 May 2021 09:37:08 -0400 Subject: [PATCH 09/12] Add StraightVOInit Plugin --- .../autonomy/StraightVOInit/StraightVOInit.h | 96 +++++ .../StraightVOInit/StraightVOInit.xml | 12 + missions/quad-airsim-ex-straight_VOinit.xml | 278 ++++++++++++++ .../autonomy/StraightVOInit/CMakeLists.txt | 42 ++ .../StraightVOInit/StraightVOInit.cpp | 362 ++++++++++++++++++ 5 files changed, 790 insertions(+) create mode 100644 include/scrimmage/plugins/autonomy/StraightVOInit/StraightVOInit.h create mode 100644 include/scrimmage/plugins/autonomy/StraightVOInit/StraightVOInit.xml create mode 100644 missions/quad-airsim-ex-straight_VOinit.xml create mode 100644 src/plugins/autonomy/StraightVOInit/CMakeLists.txt create mode 100644 src/plugins/autonomy/StraightVOInit/StraightVOInit.cpp diff --git a/include/scrimmage/plugins/autonomy/StraightVOInit/StraightVOInit.h b/include/scrimmage/plugins/autonomy/StraightVOInit/StraightVOInit.h new file mode 100644 index 0000000000..3a6a2f998a --- /dev/null +++ b/include/scrimmage/plugins/autonomy/StraightVOInit/StraightVOInit.h @@ -0,0 +1,96 @@ +/*! + * @file + * + * @section LICENSE + * + * Copyright (C) 2017 by the Georgia Tech Research Institute (GTRI) + * + * This file is part of SCRIMMAGE. + * + * SCRIMMAGE is free software: you can redistribute it and/or modify it under + * the terms of the GNU Lesser General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * SCRIMMAGE is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public + * License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with SCRIMMAGE. If not, see . + * + * @author Kevin DeMarco + * @author Eric Squires + * @date 31 July 2017 + * @version 0.1.0 + * @brief Brief file description. + * @section DESCRIPTION + * A Long description goes here. + * + */ + +#ifndef INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_STRAIGHTVOINIT_STRAIGHTVOINIT_H_ +#define INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_STRAIGHTVOINIT_STRAIGHTVOINIT_H_ +#include +#include + +#include + +#include +#include +#include + +namespace scrimmage { + +namespace interaction { +class BoundaryBase; +} + +namespace autonomy { + +class InitManeuverType { + public: + bool active = false; +}; + +class StraightVOInit : public scrimmage::Autonomy{ + public: + void init(std::map ¶ms) override; + bool step_autonomy(double t, double dt) override; + + protected: + double speed_; + Eigen::Vector3d goal_; + Eigen::Vector3d init_goal_; + + int frame_number_; + bool show_camera_images_; + bool save_camera_images_; + bool show_text_label_; + + bool enable_boundary_control_ = false; + std::shared_ptr boundary_; + + int desired_alt_idx_ = 0; + int desired_speed_idx_ = 0; + int desired_heading_idx_ = 0; + + bool noisy_state_set_ = false; + State noisy_state_; + ContactMap noisy_contacts_; + + double desired_z_ = 0; + Eigen::Vector3d prev_diff_ = {0,0,0}; + + PublisherPtr init_maneuver_pub_; + bool use_init_maneuver_ = false; + bool init_maneuver_finished_ = true; + std::deque get_init_maneuver_positions(); + std::deque man_positions_; + Eigen::Vector3d init_man_goal_pos_; + Quaternion init_man_goal_quat_; +}; +} // namespace autonomy +} // namespace scrimmage +#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_STRAIGHTVOINIT_STRAIGHTVOINIT_H_ diff --git a/include/scrimmage/plugins/autonomy/StraightVOInit/StraightVOInit.xml b/include/scrimmage/plugins/autonomy/StraightVOInit/StraightVOInit.xml new file mode 100644 index 0000000000..287691b58a --- /dev/null +++ b/include/scrimmage/plugins/autonomy/StraightVOInit/StraightVOInit.xml @@ -0,0 +1,12 @@ + + + + StraightVOInit_plugin + 21 + false + false + false + + false + + diff --git a/missions/quad-airsim-ex-straight_VOinit.xml b/missions/quad-airsim-ex-straight_VOinit.xml new file mode 100644 index 0000000000..19b8160f3e --- /dev/null +++ b/missions/quad-airsim-ex-straight_VOinit.xml @@ -0,0 +1,278 @@ + + + + + + + false + + 50051 + localhost + + time, all_dead + + 10 + 1000 + + mcmillan + 191 191 191 + 10 + + false + all + false + + SimpleCollisionMetrics + + ~/.scrimmage/logs + + 35.721025 + -120.767925 + 300 + true + 10 + + Boundary + + SimpleCollision + ROSClockServer + + GlobalNetwork + LocalNetwork + + 2147483648 + + + + + + + 1 + 77 77 255 + 1 + 1 + 1 + + + + + + 0.0 + 0.0 + 14.0 + 0 + + + + + + + + + + + + + + + + AirSimSensor + + + + + + + + + ROSAltimeter + ROSCompass + ROSIMUSensor + + + + + + + + + + + + + + + ROSAirSim + + + + + + + StraightVOInit + + + + + + + + + + + + + + + + + DoubleIntegrator + DoubleIntegratorControllerVelYaw + iris + + + + + + 2 + 77 77 255 + 1 + 1 + 1 + + + + + + 4.0 + 0.0 + 10.0 + + 0 + + + + + + + + + + + + + + + AirSimSensor + + + + + + + + + ROSAltimeter + ROSCompass + ROSIMUSensor + + + + + + + + + + + + + + + ROSAirSim + + + + + + + + StraightVOInit + + + + + + + + + + + + + + + + + DoubleIntegrator + DoubleIntegratorControllerVelYaw + iris + + + + diff --git a/src/plugins/autonomy/StraightVOInit/CMakeLists.txt b/src/plugins/autonomy/StraightVOInit/CMakeLists.txt new file mode 100644 index 0000000000..e473bfe109 --- /dev/null +++ b/src/plugins/autonomy/StraightVOInit/CMakeLists.txt @@ -0,0 +1,42 @@ +#-------------------------------------------------------- +# Library Creation +#-------------------------------------------------------- +SET (LIBRARY_NAME StraightVOInit_plugin) +SET (LIB_MAJOR 0) +SET (LIB_MINOR 0) +SET (LIB_RELEASE 1) + +file(GLOB SRCS *.cpp) + +ADD_LIBRARY(${LIBRARY_NAME} SHARED + ${SRCS} + ) + +TARGET_LINK_LIBRARIES(${LIBRARY_NAME} + Boundary_plugin + scrimmage-core + ) + +if (OpenCV_FOUND) + TARGET_LINK_LIBRARIES(${LIBRARY_NAME} + scrimmage-opencv + ) +endif() + +SET (_soversion ${LIB_MAJOR}.${LIB_MINOR}.${LIB_RELEASE}) + +set_target_properties(${LIBRARY_NAME} PROPERTIES + SOVERSION ${LIB_MAJOR} + VERSION ${_soversion} + LIBRARY_OUTPUT_DIRECTORY ${PROJECT_PLUGIN_LIBS_DIR} + ) + +install(TARGETS ${LIBRARY_NAME} + # IMPORTANT: Add the library to the "export-set" + EXPORT ${PROJECT_NAME}-targets + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib/${PROJECT_NAME}/plugin_libs +) + +# Push up the PROJECT_PLUGINS variable +set(PROJECT_PLUGINS ${PROJECT_PLUGINS} ${LIBRARY_NAME} PARENT_SCOPE) diff --git a/src/plugins/autonomy/StraightVOInit/StraightVOInit.cpp b/src/plugins/autonomy/StraightVOInit/StraightVOInit.cpp new file mode 100644 index 0000000000..a3a939cf64 --- /dev/null +++ b/src/plugins/autonomy/StraightVOInit/StraightVOInit.cpp @@ -0,0 +1,362 @@ +/*! + * @file + * + * @section LICENSE + * + * Copyright (C) 2017 by the Georgia Tech Research Institute (GTRI) + * + * This file is part of SCRIMMAGE. + * + * SCRIMMAGE is free software: you can redistribute it and/or modify it under + * the terms of the GNU Lesser General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * SCRIMMAGE is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public + * License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with SCRIMMAGE. If not, see . + * + * @author Kevin DeMarco + * @author Eric Squires + * @date 31 July 2017 + * @version 0.1.0 + * @brief Brief file description. + * @section DESCRIPTION + * A Long description goes here. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if ENABLE_OPENCV == 1 +#include +#include +#include +#include +#endif + +#if ENABLE_AIRSIM == 1 +#include +#endif + +#include + +namespace sc = scrimmage; +namespace sp = scrimmage_proto; +namespace sci = scrimmage::interaction; + +#include + +#define BOOST_NO_CXX11_SCOPED_ENUMS +#include +#undef BOOST_NO_CXX11_SCOPED_ENUMS +namespace fs = boost::filesystem; + +REGISTER_PLUGIN(scrimmage::Autonomy, + scrimmage::autonomy::StraightVOInit, + StraightVOInit_plugin) + +namespace scrimmage { +namespace autonomy { + +std::deque StraightVOInit::get_init_maneuver_positions() { + + std::deque man_positions; + + // get the starting position + sc::StatePtr &state = parent_->state_truth(); + State init_state(*state); + State current_state(*state); + // man_positions.push_back(current_state); + + // Rise up to 5 meters above the starting position +// for (double i=0.0; i<5.0; i+=delta) { +// current_state.pos()(2) = current_state.pos()(2) + delta; +// man_positions.push_back(current_state); +// } + // cout << "starting_state: " << current_state.pos() << endl; + // add 5m in altitude + // current_state.pos()(2) = current_state.pos()(2) + 5.0; + man_positions.push_back(current_state); + + // move left - right + current_state.pos()(1) = current_state.pos()(1) + 4.0; + man_positions.push_back(current_state); + current_state.pos()(1) = current_state.pos()(1) - 8.0; + man_positions.push_back(current_state); + current_state.pos()(1) = current_state.pos()(1) + 4.0; + man_positions.push_back(current_state); + + // move up-down + current_state.pos()(2) = current_state.pos()(2) + 4.0; + man_positions.push_back(current_state); + current_state.pos()(2) = current_state.pos()(2) - 8.0; + man_positions.push_back(current_state); + current_state.pos()(2) = current_state.pos()(2) + 4.0; + man_positions.push_back(current_state); + + // move in a rectangle - right, forward, left, backward, right + current_state.pos()(1) = current_state.pos()(1) + 4.0; + man_positions.push_back(current_state); + current_state.pos()(0) = current_state.pos()(0) + 4.0; + man_positions.push_back(current_state); + current_state.pos()(1) = current_state.pos()(1) - 8.0; + man_positions.push_back(current_state); + current_state.pos()(0) = current_state.pos()(0) - 4.0; + man_positions.push_back(current_state); + current_state.pos()(1) = current_state.pos()(1) + 4.0; + man_positions.push_back(current_state); + + // place back at beginning for Straight Plugin + man_positions.push_back(init_state); + man_positions.push_back(init_state); + man_positions.push_back(init_state); + + + return man_positions; +} + +void StraightVOInit::init(std::map ¶ms) { + speed_ = scrimmage::get("speed", params, 0.0); + show_camera_images_ = scrimmage::get("show_camera_images", params, false); + save_camera_images_ = scrimmage::get("save_camera_images", params, false); + + // Project goal in front... + Eigen::Vector3d rel_pos = Eigen::Vector3d::UnitX()*1e6; + Eigen::Vector3d unit_vector = rel_pos.normalized(); + unit_vector = state_->quat().rotate(unit_vector); + goal_ = state_->pos() + unit_vector * rel_pos.norm(); + goal_(2) = state_->pos()(2); + init_goal_ = goal_; + + // Set the desired_z to our initial position. + // desired_z_ = state_->pos()(2); + + // Register the desired_z parameter with the parameter server + auto param_cb = [&](const double &desired_z) { + std::cout << "desired_z param changed at: " << time_->t() + << ", with value: " << desired_z << endl; + }; + register_param("desired_z", goal_(2), param_cb); + + if (save_camera_images_) { + ///////////////////////////////////////////////////////// + // Remove all img files from previous run + if (fs::exists("./imgs")) { + fs::recursive_directory_iterator it("./imgs"); + fs::recursive_directory_iterator endit; + std::list paths_to_rm; + while (it != endit) { + fs::path path = it->path(); + if (fs::is_regular_file(*it) && path.extension() == ".png") { + paths_to_rm.push_back(path); + } + ++it; + } + for (fs::path p : paths_to_rm) { + fs::remove(p); + } + } else { + fs::create_directories("./imgs"); + } + ///////////////////////////////////////////////////////// + } + + frame_number_ = 0; + + // If using an initialization maneuver, then compile the position vector + use_init_maneuver_ = sc::get("use_init_maneuver", params, "false"); + // create an init maneuver publisher + init_maneuver_pub_ = advertise("LocalNetwork", "InitManeuver"); + init_maneuver_finished_ = true; + if (use_init_maneuver_) { + man_positions_ = get_init_maneuver_positions(); + init_maneuver_finished_ = false; + cout << "[StraightVOInit Autonomy] Start VO Initialization Maneuver." << endl; + + State next_state = man_positions_.front(); + man_positions_.pop_front(); + init_man_goal_pos_ = next_state.pos(); + init_man_goal_quat_ = next_state.quat(); + } + + enable_boundary_control_ = get("enable_boundary_control", params, false); + + auto bd_cb = [&](auto &msg) {boundary_ = sci::Boundary::make_boundary(msg->data);}; + subscribe("GlobalNetwork", "Boundary", bd_cb); + + auto state_cb = [&](auto &msg) { + noisy_state_set_ = true; + noisy_state_ = msg->data; + }; + subscribe("LocalNetwork", "StateWithCovariance", state_cb); + + auto cnt_cb = [&](scrimmage::MessagePtr &msg) { + noisy_contacts_ = msg->data; // Save map of noisy contacts + }; + subscribe("LocalNetwork", "ContactsWithCovariances", cnt_cb); + +#if (ENABLE_OPENCV == 1 && ENABLE_AIRSIM == 1) + auto airsim_cb = [&](auto &msg) { + for (sc::sensor::AirSimImageType a : msg->data) { + if (show_camera_images_) { + // Get Camera Name + std::string window_name = a.vehicle_name + "_" + a.camera_config.cam_name + "_" + a.camera_config.img_type_name; + // for depth images CV imshow expects grayscale image values to be between 0 and 1. + if (a.camera_config.img_type_name == "DepthPerspective" || a.camera_config.img_type_name == "DepthPlanner") { + // Worked before building with ROS + cv::Mat tempImage; + a.img.convertTo(tempImage, CV_32FC1, 1.f/255); + // cv::normalize(a.img, tempImage, 0, 1, cv::NORM_MINMAX); + // cout << tempImage << endl; + cv::imshow(window_name, tempImage); + } else { + // other image types are int 0-255. + if (a.img.channels() == 4) { + cout << "image channels: " << a.img.channels() << endl; + cout << "Warning: Old AirSim Linux Asset Environments have 4 channels. Color images will not display correctly." << endl; + cout << "Warning: Use Asset Environment versions Linux-v1.3.1+." << endl; + cv::Mat tempImage; + cv::cvtColor(a.img , tempImage, CV_RGBA2RGB); + cv::imshow(window_name, tempImage); + } else { + cv::imshow(window_name, a.img); + } + } + cv::waitKey(1); + } + } + }; + subscribe>("LocalNetwork", "AirSimImages", airsim_cb); +#endif + +#if ENABLE_OPENCV == 1 + auto blob_cb = [&](auto &msg) { + if (save_camera_images_) { + std::string img_name = "./imgs/camera_" + + std::to_string(frame_number_++) + ".png"; + cv::imwrite(img_name, msg->data.frame); + } + + if (show_camera_images_) { + cv::imshow("Camera Sensor", msg->data.frame); + cv::waitKey(1); + } + }; + subscribe("LocalNetwork", "ContactBlobCamera", blob_cb); +#endif + + desired_alt_idx_ = vars_.declare(VariableIO::Type::desired_altitude, VariableIO::Direction::Out); + desired_speed_idx_ = vars_.declare(VariableIO::Type::desired_speed, VariableIO::Direction::Out); + desired_heading_idx_ = vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::Out); +} + +bool StraightVOInit::step_autonomy(double t, double dt) { + + // Read data from sensors... + if (!noisy_state_set_) { + noisy_state_ = *state_; + } + + Eigen::Vector3d current_goal; + Eigen::Vector3d diff; + Eigen::Vector3d v; + double altitude = goal_(2); + + if (man_positions_.size() == 0 && use_init_maneuver_ && init_maneuver_finished_ == false) { + init_maneuver_finished_ = true; + goal_ = init_goal_; + prev_diff_ = {0,0,0}; + cout << "init maneuver finished at time: " << t << endl; + } + + // Publish the Init Maneuver Status to send to AirSimSensor + // This is necessary to prevent the quadcopter from rotating during the init maneuver + // for Visual Odometry init maneuver we need the camera to stay facing forward + auto msg = std::make_shared>(); + msg->data.active = !init_maneuver_finished_; + init_maneuver_pub_->publish(msg); + + if (!init_maneuver_finished_ && use_init_maneuver_) { + // Pop a state off the front of man_positions_ vector and delete the element + float distance = sqrt(pow((init_man_goal_pos_(0) - noisy_state_.pos()(0)), 2) + pow((init_man_goal_pos_(1) - noisy_state_.pos()(1)), 2) + pow((init_man_goal_pos_(2) - noisy_state_.pos()(2)), 2)); + if (distance < 0.5) { + State next_state = man_positions_.front(); + man_positions_.pop_front(); + init_man_goal_pos_ = next_state.pos(); + v = {0, 0, 0}; + altitude = init_man_goal_pos_(2); + prev_diff_ = {0, 0, 0}; + } + + // PD Controller + // create desired goal and velocity + // 1.0, 1.0 - bad overshoot + // 0.5, 50 is good + // 0.4/50 and 0.5/60 and 0.5/55 gives dead stop at corners + double kp = 0.6; + double kd = 60.0; + double init_man_speed = 5.0; + diff = init_man_goal_pos_ - noisy_state_.pos(); + v = init_man_speed * (kp*diff + kd*(diff - prev_diff_)); + // calculate the new desired altitude + altitude = noisy_state_.pos()(2) + v(2)*dt; + // do not count z in the velocity normalization + v(2) = 0.0; + // heading should stay facing the original heading + noisy_state_.set_quat(init_man_goal_quat_); + prev_diff_ = diff; + + } else { + + // If not performing init maneuver move the quadcopter in a straight line + + // If quadcopter reaches boundary lines + if (boundary_ != nullptr && enable_boundary_control_) { + if (!boundary_->contains(noisy_state_.pos())) { + // Project goal through center of boundary + Eigen::Vector3d center = boundary_->center(); + center(2) = noisy_state_.pos()(2); // maintain altitude + Eigen::Vector3d diff = center - noisy_state_.pos(); + goal_ = noisy_state_.pos() + diff.normalized() * 1e6; + } + } + + Eigen::Vector3d diff = goal_ - noisy_state_.pos(); + v = speed_ * diff.normalized(); + altitude = goal_(2); + } + + /////////////////////////////////////////////////////////////////////////// + // Convert desired velocity to desired speed, heading, and pitch controls + /////////////////////////////////////////////////////////////////////////// + double heading = Angles::angle_2pi(atan2(v(1), v(0))); + vars_.output(desired_alt_idx_, altitude); + vars_.output(desired_speed_idx_, v.norm()); + vars_.output(desired_heading_idx_, heading); + + noisy_state_set_ = false; + return true; +} +} // namespace autonomy +} // namespace scrimmage From 9b681d0769081907da6b9a8d7db767c79c684f5c Mon Sep 17 00:00:00 2001 From: Natalie Rakoski Date: Thu, 13 May 2021 09:57:56 -0400 Subject: [PATCH 10/12] set use_init_man to true as default --- missions/quad-airsim-ex-straight_VOinit.xml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/missions/quad-airsim-ex-straight_VOinit.xml b/missions/quad-airsim-ex-straight_VOinit.xml index 19b8160f3e..64832d9d7c 100644 --- a/missions/quad-airsim-ex-straight_VOinit.xml +++ b/missions/quad-airsim-ex-straight_VOinit.xml @@ -5,7 +5,7 @@ @@ -139,7 +139,7 @@ StraightVOInit + use_init_maneuver="true">StraightVOInit @@ -247,7 +247,7 @@ StraightVOInit + use_init_maneuver="true">StraightVOInit From 28aa712c45ea6497f0c3d195d7b56c7743bb9ea4 Mon Sep 17 00:00:00 2001 From: Natalie Rakoski Date: Thu, 13 May 2021 12:29:18 -0400 Subject: [PATCH 11/12] fix yaw offset after init maneuver --- src/plugins/autonomy/Square/Square.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/plugins/autonomy/Square/Square.cpp b/src/plugins/autonomy/Square/Square.cpp index c823f21880..61d8f6c3c7 100644 --- a/src/plugins/autonomy/Square/Square.cpp +++ b/src/plugins/autonomy/Square/Square.cpp @@ -204,6 +204,7 @@ std::deque Square::get_init_maneuver_positions() { // for (auto state : man_positions) { // cout << state.pos()(0) << ", " << state.pos()(1) << ", " << state.pos()(2) << "\n" << endl; +// // cout << state.quat().yaw() << endl; // } return man_positions; @@ -376,6 +377,7 @@ bool Square::step_autonomy(double t, double dt) { next_goal_ = goal2_; prev_diff_ = {0,0,0}; cout << "init maneuver finished at time: " << t << endl; + state_->set_quat(init_man_goal_quat_); } // Publish the Init Maneuver Status to send to AirSimSensor @@ -412,12 +414,16 @@ bool Square::step_autonomy(double t, double dt) { // do not count z in the velocity normalization v(2) = 0.0; // heading should stay facing the original heading - noisy_state_.set_quat(init_man_goal_quat_); + state_->set_quat(init_man_goal_quat_); prev_diff_ = diff; } else { // if not performing init maneuver + // make sure state us using the correct yaw + // Quaternion quat_now = noisy_state_.quat(); + // quat_now + // check if we have completed this side // float distance = sqrt(pow((init_goal_(0) - state_->pos()(0)), 2) + pow((init_goal_(1) - state_->pos()(1)), 2)); float distance = sqrt(pow((init_goal_(0) - noisy_state_.pos()(0)), 2) + pow((init_goal_(1) - noisy_state_.pos()(1)), 2)); @@ -488,10 +494,13 @@ bool Square::step_autonomy(double t, double dt) { prev_diff_ = diff; } + // cout << "yaw: " << noisy_state_.quat().yaw() << endl; + /////////////////////////////////////////////////////////////////////////// // Convert desired velocity to desired speed, heading, and pitch controls /////////////////////////////////////////////////////////////////////////// double heading = Angles::angle_2pi(atan2(v(1), v(0))); + // cout << "hdn: " << heading << endl; vars_.output(desired_alt_idx_, altitude); vars_.output(desired_speed_idx_, v.norm()); vars_.output(desired_heading_idx_, heading); From f8dcbb3f0b778770eec7aab3452119392aede81d Mon Sep 17 00:00:00 2001 From: Natalie Rakoski Date: Fri, 14 May 2021 21:04:12 -0400 Subject: [PATCH 12/12] fix yaw offset error between init maneuver and square/straight flight pattern --- .../plugins/autonomy/Square/Square.h | 2 + .../autonomy/StraightVOInit/StraightVOInit.h | 2 + .../sensor/AirSimSensor/AirSimSensor.h | 1 + missions/quad-airsim-ex-straight_VOinit.xml | 14 +- src/plugins/autonomy/Square/Square.cpp | 131 +++++------------- .../StraightVOInit/StraightVOInit.cpp | 58 +++++--- .../sensor/AirSimSensor/AirSimSensor.cpp | 40 ++---- 7 files changed, 106 insertions(+), 142 deletions(-) diff --git a/include/scrimmage/plugins/autonomy/Square/Square.h b/include/scrimmage/plugins/autonomy/Square/Square.h index e0131747b1..32f9398e1a 100644 --- a/include/scrimmage/plugins/autonomy/Square/Square.h +++ b/include/scrimmage/plugins/autonomy/Square/Square.h @@ -53,6 +53,8 @@ namespace autonomy { class InitManeuverType { public: bool active = false; + Quaternion init_man_goal_quat; + bool stay_straight = false; }; class Square : public scrimmage::Autonomy{ diff --git a/include/scrimmage/plugins/autonomy/StraightVOInit/StraightVOInit.h b/include/scrimmage/plugins/autonomy/StraightVOInit/StraightVOInit.h index 3a6a2f998a..97a23af769 100644 --- a/include/scrimmage/plugins/autonomy/StraightVOInit/StraightVOInit.h +++ b/include/scrimmage/plugins/autonomy/StraightVOInit/StraightVOInit.h @@ -52,6 +52,8 @@ namespace autonomy { class InitManeuverType { public: bool active = false; + Quaternion init_man_goal_quat; + bool stay_straight = false; }; class StraightVOInit : public scrimmage::Autonomy{ diff --git a/include/scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.h b/include/scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.h index d85c99e744..99ac5a877c 100644 --- a/include/scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.h +++ b/include/scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.h @@ -195,6 +195,7 @@ class AirSimSensor : public scrimmage::Sensor { // Init Maneuver bool init_maneuver_active_ = false; Quaternion init_man_orig_quat_; + bool stay_straight_ = false; // period at which the data acquisition is run [seconds] // double data_acquisition_period_ = .1; diff --git a/missions/quad-airsim-ex-straight_VOinit.xml b/missions/quad-airsim-ex-straight_VOinit.xml index 64832d9d7c..4b5a630658 100644 --- a/missions/quad-airsim-ex-straight_VOinit.xml +++ b/missions/quad-airsim-ex-straight_VOinit.xml @@ -3,7 +3,7 @@ - Boundary @@ -138,8 +138,10 @@ + StraightVOInit + use_init_maneuver="true" + enable_boundary_control="false">StraightVOInit @@ -246,8 +248,10 @@ + StraightVOInit + use_init_maneuver="true" + enable_boundary_control="false">StraightVOInit diff --git a/src/plugins/autonomy/Square/Square.cpp b/src/plugins/autonomy/Square/Square.cpp index 61d8f6c3c7..8fbf9c71e4 100644 --- a/src/plugins/autonomy/Square/Square.cpp +++ b/src/plugins/autonomy/Square/Square.cpp @@ -89,121 +89,50 @@ std::deque Square::get_init_maneuver_positions() { State current_state(*state); // man_positions.push_back(current_state); - // Rise up to 5 meters above the starting position -// for (double i=0.0; i<5.0; i+=delta) { -// current_state.pos()(2) = current_state.pos()(2) + delta; -// man_positions.push_back(current_state); -// } // cout << "starting_state: " << current_state.pos() << endl; // add 5m in altitude - // current_state.pos()(2) = current_state.pos()(2) + 5.0; + current_state.pos()(2) = current_state.pos()(2) + 3.0; man_positions.push_back(current_state); -// // repeat the below right-left movements 1 times -// for (int repeat = 0; repeat<1; repeat++){ -// // move 2 meters to the right, back to the center -// for (double i=0.0; i<5.0; i+=delta) { -// current_state.pos()(1) = current_state.pos()(1) + delta;; -// man_positions.push_back(current_state); -// } -// for (double i=0.0; i<5.0; i+=delta) { -// current_state.pos()(1) = current_state.pos()(1) - delta; -// man_positions.push_back(current_state); -// } -// // move 2 meters to the left, back to the center -// for (double i=0.0; i<5.0; i+=delta) { -// current_state.pos()(1) = current_state.pos()(1) - delta; -// man_positions.push_back(current_state); -// } -// for (double i=0.0; i<5.0; i+=delta) { -// current_state.pos()(1) = current_state.pos()(1) + delta; -// man_positions.push_back(current_state); -// } -// } // move left - right - current_state.pos()(1) = current_state.pos()(1) + 4.0; + current_state.pos()(1) = current_state.pos()(1) + 3.0; man_positions.push_back(current_state); - current_state.pos()(1) = current_state.pos()(1) - 8.0; + current_state.pos()(1) = current_state.pos()(1) - 6.0; man_positions.push_back(current_state); - current_state.pos()(1) = current_state.pos()(1) + 4.0; + current_state.pos()(1) = current_state.pos()(1) + 3.0; man_positions.push_back(current_state); -// // repeat the below up-down movements 3 times -// for (int repeat = 0; repeat<1; repeat++){ -// // move 1 meter up, back to the center -// for (double i=0.0; i<5.0; i+=delta) { -// current_state.pos()(2) = current_state.pos()(2) + delta; -// man_positions.push_back(current_state); -// } -// for (double i=0.0; i<5.0; i+=delta) { -// current_state.pos()(2) = current_state.pos()(2) - delta; -// man_positions.push_back(current_state); -// } -// // move 1 meter down, back to the center -// for (double i=0.0; i<5.0; i+=delta) { -// current_state.pos()(2) = current_state.pos()(2) - delta; -// man_positions.push_back(current_state); -// } -// for (double i=0.0; i<5.0; i+=delta) { -// current_state.pos()(2) = current_state.pos()(2) + delta; -// man_positions.push_back(current_state); -// } -// } // move up-down - current_state.pos()(2) = current_state.pos()(2) + 4.0; + current_state.pos()(2) = current_state.pos()(2) + 2.0; man_positions.push_back(current_state); - current_state.pos()(2) = current_state.pos()(2) - 8.0; + current_state.pos()(2) = current_state.pos()(2) - 4.0; man_positions.push_back(current_state); - current_state.pos()(2) = current_state.pos()(2) + 4.0; + current_state.pos()(2) = current_state.pos()(2) + 2.0; man_positions.push_back(current_state); -// // move in a 4mx2m rectangle parallel to the ground 3 times -// for (int repeat = 0; repeat<1; repeat++){ -// // move 1 meter in the +y direction -// for (double i=0.0; i<5.0; i+=delta) { -// current_state.pos()(1) = current_state.pos()(1) + delta; -// man_positions.push_back(current_state); -// } -// // move 1 meter in the +x direction -// for (double i=0.0; i<5.0; i+=delta) { -// current_state.pos()(0) = current_state.pos()(0) + delta; -// man_positions.push_back(current_state); -// } -// // move 2 meters in the -y direction -// for (double i=0.0; i<10.0; i+=delta) { -// current_state.pos()(1) = current_state.pos()(1) - delta; -// man_positions.push_back(current_state); -// } -// // move 1 meters in the -x direction -// for (double i=0.0; i<5.0; i+=delta) { -// current_state.pos()(0) = current_state.pos()(0) - delta; -// man_positions.push_back(current_state); -// } -// // move 1 meter in the +y direction -// for (double i=0.0; i<5.0; i+=delta) { -// current_state.pos()(1) = current_state.pos()(1) + delta; -// man_positions.push_back(current_state); -// } -// } // move in a rectangle - right, forward, left, backward, right - current_state.pos()(1) = current_state.pos()(1) + 4.0; + current_state.pos()(1) = current_state.pos()(1) + 2.0; man_positions.push_back(current_state); - current_state.pos()(0) = current_state.pos()(0) + 4.0; + current_state.pos()(0) = current_state.pos()(0) + 2.0; man_positions.push_back(current_state); - current_state.pos()(1) = current_state.pos()(1) - 8.0; + current_state.pos()(1) = current_state.pos()(1) - 4.0; man_positions.push_back(current_state); - current_state.pos()(0) = current_state.pos()(0) - 4.0; + current_state.pos()(0) = current_state.pos()(0) - 2.0; man_positions.push_back(current_state); - current_state.pos()(1) = current_state.pos()(1) + 4.0; + current_state.pos()(1) = current_state.pos()(1) + 2.0; man_positions.push_back(current_state); // place back at beginning for Straight Plugin + // 5.0 = 0.14, 3.0 = -0.15, 0 = -1.9 man_positions.push_back(init_state); + init_state.pos()(0) = init_state.pos()(0) + 5.0; man_positions.push_back(init_state); + init_state.pos()(0) = init_state.pos()(0) + 5.0; man_positions.push_back(init_state); // for (auto state : man_positions) { // cout << state.pos()(0) << ", " << state.pos()(1) << ", " << state.pos()(2) << "\n" << endl; +// // cout << state.quat().roll() << ", " << state.quat().pitch() << ", " << state.quat().yaw() << "\n" << endl; // // cout << state.quat().yaw() << endl; // } @@ -219,6 +148,7 @@ void Square::init(std::map ¶ms) { // Save the initial starting position init_goal_ = state_->pos(); + init_man_goal_quat_ = state_->quat(); // initialize the corner locations of the square cout << "[SquareAutonomy] Square Corner Locations:" << endl; // positve x direction @@ -360,6 +290,7 @@ void Square::init(std::map ¶ms) { } bool Square::step_autonomy(double t, double dt) { + sc::StatePtr &state_ = parent_->state_truth(); // Read data from sensors... if (!noisy_state_set_) { @@ -371,13 +302,18 @@ bool Square::step_autonomy(double t, double dt) { Eigen::Vector3d v; double altitude = goal_(2); + // If Init Maneuver is finished if (man_positions_.size() == 0 && use_init_maneuver_ && init_maneuver_finished_ == false) { init_maneuver_finished_ = true; goal_ = goal1_; next_goal_ = goal2_; prev_diff_ = {0,0,0}; cout << "init maneuver finished at time: " << t << endl; + // Use the keep straight action one last time + parent_->state_truth()->set_quat(init_man_goal_quat_); + state_ = parent_->state_truth(); state_->set_quat(init_man_goal_quat_); + noisy_state_ = *state_; } // Publish the Init Maneuver Status to send to AirSimSensor @@ -385,8 +321,21 @@ bool Square::step_autonomy(double t, double dt) { // for Visual Odometry init maneuver we need the camera to stay facing forward auto msg = std::make_shared>(); msg->data.active = !init_maneuver_finished_; + msg->data.init_man_goal_quat = init_man_goal_quat_; + // if using init maneuver, keep the quadcopter at the same roll, pitch, yaw during the init maneuver + if (init_maneuver_finished_ == false) { + // Keep the quadcopter Straight + msg->data.stay_straight = true; + parent_->state_truth()->set_quat(init_man_goal_quat_); + state_ = parent_->state_truth(); + state_->set_quat(init_man_goal_quat_); + noisy_state_ = *state_; + } else { + msg->data.stay_straight = false; + } init_maneuver_pub_->publish(msg); + // Perform Init Maneuver if (!init_maneuver_finished_ && use_init_maneuver_) { // Pop a state off the front of man_positions_ vector and delete the element float distance = sqrt(pow((init_man_goal_pos_(0) - noisy_state_.pos()(0)), 2) + pow((init_man_goal_pos_(1) - noisy_state_.pos()(1)), 2) + pow((init_man_goal_pos_(2) - noisy_state_.pos()(2)), 2)); @@ -413,19 +362,13 @@ bool Square::step_autonomy(double t, double dt) { altitude = noisy_state_.pos()(2) + v(2)*dt; // do not count z in the velocity normalization v(2) = 0.0; - // heading should stay facing the original heading - state_->set_quat(init_man_goal_quat_); prev_diff_ = diff; } else { // if not performing init maneuver - // make sure state us using the correct yaw - // Quaternion quat_now = noisy_state_.quat(); - // quat_now - // check if we have completed this side - // float distance = sqrt(pow((init_goal_(0) - state_->pos()(0)), 2) + pow((init_goal_(1) - state_->pos()(1)), 2)); + // This gives us distance travelled from the start to the current location on the current side of the square float distance = sqrt(pow((init_goal_(0) - noisy_state_.pos()(0)), 2) + pow((init_goal_(1) - noisy_state_.pos()(1)), 2)); if (distance >= sq_side_length_m_ && square_side_ > 0){ // update which side of the square we are on. diff --git a/src/plugins/autonomy/StraightVOInit/StraightVOInit.cpp b/src/plugins/autonomy/StraightVOInit/StraightVOInit.cpp index a3a939cf64..29c6ce366b 100644 --- a/src/plugins/autonomy/StraightVOInit/StraightVOInit.cpp +++ b/src/plugins/autonomy/StraightVOInit/StraightVOInit.cpp @@ -89,49 +89,52 @@ std::deque StraightVOInit::get_init_maneuver_positions() { State current_state(*state); // man_positions.push_back(current_state); - // Rise up to 5 meters above the starting position -// for (double i=0.0; i<5.0; i+=delta) { -// current_state.pos()(2) = current_state.pos()(2) + delta; -// man_positions.push_back(current_state); -// } // cout << "starting_state: " << current_state.pos() << endl; // add 5m in altitude - // current_state.pos()(2) = current_state.pos()(2) + 5.0; + current_state.pos()(2) = current_state.pos()(2) + 3.0; man_positions.push_back(current_state); // move left - right - current_state.pos()(1) = current_state.pos()(1) + 4.0; + current_state.pos()(1) = current_state.pos()(1) + 3.0; man_positions.push_back(current_state); - current_state.pos()(1) = current_state.pos()(1) - 8.0; + current_state.pos()(1) = current_state.pos()(1) - 6.0; man_positions.push_back(current_state); - current_state.pos()(1) = current_state.pos()(1) + 4.0; + current_state.pos()(1) = current_state.pos()(1) + 3.0; man_positions.push_back(current_state); // move up-down - current_state.pos()(2) = current_state.pos()(2) + 4.0; + current_state.pos()(2) = current_state.pos()(2) + 2.0; man_positions.push_back(current_state); - current_state.pos()(2) = current_state.pos()(2) - 8.0; + current_state.pos()(2) = current_state.pos()(2) - 4.0; man_positions.push_back(current_state); - current_state.pos()(2) = current_state.pos()(2) + 4.0; + current_state.pos()(2) = current_state.pos()(2) + 2.0; man_positions.push_back(current_state); // move in a rectangle - right, forward, left, backward, right - current_state.pos()(1) = current_state.pos()(1) + 4.0; + current_state.pos()(1) = current_state.pos()(1) + 2.0; man_positions.push_back(current_state); - current_state.pos()(0) = current_state.pos()(0) + 4.0; + current_state.pos()(0) = current_state.pos()(0) + 2.0; man_positions.push_back(current_state); - current_state.pos()(1) = current_state.pos()(1) - 8.0; + current_state.pos()(1) = current_state.pos()(1) - 4.0; man_positions.push_back(current_state); - current_state.pos()(0) = current_state.pos()(0) - 4.0; + current_state.pos()(0) = current_state.pos()(0) - 2.0; man_positions.push_back(current_state); - current_state.pos()(1) = current_state.pos()(1) + 4.0; + current_state.pos()(1) = current_state.pos()(1) + 2.0; man_positions.push_back(current_state); // place back at beginning for Straight Plugin + // 5.0 = 0.14, 3.0 = -0.15, 0 = -1.9 man_positions.push_back(init_state); + init_state.pos()(0) = init_state.pos()(0) + 5.0; man_positions.push_back(init_state); + init_state.pos()(0) = init_state.pos()(0) + 5.0; man_positions.push_back(init_state); +// for (auto state : man_positions) { +// cout << state.pos()(0) << ", " << state.pos()(1) << ", " << state.pos()(2) << "\n" << endl; +// // cout << state.quat().roll() << ", " << state.quat().pitch() << ", " << state.quat().yaw() << "\n" << endl; +// // cout << state.quat().yaw() << endl; +// } return man_positions; } @@ -148,6 +151,7 @@ void StraightVOInit::init(std::map ¶ms) { goal_ = state_->pos() + unit_vector * rel_pos.norm(); goal_(2) = state_->pos()(2); init_goal_ = goal_; + init_man_goal_quat_ = state_->quat(); // Set the desired_z to our initial position. // desired_z_ = state_->pos()(2); @@ -272,6 +276,7 @@ void StraightVOInit::init(std::map ¶ms) { } bool StraightVOInit::step_autonomy(double t, double dt) { + sc::StatePtr &state_ = parent_->state_truth(); // Read data from sensors... if (!noisy_state_set_) { @@ -283,11 +288,17 @@ bool StraightVOInit::step_autonomy(double t, double dt) { Eigen::Vector3d v; double altitude = goal_(2); + // If Init Maneuver is finished if (man_positions_.size() == 0 && use_init_maneuver_ && init_maneuver_finished_ == false) { init_maneuver_finished_ = true; goal_ = init_goal_; prev_diff_ = {0,0,0}; cout << "init maneuver finished at time: " << t << endl; + // Use the keep straight action one last time + parent_->state_truth()->set_quat(init_man_goal_quat_); + state_ = parent_->state_truth(); + state_->set_quat(init_man_goal_quat_); + noisy_state_ = *state_; } // Publish the Init Maneuver Status to send to AirSimSensor @@ -295,8 +306,21 @@ bool StraightVOInit::step_autonomy(double t, double dt) { // for Visual Odometry init maneuver we need the camera to stay facing forward auto msg = std::make_shared>(); msg->data.active = !init_maneuver_finished_; + msg->data.init_man_goal_quat = init_man_goal_quat_; + // if using init maneuver, keep the quadcopter at the same roll, pitch, yaw during the init maneuver + if (init_maneuver_finished_ == false) { + // Keep the quadcopter Straight + msg->data.stay_straight = true; + parent_->state_truth()->set_quat(init_man_goal_quat_); + state_ = parent_->state_truth(); + state_->set_quat(init_man_goal_quat_); + noisy_state_ = *state_; + } else { + msg->data.stay_straight = false; + } init_maneuver_pub_->publish(msg); + // Perform Init Maneuver if (!init_maneuver_finished_ && use_init_maneuver_) { // Pop a state off the front of man_positions_ vector and delete the element float distance = sqrt(pow((init_man_goal_pos_(0) - noisy_state_.pos()(0)), 2) + pow((init_man_goal_pos_(1) - noisy_state_.pos()(1)), 2) + pow((init_man_goal_pos_(2) - noisy_state_.pos()(2)), 2)); diff --git a/src/plugins/sensor/AirSimSensor/AirSimSensor.cpp b/src/plugins/sensor/AirSimSensor/AirSimSensor.cpp index d025182d6f..cd519e8674 100644 --- a/src/plugins/sensor/AirSimSensor/AirSimSensor.cpp +++ b/src/plugins/sensor/AirSimSensor/AirSimSensor.cpp @@ -232,8 +232,12 @@ void AirSimSensor::init(std::map ¶ms) { ///// Init Maneuver ////// // Subscribe to initialization maneuver active flag + sc::StatePtr &state = parent_->state_truth(); + init_man_orig_quat_ = state->quat(); auto init_man_cb = [&](auto &msg) { init_maneuver_active_ = msg->data.active; + init_man_orig_quat_ = msg->data.init_man_goal_quat; + stay_straight_ = msg->data.stay_straight; }; subscribe("LocalNetwork", "InitManeuver", init_man_cb); if (init_maneuver_active_) { @@ -631,27 +635,14 @@ bool AirSimSensor::step() { // Setup state information for AirSim sc::StatePtr &state = parent_->state_truth(); - - // record init state - if (airsim_frame_num_ == 0){ - sc::StatePtr &init_state = parent_->state_truth(); - init_man_orig_quat_ = init_state->quat(); - } - State next_state(*state); -// if (man_positions_.size() == 0) { -// init_maneuver_finished_ = true; -// // reset to the original state -// // parent_->state_truth() = init_state_; -// std::vector autos = parent_->autonomies(); -// // autos.front()->set_desired_state(init_state_); -// for (auto a : autos) { -// // (*a).set_desired_state(init_state_); -// // autos.front()->set_desired_state(init_state_); -// a->set_desired_state(init_state_); -// } -// } + // if using init maneuver, keep the quadcopter at the same roll, pitch, yaw during the init maneuver + // and for a little while afterwards in order to ensure a smooth transition. + if (stay_straight_ == true) { + parent_->state_truth()->set_quat(init_man_orig_quat_); + next_state.set_quat(init_man_orig_quat_); + } if (init_maneuver_active_) { // move the quadcopter, but keep its heading facing forward so that the camera can @@ -665,9 +656,6 @@ bool AirSimSensor::step() { // pitch, roll, yaw // note, the negative pitch and yaw are required because of the wsu coordinate frame -// ma::Quaternionr qd = ma::VectorMath::toQuaternion(-state->quat().pitch(), -// state->quat().roll(), -// airsim_yaw_rad); ma::Quaternionr qd = ma::VectorMath::toQuaternion(-init_man_orig_quat_.pitch(), init_man_orig_quat_.roll(), airsim_yaw_rad); @@ -677,15 +665,15 @@ bool AirSimSensor::step() { } else { // Go through the motions of the autonomy plugin defined in the mission file. // convert from ENU to NED frame - ma::Vector3r pos(state->pos()(1), state->pos()(0), -state->pos()(2)); + ma::Vector3r pos(next_state.pos()(1), next_state.pos()(0), -next_state.pos()(2)); - enu_to_ned_yaw_.set_angle(ang::rad2deg(state->quat().yaw())); + enu_to_ned_yaw_.set_angle(ang::rad2deg(next_state.quat().yaw())); double airsim_yaw_rad = ang::deg2rad(enu_to_ned_yaw_.angle()); // pitch, roll, yaw // note, the negative pitch and yaw are required because of the wsu coordinate frame - ma::Quaternionr qd = ma::VectorMath::toQuaternion(-state->quat().pitch(), - state->quat().roll(), + ma::Quaternionr qd = ma::VectorMath::toQuaternion(-next_state.quat().pitch(), + next_state.quat().roll(), airsim_yaw_rad); // Send state information to AirSim