diff --git a/include/scrimmage/plugins/autonomy/Square/Square.h b/include/scrimmage/plugins/autonomy/Square/Square.h new file mode 100644 index 0000000000..32f9398e1a --- /dev/null +++ b/include/scrimmage/plugins/autonomy/Square/Square.h @@ -0,0 +1,110 @@ +/*! + * @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 InitManeuverType { + public: + bool active = false; + Quaternion init_man_goal_quat; + bool stay_straight = false; +}; + +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 next_goal_; + Eigen::Vector3d goal1_; + Eigen::Vector3d goal2_; + Eigen::Vector3d goal3_; + Eigen::Vector3d goal4_; + + int frame_number_; + bool show_camera_images_; + bool save_camera_images_; + + 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 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_; + + 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}; + +}; +} // 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..23810d131c --- /dev/null +++ b/include/scrimmage/plugins/autonomy/Square/Square.xml @@ -0,0 +1,14 @@ + + + + Square_plugin + 21 + false + false + + + false + 100 + + 10 + diff --git a/include/scrimmage/plugins/autonomy/StraightVOInit/StraightVOInit.h b/include/scrimmage/plugins/autonomy/StraightVOInit/StraightVOInit.h new file mode 100644 index 0000000000..97a23af769 --- /dev/null +++ b/include/scrimmage/plugins/autonomy/StraightVOInit/StraightVOInit.h @@ -0,0 +1,98 @@ +/*! + * @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; + Quaternion init_man_goal_quat; + bool stay_straight = 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/include/scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.h b/include/scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.h index 37bce8ec10..99ac5a877c 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 @@ -132,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; @@ -189,6 +191,12 @@ class AirSimSensor : public scrimmage::Sensor { bool get_image_data_ = true; bool get_lidar_data_ = true; bool get_imu_data_ = true; + + // 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; double image_acquisition_period_ = .1; 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/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-ex-square.xml b/missions/quad-airsim-ex-square.xml new file mode 100644 index 0000000000..dc7d643391 --- /dev/null +++ b/missions/quad-airsim-ex-square.xml @@ -0,0 +1,274 @@ + + + + + + + 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-ex-straight_VOinit.xml b/missions/quad-airsim-ex-straight_VOinit.xml new file mode 100644 index 0000000000..4b5a630658 --- /dev/null +++ b/missions/quad-airsim-ex-straight_VOinit.xml @@ -0,0 +1,282 @@ + + + + + + + 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/Square/CMakeLists.txt b/src/plugins/autonomy/Square/CMakeLists.txt new file mode 100644 index 0000000000..d9e8af868c --- /dev/null +++ b/src/plugins/autonomy/Square/CMakeLists.txt @@ -0,0 +1,42 @@ +#-------------------------------------------------------- +# Library Creation +#-------------------------------------------------------- +SET (LIBRARY_NAME Square_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/Square/Square.cpp b/src/plugins/autonomy/Square/Square.cpp new file mode 100644 index 0000000000..8fbf9c71e4 --- /dev/null +++ b/src/plugins/autonomy/Square/Square.cpp @@ -0,0 +1,455 @@ +/*! + * @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. + * + */ + +#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); + + // cout << "starting_state: " << current_state.pos() << endl; + // add 5m in altitude + 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) + 3.0; + man_positions.push_back(current_state); + current_state.pos()(1) = current_state.pos()(1) - 6.0; + man_positions.push_back(current_state); + 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) + 2.0; + man_positions.push_back(current_state); + current_state.pos()(2) = current_state.pos()(2) - 4.0; + man_positions.push_back(current_state); + 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) + 2.0; + man_positions.push_back(current_state); + 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; + man_positions.push_back(current_state); + current_state.pos()(0) = current_state.pos()(0) - 2.0; + man_positions.push_back(current_state); + 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; +} + +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); + start_corner_turn_ = scrimmage::get("start_corner_turn", params, 10); + + // 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 + goal1_ = state_->pos(); + goal1_(0) = goal1_(0) + sq_side_length_m_; + 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_(0) << ", " << goal2_(1) << ", " << goal2_(2) << endl; + // negative x direction + goal3_ = goal2_; + goal3_(0) = goal3_(0) - sq_side_length_m_; + 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_(0) << ", " << goal4_(1) << ", " << goal4_(2) << endl; + + // Save the initial starting position + // cout << "starting position: " << 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"); + // 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 << "[SquareAutonomy] 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(); + + } else { + goal_ = goal1_; + next_goal_ = goal2_; + } + + 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 Square::step_autonomy(double t, double dt) { + sc::StatePtr &state_ = parent_->state_truth(); + + // 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 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 + // 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_; + 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)); + 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; + prev_diff_ = diff; + + } else { + // if not performing init maneuver + + // check if we have completed this side + // 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. + square_side_ += 1; + init_goal_ = goal_; + + // rotate the goal + int current_side = square_side_ % 4; + switch(current_side) { + case 1: + { + // Move in positive X direction + goal_ = goal1_; + next_goal_ = goal2_; + break; + } + case 2 : + { + // Move in positive Y direction + goal_ = goal2_; + next_goal_ = goal3_; + break; + } + case 3: + { + // Move in negative X direction + goal_ = goal3_; + next_goal_ = goal4_; + break; + } + case 0: + { + // Move in negative Y direction + goal_ = goal4_; + next_goal_ = goal1_; + break; + } + } + } // 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 = 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; + } + + // 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); + + noisy_state_set_ = false; + return true; +} +} // namespace autonomy +} // namespace scrimmage 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..29c6ce366b --- /dev/null +++ b/src/plugins/autonomy/StraightVOInit/StraightVOInit.cpp @@ -0,0 +1,386 @@ +/*! + * @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); + + // cout << "starting_state: " << current_state.pos() << endl; + // add 5m in altitude + 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) + 3.0; + man_positions.push_back(current_state); + current_state.pos()(1) = current_state.pos()(1) - 6.0; + man_positions.push_back(current_state); + 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) + 2.0; + man_positions.push_back(current_state); + current_state.pos()(2) = current_state.pos()(2) - 4.0; + man_positions.push_back(current_state); + 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) + 2.0; + man_positions.push_back(current_state); + 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; + man_positions.push_back(current_state); + current_state.pos()(0) = current_state.pos()(0) - 2.0; + man_positions.push_back(current_state); + 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; +} + +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_; + init_man_goal_quat_ = state_->quat(); + + // 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) { + sc::StatePtr &state_ = parent_->state_truth(); + + // 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 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 + // 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_; + 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)); + 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 diff --git a/src/plugins/sensor/AirSimSensor/AirSimSensor.cpp b/src/plugins/sensor/AirSimSensor/AirSimSensor.cpp index 607227e4ff..cd519e8674 100644 --- a/src/plugins/sensor/AirSimSensor/AirSimSensor.cpp +++ b/src/plugins/sensor/AirSimSensor/AirSimSensor.cpp @@ -41,8 +41,8 @@ #include #include #include -#include -#include +#include +#include #include #include @@ -230,6 +230,19 @@ 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); + ///// 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_) { + cout << "[AirSimSensor] Start VO Initialization Maneuver." << endl; + } // Open airsim_data CSV for append (app) and set column headers std::string csv_filename = @@ -622,20 +635,66 @@ 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)); + State next_state(*state); - enu_to_ned_yaw_.set_angle(ang::rad2deg(state->quat().yaw())); - double airsim_yaw_rad = ang::deg2rad(enu_to_ned_yaw_.angle()); + // 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_); + } - // 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); + 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(state->pos()(1), state->pos()(0), -state->pos()(2)); + + 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(-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_); + + } else { + // 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)); + + 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_); + } - // Send state information to AirSim - 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_) { @@ -656,7 +715,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); } @@ -706,7 +765,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(); @@ -735,12 +794,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; } diff --git a/src/plugins/sensor/ROSAltimeter/ROSAltimeter.cpp b/src/plugins/sensor/ROSAltimeter/ROSAltimeter.cpp index aaa5421e48..fdde55b31b 100644 --- a/src/plugins/sensor/ROSAltimeter/ROSAltimeter.cpp +++ b/src/plugins/sensor/ROSAltimeter/ROSAltimeter.cpp @@ -79,6 +79,14 @@ void ROSAltimeter::init(std::map ¶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