From 512adb7460dd95cd36cfce14c358c76e10a417e9 Mon Sep 17 00:00:00 2001 From: Natalie Date: Wed, 20 Sep 2023 21:35:11 -0400 Subject: [PATCH 01/13] Initial commit for basic formation control --- .../plugins/autonomy/Formation/Formation.h | 89 +++++ .../plugins/autonomy/Formation/Formation.xml | 8 + missions/v_formation.xml | 366 ++++++++++++++++++ missions/waypoint.xml | 366 ++++++++++++++++++ missions/waypoint_follower.xml | 67 +++- msgs/Event.proto | 6 + src/plugins/autonomy/Formation/CMakeLists.txt | 42 ++ src/plugins/autonomy/Formation/Formation.cpp | 205 ++++++++++ 8 files changed, 1144 insertions(+), 5 deletions(-) create mode 100644 include/scrimmage/plugins/autonomy/Formation/Formation.h create mode 100644 include/scrimmage/plugins/autonomy/Formation/Formation.xml create mode 100644 missions/v_formation.xml create mode 100644 missions/waypoint.xml create mode 100644 src/plugins/autonomy/Formation/CMakeLists.txt create mode 100644 src/plugins/autonomy/Formation/Formation.cpp diff --git a/include/scrimmage/plugins/autonomy/Formation/Formation.h b/include/scrimmage/plugins/autonomy/Formation/Formation.h new file mode 100644 index 0000000000..02afa91e1a --- /dev/null +++ b/include/scrimmage/plugins/autonomy/Formation/Formation.h @@ -0,0 +1,89 @@ +/*! + * @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_FORMATION_FORMATION_H_ +#define INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_FORMATION_FORMATION_H_ +#include +#include + +#include + +#include +#include +#include + +namespace scrimmage { + +namespace interaction { +class BoundaryBase; +} + +namespace autonomy { +class Formation : public scrimmage::Autonomy{ + public: + void init(std::map ¶ms) override; + bool step_autonomy(double t, double dt) override; + + protected: + double speed_; + Eigen::Vector3d goal_; + + int frame_number_; + bool leader_; + + 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; + + double prev_gen_time_ = -1.0; + + scrimmage::PublisherPtr leader_pub_; + // float leader_x_pos; + // float leader_y_pos; + // float leader_z_pos; +}; +} // namespace autonomy +} // namespace scrimmage +#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_FORMATION_FORMATION_H_ diff --git a/include/scrimmage/plugins/autonomy/Formation/Formation.xml b/include/scrimmage/plugins/autonomy/Formation/Formation.xml new file mode 100644 index 0000000000..b37800d1c5 --- /dev/null +++ b/include/scrimmage/plugins/autonomy/Formation/Formation.xml @@ -0,0 +1,8 @@ + + + + Formation_plugin + 21 + false + false + diff --git a/missions/v_formation.xml b/missions/v_formation.xml new file mode 100644 index 0000000000..63b6bf0c6f --- /dev/null +++ b/missions/v_formation.xml @@ -0,0 +1,366 @@ + + + + + + + false + 50051 + localhost + + time, all_dead + + 10 + 1000 + + + 191 191 191 + 10 + + true + all + false + false + + SimpleCollisionMetrics + + ~/.scrimmage/logs + true + + 35.721025 + -120.767925 + 300 + true + 10 + + Boundary + SimpleCollision + GroundCollision + + false + + GlobalNetwork + LocalNetwork + + + 2147483648 + + + + + + + + uav_entity + 1 + 1 + 1 + 1 + + -900 + 0 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + Straight + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + uav_entity + 1 + 1 + 1 + 1 + + -903 + 3 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + Straight + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + uav_entity + 1 + 1 + 1 + 1 + + -903 + -3 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + Straight + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + uav_entity + 1 + 1 + 1 + 1 + + -906 + 6 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + Straight + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + uav_entity + 1 + 1 + 1 + 1 + + -906 + -6 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + Straight + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + + + + + uav_entity + 2 + 1 + 1 + 1 + + 900 + 0 + 195 + 180 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-red + + Straight + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + uav_entity + 2 + 1 + 1 + 1 + + 903 + 3 + 195 + 180 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-red + + Straight + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + uav_entity + 2 + 1 + 1 + 1 + + 903 + -3 + 195 + 180 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-red + + Straight + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + uav_entity + 2 + 1 + 1 + 1 + + 906 + 6 + 195 + 180 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-red + + Straight + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + uav_entity + 2 + 1 + 1 + 1 + + 906 + -6 + 195 + 180 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-red + + Straight + + + 35.721112 + -120.770305 + 300 + 25 + + + diff --git a/missions/waypoint.xml b/missions/waypoint.xml new file mode 100644 index 0000000000..63b6bf0c6f --- /dev/null +++ b/missions/waypoint.xml @@ -0,0 +1,366 @@ + + + + + + + false + 50051 + localhost + + time, all_dead + + 10 + 1000 + + + 191 191 191 + 10 + + true + all + false + false + + SimpleCollisionMetrics + + ~/.scrimmage/logs + true + + 35.721025 + -120.767925 + 300 + true + 10 + + Boundary + SimpleCollision + GroundCollision + + false + + GlobalNetwork + LocalNetwork + + + 2147483648 + + + + + + + + uav_entity + 1 + 1 + 1 + 1 + + -900 + 0 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + Straight + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + uav_entity + 1 + 1 + 1 + 1 + + -903 + 3 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + Straight + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + uav_entity + 1 + 1 + 1 + 1 + + -903 + -3 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + Straight + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + uav_entity + 1 + 1 + 1 + 1 + + -906 + 6 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + Straight + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + uav_entity + 1 + 1 + 1 + 1 + + -906 + -6 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + Straight + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + + + + + uav_entity + 2 + 1 + 1 + 1 + + 900 + 0 + 195 + 180 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-red + + Straight + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + uav_entity + 2 + 1 + 1 + 1 + + 903 + 3 + 195 + 180 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-red + + Straight + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + uav_entity + 2 + 1 + 1 + 1 + + 903 + -3 + 195 + 180 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-red + + Straight + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + uav_entity + 2 + 1 + 1 + 1 + + 906 + 6 + 195 + 180 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-red + + Straight + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + uav_entity + 2 + 1 + 1 + 1 + + 906 + -6 + 195 + 180 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-red + + Straight + + + 35.721112 + -120.770305 + 300 + 25 + + + diff --git a/missions/waypoint_follower.xml b/missions/waypoint_follower.xml index 3a0cb7d56a..2e984a3155 100644 --- a/missions/waypoint_follower.xml +++ b/missions/waypoint_follower.xml @@ -46,7 +46,7 @@ GlobalNetwork LocalNetwork - + 1 77 77 255 @@ -65,14 +65,14 @@ WaypointDispatcher - MotorSchemas - + 1 77 77 255 @@ -86,7 +86,64 @@ 0 sea-angler - WaypointGenerator + + + WaypointGenerator + + + + + 2 + 77 77 255 + 1 + 1 + 1 + + 0 + 5 + 100 + 0 + SimpleAircraftControllerPID + SimpleAircraft + zephyr-red + + WaypointDispatcher + + MotorSchemas + + ===================== TEAM 2 Ground Control Station ====================== + + diff --git a/msgs/Event.proto b/msgs/Event.proto index 14528f0fe0..3bd5bc2ac9 100644 --- a/msgs/Event.proto +++ b/msgs/Event.proto @@ -53,3 +53,9 @@ message GenerateEntity { int32 entity_id = 4; repeated KeyValueAttr plugin_param = 5; } + +message FormationLeader{ + float x_pos = 1; + float y_pos = 2; + float z_pos = 3; +} diff --git a/src/plugins/autonomy/Formation/CMakeLists.txt b/src/plugins/autonomy/Formation/CMakeLists.txt new file mode 100644 index 0000000000..08733f52a5 --- /dev/null +++ b/src/plugins/autonomy/Formation/CMakeLists.txt @@ -0,0 +1,42 @@ +#-------------------------------------------------------- +# Library Creation +#-------------------------------------------------------- +SET (LIBRARY_NAME Formation_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/Formation/Formation.cpp b/src/plugins/autonomy/Formation/Formation.cpp new file mode 100644 index 0000000000..810b9a84d9 --- /dev/null +++ b/src/plugins/autonomy/Formation/Formation.cpp @@ -0,0 +1,205 @@ +/*! + * @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 + +#include + +namespace sc = scrimmage; +namespace sp = scrimmage_proto; +namespace sci = scrimmage::interaction; + +using std::cout; +using std::endl; + +#include + +#define BOOST_NO_CXX11_SCOPED_ENUMS +#include +#undef BOOST_NO_CXX11_SCOPED_ENUMS +namespace fs = boost::filesystem; + +REGISTER_PLUGIN(scrimmage::Autonomy, + scrimmage::autonomy::Formation, + Formation_plugin) + +namespace scrimmage { +namespace autonomy { + +void Formation::init(std::map ¶ms) { + speed_ = scrimmage::get("speed", params, 0.0); + leader_ = scrimmage::get("leader", 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); + + // Nat - what if goal isn't in front? see if it turns + // Should probably have a tag that is for leader/follower with the given entity + // Instead of speed, probably want displacement compared to the leader instead. + // Might be able to stack autonomies for go to waypoint and avoid entity MS + // + // Will need logic if the entitiy is not the leader. If it is the leader, it can + // just continue to fly straight. Maybe a good way to do this if it is not the leader, + // it should publish messages from follower, saying it is in the formation. The leader + // should publish messages with its location, that way the follower can do proper displacement + + // 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); + + frame_number_ = 0; + + 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) { // Nat - this may be good for locating entities nearby + noisy_contacts_ = msg->data; // Save map of noisy contacts + }; + subscribe("LocalNetwork", "ContactsWithCovariances", cnt_cb); + + 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); + + // Set up leader publisher + leader_pub_ = advertise("GlobalNetwork", "FormationLeader"); + + // Set up follower subscriber + auto follower_cb = [&](auto &msg) { // Nat - this may be good for locating entities nearby + auto leader_x_pos = msg->data.x_pos; + auto leader_y_pos = msg->data.y_pos; + auto leader_z_pos = msg->data.z_pos; + }; + subscribe("GlobalNetwork", "FormationLeader", follower_cb); +} + +bool Formation::step_autonomy(double t, double dt) { + + // Nat - Need a case where the entity senses a new nearby autonomy + // If it does, then need to do new leader selection + if(!leader_){ + goal_(0) = leader_x_pos; + goal_(1) = leader_y_pos; + goal_(2) = leader_z_pos; + + cout << "Follower goal positions: x_pos: " << goal_(0) + << " y_pos: " << goal_(1) + << " z_pos: " << goal_(2) + << endl; + } + + // 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; + } + } + + // If the entity is the leader, the goal is in front + Eigen::Vector3d diff = goal_ - noisy_state_.pos(); + Eigen::Vector3d v = speed_ * diff.normalized(); + + /////////////////////////////////////////////////////////////////////////// + // Convert desired velocity to desired speed, heading, and pitch controls + /////////////////////////////////////////////////////////////////////////// + // Nat - this should be if you are the leader, then do normal straight flying + if(leader_){ + // If the entity is the leader, the goal is in front + Eigen::Vector3d diff = goal_ - noisy_state_.pos(); + Eigen::Vector3d v = speed_ * diff.normalized(); + + double heading = Angles::angle_2pi(atan2(v(1), v(0))); + vars_.output(desired_alt_idx_, goal_(2)); + vars_.output(desired_speed_idx_, v.norm()); + vars_.output(desired_heading_idx_, heading); + + auto leader_msg = std::make_shared>(); + leader_msg->data.set_x_pos(state_->pos()(0)); + leader_msg->data.set_y_pos(state_->pos()(1)); + leader_msg->data.set_z_pos(state_->pos()(2)); + leader_pub_->publish(leader_msg); + + + } else { // If it is not the leader, take the leader's heading and add a displacement + + } + + // Nat - if you are not the leader, get the leader's position and do proper displacements + // for desired alt, speed, and heading + + noisy_state_set_ = false; + return true; +} +} // namespace autonomy +} // namespace scrimmage From 50ad294e2a2fba070ea15941b1188f5ac9a06118 Mon Sep 17 00:00:00 2001 From: Natalie Date: Wed, 20 Sep 2023 21:50:17 -0400 Subject: [PATCH 02/13] Build fix --- include/scrimmage/plugins/autonomy/Formation/Formation.h | 6 +++--- src/plugins/autonomy/Formation/Formation.cpp | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/include/scrimmage/plugins/autonomy/Formation/Formation.h b/include/scrimmage/plugins/autonomy/Formation/Formation.h index 02afa91e1a..1fc129afca 100644 --- a/include/scrimmage/plugins/autonomy/Formation/Formation.h +++ b/include/scrimmage/plugins/autonomy/Formation/Formation.h @@ -80,9 +80,9 @@ class Formation : public scrimmage::Autonomy{ double prev_gen_time_ = -1.0; scrimmage::PublisherPtr leader_pub_; - // float leader_x_pos; - // float leader_y_pos; - // float leader_z_pos; + float leader_x_pos; + float leader_y_pos; + float leader_z_pos; }; } // namespace autonomy } // namespace scrimmage diff --git a/src/plugins/autonomy/Formation/Formation.cpp b/src/plugins/autonomy/Formation/Formation.cpp index 810b9a84d9..73d02fc3e4 100644 --- a/src/plugins/autonomy/Formation/Formation.cpp +++ b/src/plugins/autonomy/Formation/Formation.cpp @@ -129,9 +129,9 @@ void Formation::init(std::map ¶ms) { // Set up follower subscriber auto follower_cb = [&](auto &msg) { // Nat - this may be good for locating entities nearby - auto leader_x_pos = msg->data.x_pos; - auto leader_y_pos = msg->data.y_pos; - auto leader_z_pos = msg->data.z_pos; + leader_x_pos = msg->data.x_pos(); + leader_y_pos = msg->data.y_pos(); + leader_z_pos = msg->data.z_pos(); }; subscribe("GlobalNetwork", "FormationLeader", follower_cb); } From 2e2f5f3013e677cba88a45594fa8750eac395716 Mon Sep 17 00:00:00 2001 From: Natalie Date: Thu, 21 Sep 2023 12:23:05 -0400 Subject: [PATCH 03/13] Functioning basic leader follower... slay --- .../plugins/autonomy/Formation/Formation.h | 2 + msgs/Event.proto | 1 + src/plugins/autonomy/Formation/Formation.cpp | 79 +++++++++++++++---- 3 files changed, 66 insertions(+), 16 deletions(-) diff --git a/include/scrimmage/plugins/autonomy/Formation/Formation.h b/include/scrimmage/plugins/autonomy/Formation/Formation.h index 1fc129afca..e8374bfd25 100644 --- a/include/scrimmage/plugins/autonomy/Formation/Formation.h +++ b/include/scrimmage/plugins/autonomy/Formation/Formation.h @@ -83,6 +83,8 @@ class Formation : public scrimmage::Autonomy{ float leader_x_pos; float leader_y_pos; float leader_z_pos; + + int ent_id; }; } // namespace autonomy } // namespace scrimmage diff --git a/msgs/Event.proto b/msgs/Event.proto index 3bd5bc2ac9..8cba3ffaee 100644 --- a/msgs/Event.proto +++ b/msgs/Event.proto @@ -58,4 +58,5 @@ message FormationLeader{ float x_pos = 1; float y_pos = 2; float z_pos = 3; + int32 bird_id = 4; } diff --git a/src/plugins/autonomy/Formation/Formation.cpp b/src/plugins/autonomy/Formation/Formation.cpp index 73d02fc3e4..7220f4bbb0 100644 --- a/src/plugins/autonomy/Formation/Formation.cpp +++ b/src/plugins/autonomy/Formation/Formation.cpp @@ -72,6 +72,8 @@ namespace scrimmage { namespace autonomy { void Formation::init(std::map ¶ms) { + ent_id = parent_->id().id(); + speed_ = scrimmage::get("speed", params, 0.0); leader_ = scrimmage::get("leader", params, false); @@ -129,15 +131,50 @@ void Formation::init(std::map ¶ms) { // Set up follower subscriber auto follower_cb = [&](auto &msg) { // Nat - this may be good for locating entities nearby + if(msg->data.bird_id() == ent_id){ + cout << "Skip calculation, entity ids are the same" << endl; + return; + } + leader_x_pos = msg->data.x_pos(); leader_y_pos = msg->data.y_pos(); leader_z_pos = msg->data.z_pos(); + cout << "Leader positions: x_pos: " << leader_x_pos + << " y_pos: " << leader_y_pos + << " z_pos: " << leader_z_pos + << endl; + + cout << "Current entity " << ent_id << "positions: x_pos: " << state_->pos()(0) + << " y_pos: " << state_->pos()(1) + << " z_pos: " << state_->pos()(2) + << endl; + + + // If the entity is a leader, but it is also receiver other leader messages, calculate + // the euclidean distance. If it is within a range, need to combine formations and define + // a new leader. For the time being, make id 1 the leader. + if(leader_){ + float x_pos_sq = (state_->pos()(0) - leader_x_pos) * (state_->pos()(0) - leader_x_pos); + float y_pos_sq = (state_->pos()(1) - leader_y_pos) * (state_->pos()(1) - leader_y_pos); + // float z_pos_sq = (state_->pos()(2) - leader_z_pos) * (state_->pos()(2) - leader_z_pos); + float euc_dist = std::sqrt(x_pos_sq + y_pos_sq); + + cout << "Euclidean distance is: " << euc_dist << " and ent id is: " << ent_id << endl; + + if(euc_dist < 50 && ent_id != 1){ + cout << "Entity " << ent_id << " is no longer the leader" << endl; + leader_ = false; + } + } }; subscribe("GlobalNetwork", "FormationLeader", follower_cb); } bool Formation::step_autonomy(double t, double dt) { + //cout << "Entity id: " << parent_->id().id() << endl; + + // Nat - Need a case where the entity senses a new nearby autonomy // If it does, then need to do new leader selection if(!leader_){ @@ -145,7 +182,8 @@ bool Formation::step_autonomy(double t, double dt) { goal_(1) = leader_y_pos; goal_(2) = leader_z_pos; - cout << "Follower goal positions: x_pos: " << goal_(0) + cout << "Updating follower goal positions for entity" << ent_id + << " x_pos: " << goal_(0) << " y_pos: " << goal_(1) << " z_pos: " << goal_(2) << endl; @@ -170,30 +208,39 @@ bool Formation::step_autonomy(double t, double dt) { Eigen::Vector3d diff = goal_ - noisy_state_.pos(); Eigen::Vector3d v = speed_ * diff.normalized(); - /////////////////////////////////////////////////////////////////////////// - // Convert desired velocity to desired speed, heading, and pitch controls - /////////////////////////////////////////////////////////////////////////// - // Nat - this should be if you are the leader, then do normal straight flying - if(leader_){ - // If the entity is the leader, the goal is in front - Eigen::Vector3d diff = goal_ - noisy_state_.pos(); - Eigen::Vector3d v = speed_ * diff.normalized(); - - double heading = Angles::angle_2pi(atan2(v(1), v(0))); - vars_.output(desired_alt_idx_, goal_(2)); - vars_.output(desired_speed_idx_, v.norm()); - vars_.output(desired_heading_idx_, heading); + double heading = Angles::angle_2pi(atan2(v(1), v(0))); + vars_.output(desired_alt_idx_, goal_(2)); + vars_.output(desired_speed_idx_, v.norm()); + vars_.output(desired_heading_idx_, heading); + if(leader_){ auto leader_msg = std::make_shared>(); leader_msg->data.set_x_pos(state_->pos()(0)); leader_msg->data.set_y_pos(state_->pos()(1)); leader_msg->data.set_z_pos(state_->pos()(2)); + leader_msg->data.set_bird_id(ent_id); leader_pub_->publish(leader_msg); + } + /////////////////////////////////////////////////////////////////////////// + // Convert desired velocity to desired speed, heading, and pitch controls + /////////////////////////////////////////////////////////////////////////// + // Nat - this should be if you are the leader, then do normal straight flying + // if(leader_){ - } else { // If it is not the leader, take the leader's heading and add a displacement + // double heading = Angles::angle_2pi(atan2(v(1), v(0))); + // vars_.output(desired_alt_idx_, goal_(2)); + // vars_.output(desired_speed_idx_, v.norm()); + // vars_.output(desired_heading_idx_, heading); - } + // auto leader_msg = std::make_shared>(); + // leader_msg->data.set_x_pos(state_->pos()(0)); + // leader_msg->data.set_y_pos(state_->pos()(1)); + // leader_msg->data.set_z_pos(state_->pos()(2)); + // leader_pub_->publish(leader_msg); + // } else { // If it is not the leader, take the leader's heading and add a displacement + + // } // Nat - if you are not the leader, get the leader's position and do proper displacements // for desired alt, speed, and heading From 29bdcd13c6be98f756f002782be4f57d988d0917 Mon Sep 17 00:00:00 2001 From: Natalie Date: Mon, 25 Sep 2023 13:50:24 -0400 Subject: [PATCH 04/13] Not working, adding to transfer across machines --- .../plugins/autonomy/Formation/Formation.h | 24 +-- .../plugins/autonomy/Formation/Formation.xml | 8 +- missions/leadfollow_formation.xml | 151 ++++++++++++++++++ msgs/Event.proto | 4 + src/plugins/autonomy/Formation/Formation.cpp | 91 +++++------ .../autonomy/MotorSchemas/MotorSchemas.cpp | 16 +- .../autonomy/MoveToGoalMS/MoveToGoalMS.cpp | 13 ++ .../WaypointDispatcher/WaypointDispatcher.cpp | 3 + 8 files changed, 247 insertions(+), 63 deletions(-) create mode 100644 missions/leadfollow_formation.xml diff --git a/include/scrimmage/plugins/autonomy/Formation/Formation.h b/include/scrimmage/plugins/autonomy/Formation/Formation.h index e8374bfd25..c99653a0c4 100644 --- a/include/scrimmage/plugins/autonomy/Formation/Formation.h +++ b/include/scrimmage/plugins/autonomy/Formation/Formation.h @@ -40,6 +40,7 @@ #include #include #include +#include namespace scrimmage { @@ -54,11 +55,23 @@ class Formation : public scrimmage::Autonomy{ bool step_autonomy(double t, double dt) override; protected: - double speed_; + bool leader_; + double leader_speed_; + float leader_x_pos; + float leader_y_pos; + float leader_z_pos; + + double follower_speed_; + + scrimmage::PublisherPtr leader_pub_; + scrimmage::PublisherPtr follower_track_pub_; + + int ent_id; + std::map follower_track; //ent_id, follower number + Eigen::Vector3d goal_; int frame_number_; - bool leader_; bool enable_boundary_control_ = false; std::shared_ptr boundary_; @@ -78,13 +91,6 @@ class Formation : public scrimmage::Autonomy{ double desired_z_ = 0; double prev_gen_time_ = -1.0; - - scrimmage::PublisherPtr leader_pub_; - float leader_x_pos; - float leader_y_pos; - float leader_z_pos; - - int ent_id; }; } // namespace autonomy } // namespace scrimmage diff --git a/include/scrimmage/plugins/autonomy/Formation/Formation.xml b/include/scrimmage/plugins/autonomy/Formation/Formation.xml index b37800d1c5..57c0d43dbd 100644 --- a/include/scrimmage/plugins/autonomy/Formation/Formation.xml +++ b/include/scrimmage/plugins/autonomy/Formation/Formation.xml @@ -2,7 +2,11 @@ Formation_plugin - 21 - false + + 15 false + + 30 + + false diff --git a/missions/leadfollow_formation.xml b/missions/leadfollow_formation.xml new file mode 100644 index 0000000000..80fbc42a43 --- /dev/null +++ b/missions/leadfollow_formation.xml @@ -0,0 +1,151 @@ + + + + + + + false + 50051 + localhost + + all_dead + + 10 + 1000 + + + 191 191 191 + 10 + + true + all + false + false + + SimpleCollisionMetrics + + ~/.scrimmage/logs + true + + 35.721025 + -120.767925 + 300 + true + 10 + + Boundary + SimpleCollision + GroundCollision + + false + + GlobalNetwork + LocalNetwork + + + 2147483648 + + + + + + + + leader_1 + 1 + 1 + 1 + 1 + + -200 + 0 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_2 + 2 + 1 + 1 + 1 + + 100 + 5 + 195 + 180 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + diff --git a/msgs/Event.proto b/msgs/Event.proto index 8cba3ffaee..d50ac85e37 100644 --- a/msgs/Event.proto +++ b/msgs/Event.proto @@ -60,3 +60,7 @@ message FormationLeader{ float z_pos = 3; int32 bird_id = 4; } + +message FollowerTrack{ + map follower_track = 1; +} diff --git a/src/plugins/autonomy/Formation/Formation.cpp b/src/plugins/autonomy/Formation/Formation.cpp index 7220f4bbb0..3888845bf9 100644 --- a/src/plugins/autonomy/Formation/Formation.cpp +++ b/src/plugins/autonomy/Formation/Formation.cpp @@ -74,9 +74,11 @@ namespace autonomy { void Formation::init(std::map ¶ms) { ent_id = parent_->id().id(); - speed_ = scrimmage::get("speed", params, 0.0); + leader_speed_ = scrimmage::get("leader_speed", params, 0.0); leader_ = scrimmage::get("leader", params, false); + follower_speed_ = scrimmage::get("follower_speed", params, 0.0); + // Project goal in front... Eigen::Vector3d rel_pos = Eigen::Vector3d::UnitX()*1e6; Eigen::Vector3d unit_vector = rel_pos.normalized(); @@ -84,16 +86,6 @@ void Formation::init(std::map ¶ms) { goal_ = state_->pos() + unit_vector * rel_pos.norm(); goal_(2) = state_->pos()(2); - // Nat - what if goal isn't in front? see if it turns - // Should probably have a tag that is for leader/follower with the given entity - // Instead of speed, probably want displacement compared to the leader instead. - // Might be able to stack autonomies for go to waypoint and avoid entity MS - // - // Will need logic if the entitiy is not the leader. If it is the leader, it can - // just continue to fly straight. Maybe a good way to do this if it is not the leader, - // it should publish messages from follower, saying it is in the formation. The leader - // should publish messages with its location, that way the follower can do proper displacement - // Set the desired_z to our initial position. // desired_z_ = state_->pos()(2); @@ -126,16 +118,14 @@ void Formation::init(std::map ¶ms) { desired_speed_idx_ = vars_.declare(VariableIO::Type::desired_speed, VariableIO::Direction::Out); desired_heading_idx_ = vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::Out); - // Set up leader publisher - leader_pub_ = advertise("GlobalNetwork", "FormationLeader"); - // Set up follower subscriber auto follower_cb = [&](auto &msg) { // Nat - this may be good for locating entities nearby - if(msg->data.bird_id() == ent_id){ + if(msg->data.bird_id() == ent_id || msg->data.bird_id() != 1){ // Only compute euc dist. for cur vs ent 1 cout << "Skip calculation, entity ids are the same" << endl; return; } + // If the bird is a follower, offsets could be put here depending on the follower number leader_x_pos = msg->data.x_pos(); leader_y_pos = msg->data.y_pos(); leader_z_pos = msg->data.z_pos(); @@ -156,29 +146,39 @@ void Formation::init(std::map ¶ms) { if(leader_){ float x_pos_sq = (state_->pos()(0) - leader_x_pos) * (state_->pos()(0) - leader_x_pos); float y_pos_sq = (state_->pos()(1) - leader_y_pos) * (state_->pos()(1) - leader_y_pos); - // float z_pos_sq = (state_->pos()(2) - leader_z_pos) * (state_->pos()(2) - leader_z_pos); + //float z_pos_sq = (state_->pos()(2) - leader_z_pos) * (state_->pos()(2) - leader_z_pos); // IF using z, will need a diff distance threshold float euc_dist = std::sqrt(x_pos_sq + y_pos_sq); cout << "Euclidean distance is: " << euc_dist << " and ent id is: " << ent_id << endl; if(euc_dist < 50 && ent_id != 1){ - cout << "Entity " << ent_id << " is no longer the leader" << endl; leader_ = false; + follower_track[ent_id] = follower_track.size(); + cout << "Entity " << ent_id << " is no longer the leader. It is now follower #" << follower_track[ent_id] << endl; } } }; subscribe("GlobalNetwork", "FormationLeader", follower_cb); + leader_pub_ = advertise("GlobalNetwork", "FormationLeader"); + + // Set up follower tracker subscriber + // auto tracker_cb = [&](auto &msg) { + // follower_track = msg->data.follower_track(); + // cout << "In the follower track call back. The map is currently the following size: " << follower_track.size() << endl; + // }; + // subscribe("GlobalNetwork", "FollowerTrack", tracker_cb); + // follower_track_pub_ = advertise("GlobalNetwork", "FollowerTrack"); } bool Formation::step_autonomy(double t, double dt) { - - //cout << "Entity id: " << parent_->id().id() << endl; - - // Nat - Need a case where the entity senses a new nearby autonomy - // If it does, then need to do new leader selection + // Goal displacements if a follower if(!leader_){ - goal_(0) = leader_x_pos; + // When adding this kind of offset with the way velocity is calculated, + // if the drone enters a zone that is closer than the displacement, + // it turns around. Instead, need to have it slow and if it passes into the + // displacement zone, decrease speed but never spin in a circle. + goal_(0) = leader_x_pos - 5; goal_(1) = leader_y_pos; goal_(2) = leader_z_pos; @@ -204,9 +204,23 @@ bool Formation::step_autonomy(double t, double dt) { } } - // If the entity is the leader, the goal is in front + // If the entity is the leader, the goal is in front // look at unicycle pid Eigen::Vector3d diff = goal_ - noisy_state_.pos(); - Eigen::Vector3d v = speed_ * diff.normalized(); + Eigen::Vector3d v; + if(!leader_){ + // Calculate velocity using a PID controller + v = follower_speed_ * diff.normalized(); + } else { + v = leader_speed_ * diff.normalized(); + } + + // Straight's way of calculating speed + //Eigen::Vector3d v = speed_ * diff.normalized(); + + if(ent_id != 1){ + cout << "The diff between goal and noisy state is: " << diff + << " The velocity vector is: " << v << endl; + } double heading = Angles::angle_2pi(atan2(v(1), v(0))); vars_.output(desired_alt_idx_, goal_(2)); @@ -220,31 +234,12 @@ bool Formation::step_autonomy(double t, double dt) { leader_msg->data.set_z_pos(state_->pos()(2)); leader_msg->data.set_bird_id(ent_id); leader_pub_->publish(leader_msg); - } - - /////////////////////////////////////////////////////////////////////////// - // Convert desired velocity to desired speed, heading, and pitch controls - /////////////////////////////////////////////////////////////////////////// - // Nat - this should be if you are the leader, then do normal straight flying - // if(leader_){ - - // double heading = Angles::angle_2pi(atan2(v(1), v(0))); - // vars_.output(desired_alt_idx_, goal_(2)); - // vars_.output(desired_speed_idx_, v.norm()); - // vars_.output(desired_heading_idx_, heading); - - // auto leader_msg = std::make_shared>(); - // leader_msg->data.set_x_pos(state_->pos()(0)); - // leader_msg->data.set_y_pos(state_->pos()(1)); - // leader_msg->data.set_z_pos(state_->pos()(2)); - // leader_pub_->publish(leader_msg); - // } else { // If it is not the leader, take the leader's heading and add a displacement - + } //else{ + // auto follower_track_msg = std::make_shared>(); + // follower_track_msg->data.mutable_follower_track()[ent_id] = follower_track.size(); + // follower_track_pub_->publish(follower_track_msg); // } - // Nat - if you are not the leader, get the leader's position and do proper displacements - // for desired alt, speed, and heading - noisy_state_set_ = false; return true; } diff --git a/src/plugins/autonomy/MotorSchemas/MotorSchemas.cpp b/src/plugins/autonomy/MotorSchemas/MotorSchemas.cpp index 1765c751a3..b9c1467f28 100644 --- a/src/plugins/autonomy/MotorSchemas/MotorSchemas.cpp +++ b/src/plugins/autonomy/MotorSchemas/MotorSchemas.cpp @@ -158,7 +158,8 @@ bool MotorSchemas::step_autonomy(double t, double dt) { for (motor_schemas::BehaviorBasePtr &behavior : current_behaviors_) { behavior->shapes().clear(); - // cout << "Behavior: " << behavior->name() << endl; + cout << "-------------------------------------------------------" << endl; + cout << "Behavior: " << behavior->name() << endl; // Execute callbacks for received messages before calling // step_autonomy @@ -183,9 +184,9 @@ bool MotorSchemas::step_autonomy(double t, double dt) { continue; } - // cout << "desired_vector: " << desired_vector << endl; - // cout << "gain: " << behavior->gain() << endl; - // cout << "desired_vector.norm(): " << desired_vector.norm() << endl; + cout << "desired_vector: " << desired_vector << endl; + cout << "gain: " << behavior->gain() << endl; + cout << "desired_vector.norm(): " << desired_vector.norm() << endl; // Keep a running sum of all vectors with gains vec_w_gain += desired_vector * behavior->gain(); @@ -225,11 +226,15 @@ bool MotorSchemas::step_autonomy(double t, double dt) { vars_.output(output_vel_x_idx_, vel_result(0)); vars_.output(output_vel_y_idx_, vel_result(1)); vars_.output(output_vel_z_idx_, vel_result(2)); + + cout << "Publishing vel vector" << endl; } else { double heading = sc::Angles::angle_2pi(atan2(vel_result(1), vel_result(0))); vars_.output(desired_alt_idx_, state_->pos()(2) + vel_result(2)); vars_.output(desired_speed_idx_, vel_result.norm()); vars_.output(desired_heading_idx_, heading); + + cout << "Converting vector to heading, speed, and vel command" << endl; } /////////////////////////////////////////////////////////////////////////// @@ -246,6 +251,9 @@ bool MotorSchemas::step_autonomy(double t, double dt) { sc::set(line_shape_->mutable_line()->mutable_end(), vel_result + state_->pos()); draw_shape(line_shape_); } + + cout << "-------------------------------------------------------" << endl; + return true; } } // namespace autonomy diff --git a/src/plugins/autonomy/MoveToGoalMS/MoveToGoalMS.cpp b/src/plugins/autonomy/MoveToGoalMS/MoveToGoalMS.cpp index aea1517671..e052850d4d 100644 --- a/src/plugins/autonomy/MoveToGoalMS/MoveToGoalMS.cpp +++ b/src/plugins/autonomy/MoveToGoalMS/MoveToGoalMS.cpp @@ -97,20 +97,33 @@ void MoveToGoalMS::init(std::map ¶ms) { wp_.set_position_tolerance(1); wp_.set_quat_tolerance(1); + cout << "%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%" << endl; + cout << "Way point data: 1. " << wp_local_(0) << " 2. " << wp_local_(1) << " 3. " << wp_local_(2) + << " 4. " << lat << " 5. " << lon << " 6. " << alt << endl; + cout << "%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%" << endl; + auto wp_cb = [&] (scrimmage::MessagePtr msg) { wp_ = msg->data; parent_->projection()->Forward(wp_.latitude(), wp_.longitude(), wp_.altitude(), wp_local_(0), wp_local_(1), wp_local_(2)); + cout << "%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%" << endl; + cout << "LAMBDA FUNCTION Way point data: 1. " << wp_local_(0) << " 2. " << wp_local_(1) << " 3. " << wp_local_(2) + << " 4. " << wp_.latitude() << " 5. " << wp_.longitude() << " 6. " << wp_.altitude() << endl; + cout << "%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%" << endl; }; subscribe("LocalNetwork", "Waypoint", wp_cb); } bool MoveToGoalMS::step_autonomy(double t, double dt) { + cout << "++++++++++++++++++++++++++++++++++++++" << endl; + cout << "Stepping MOVE TO GOAL autonomy" << endl; + cout << "Waypoint value: " << wp_local_ << endl; double measurement = -(wp_local_ - state_->pos()).norm(); double speed_factor = speed_pid_.step(dt, measurement); desired_vector_ = (wp_local_ - state_->pos()).normalized() * speed_factor; + cout << "++++++++++++++++++++++++++++++++++++++" << endl; return true; } } // namespace motor_schemas diff --git a/src/plugins/autonomy/WaypointDispatcher/WaypointDispatcher.cpp b/src/plugins/autonomy/WaypointDispatcher/WaypointDispatcher.cpp index fe29a5c4ff..fd8454e174 100644 --- a/src/plugins/autonomy/WaypointDispatcher/WaypointDispatcher.cpp +++ b/src/plugins/autonomy/WaypointDispatcher/WaypointDispatcher.cpp @@ -143,6 +143,7 @@ bool WaypointDispatcher::step_autonomy(double t, double dt) { sphere->set_opacity(1.0); sphere->mutable_sphere()->set_radius(30); sc::set(sphere->mutable_sphere()->mutable_center(), pt); + cout << "Point is: " << pt << endl; this->draw_shape(sphere); }; @@ -150,6 +151,8 @@ bool WaypointDispatcher::step_autonomy(double t, double dt) { draw_sphere(track_sphere_shape_, track_point, 0, 0, 0); draw_sphere(curr_wp_sphere_shape_, wp, 255, 0, 0); draw_sphere(prev_wp_sphere_shape_, prev_wp, 0, 0, 255); + + cout << "Track point: " << track_point << " WP: " << wp << " Prev WP: " << prev_wp << endl; } // convert track_point to lat/lon and Publish track_point! From 44350168780fccead3c1d43401c2fc477d905994 Mon Sep 17 00:00:00 2001 From: Natalie Date: Wed, 27 Sep 2023 17:04:52 -0400 Subject: [PATCH 05/13] Spheres to dictate leader/follower status. PID. Issue with turning that didn't use to be there.... --- .../plugins/autonomy/Formation/Formation.h | 8 ++ .../plugins/autonomy/Formation/Formation.xml | 8 ++ .../SimpleAircraftControllerPID.h | 4 + missions/leadfollow_formation.xml | 9 +- src/plugins/autonomy/Formation/Formation.cpp | 99 ++++++++++++++----- .../SimpleAircraftControllerPID.cpp | 17 ++++ 6 files changed, 119 insertions(+), 26 deletions(-) diff --git a/include/scrimmage/plugins/autonomy/Formation/Formation.h b/include/scrimmage/plugins/autonomy/Formation/Formation.h index c99653a0c4..23bcf4961e 100644 --- a/include/scrimmage/plugins/autonomy/Formation/Formation.h +++ b/include/scrimmage/plugins/autonomy/Formation/Formation.h @@ -60,8 +60,16 @@ class Formation : public scrimmage::Autonomy{ float leader_x_pos; float leader_y_pos; float leader_z_pos; + float safety_dist_; double follower_speed_; + double follow_v_k_; + double x_disp_; + double y_disp_; + double z_disp_; + + bool show_shapes_; + scrimmage_proto::ShapePtr circle_shape_; scrimmage::PublisherPtr leader_pub_; scrimmage::PublisherPtr follower_track_pub_; diff --git a/include/scrimmage/plugins/autonomy/Formation/Formation.xml b/include/scrimmage/plugins/autonomy/Formation/Formation.xml index 57c0d43dbd..13b7e7797f 100644 --- a/include/scrimmage/plugins/autonomy/Formation/Formation.xml +++ b/include/scrimmage/plugins/autonomy/Formation/Formation.xml @@ -5,8 +5,16 @@ 15 false + + 10 30 + 0.55 + 0 + 0 + 0 + + false false diff --git a/include/scrimmage/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.h b/include/scrimmage/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.h index df928bca63..3cbeade379 100644 --- a/include/scrimmage/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.h +++ b/include/scrimmage/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.h @@ -70,6 +70,10 @@ class SimpleAircraftControllerPID : public Controller { uint8_t output_throttle_idx_ = 0; uint8_t output_roll_rate_idx_ = 0; uint8_t output_pitch_rate_idx_ = 0; + + uint8_t desired_pitch_idx_ = 0; + uint8_t desired_roll_idx_ = 0; + uint8_t desired_speed_idx_ = 0; }; } // namespace controller } // namespace scrimmage diff --git a/missions/leadfollow_formation.xml b/missions/leadfollow_formation.xml index 80fbc42a43..80ce2eae41 100644 --- a/missions/leadfollow_formation.xml +++ b/missions/leadfollow_formation.xml @@ -80,7 +80,8 @@ zephyr-blue Formation + enable_boundary_control="true" + show_shapes="true">Formation 35.721112 @@ -109,7 +110,11 @@ zephyr-blue Formation + enable_boundary_control="true" + x_disp = "10" + y_disp = "0" + z_disp = "0" + show_shapes="true" >Formation 35.721112 diff --git a/src/plugins/autonomy/Formation/Formation.cpp b/src/plugins/autonomy/Formation/Formation.cpp index 3888845bf9..5182994443 100644 --- a/src/plugins/autonomy/Formation/Formation.cpp +++ b/src/plugins/autonomy/Formation/Formation.cpp @@ -48,6 +48,9 @@ #include #include +#include +#include + #include namespace sc = scrimmage; @@ -76,8 +79,15 @@ void Formation::init(std::map ¶ms) { leader_speed_ = scrimmage::get("leader_speed", params, 0.0); leader_ = scrimmage::get("leader", params, false); + safety_dist_ = scrimmage::get("safety_dist", params, 0.0); follower_speed_ = scrimmage::get("follower_speed", params, 0.0); + follow_v_k_ = scrimmage::get("follow_v_k", params, 0.0); + x_disp_ = scrimmage::get("x_disp", params, 0.0); + y_disp_ = scrimmage::get("y_disp", params, 0.0); + z_disp_ = scrimmage::get("z_disp", params, 0.0); + + show_shapes_ = scrimmage::get("show_shapes", params, false); // Project goal in front... Eigen::Vector3d rel_pos = Eigen::Vector3d::UnitX()*1e6; @@ -121,7 +131,7 @@ void Formation::init(std::map ¶ms) { // Set up follower subscriber auto follower_cb = [&](auto &msg) { // Nat - this may be good for locating entities nearby if(msg->data.bird_id() == ent_id || msg->data.bird_id() != 1){ // Only compute euc dist. for cur vs ent 1 - cout << "Skip calculation, entity ids are the same" << endl; + // cout << "Skip calculation, entity ids are the same" << endl; return; } @@ -129,15 +139,15 @@ void Formation::init(std::map ¶ms) { leader_x_pos = msg->data.x_pos(); leader_y_pos = msg->data.y_pos(); leader_z_pos = msg->data.z_pos(); - cout << "Leader positions: x_pos: " << leader_x_pos - << " y_pos: " << leader_y_pos - << " z_pos: " << leader_z_pos - << endl; + // cout << "Leader positions: x_pos: " << leader_x_pos + // << " y_pos: " << leader_y_pos + // << " z_pos: " << leader_z_pos + // << endl; - cout << "Current entity " << ent_id << "positions: x_pos: " << state_->pos()(0) - << " y_pos: " << state_->pos()(1) - << " z_pos: " << state_->pos()(2) - << endl; + // cout << "Current entity " << ent_id << "positions: x_pos: " << state_->pos()(0) + // << " y_pos: " << state_->pos()(1) + // << " z_pos: " << state_->pos()(2) + // << endl; // If the entity is a leader, but it is also receiver other leader messages, calculate @@ -177,16 +187,17 @@ bool Formation::step_autonomy(double t, double dt) { // When adding this kind of offset with the way velocity is calculated, // if the drone enters a zone that is closer than the displacement, // it turns around. Instead, need to have it slow and if it passes into the - // displacement zone, decrease speed but never spin in a circle. - goal_(0) = leader_x_pos - 5; - goal_(1) = leader_y_pos; - goal_(2) = leader_z_pos; - - cout << "Updating follower goal positions for entity" << ent_id - << " x_pos: " << goal_(0) - << " y_pos: " << goal_(1) - << " z_pos: " << goal_(2) - << endl; + // displacement zone, decrease speed but never spin in a circle. Will need hard + // hard set distance where it will turn around if it goes within the area + goal_(0) = leader_x_pos - x_disp_; + goal_(1) = leader_y_pos - y_disp_; + goal_(2) = leader_z_pos - z_disp_; + + // cout << "Updating follower goal positions for entity" << ent_id + // << " x_pos: " << goal_(0) + // << " y_pos: " << goal_(1) + // << " z_pos: " << goal_(2) + // << endl; } // Read data from sensors... @@ -204,12 +215,36 @@ bool Formation::step_autonomy(double t, double dt) { } } + cout << "------------------------Entity " << ent_id << "------------------------" << endl; // If the entity is the leader, the goal is in front // look at unicycle pid Eigen::Vector3d diff = goal_ - noisy_state_.pos(); Eigen::Vector3d v; + + cout << "Pre safety check diff: " << diff(0) << endl; + if(!leader_){ // Calculate velocity using a PID controller - v = follower_speed_ * diff.normalized(); + // For now, just handle the vel and distance here to get it working, then can add + // it to the pid once ready. Would be good to do this and then handle the follower messaging that + // couldn't get working last week + + // If the diff is less than the safety distance, then need to circle back. If it is less than the displacement + // distance, then slow down and match the sign of the velocities of the leader + // if (goal_(0) > 0 && safety_dist_ > diff(0)){ // Need to check more than just the x value, having issues with edge cases + // cout << "Safety #1 dist check" << endl; + // diff(0) = safety_dist_; + // } //else if(goal_(0) < 0 && -safety_dist_ < diff(0) && diff(0) < 0.0){ + // cout << "Safety #2 dist check" << endl; + // diff(0) = -safety_dist_; + // } + + v = follow_v_k_ * diff; + if(v(0)>follower_speed_){ + v(0) = follower_speed_; + } else if(v(0)<-follower_speed_){ + v(0) = -follower_speed_; + } + } else { v = leader_speed_ * diff.normalized(); } @@ -217,11 +252,14 @@ bool Formation::step_autonomy(double t, double dt) { // Straight's way of calculating speed //Eigen::Vector3d v = speed_ * diff.normalized(); - if(ent_id != 1){ - cout << "The diff between goal and noisy state is: " << diff + // if(ent_id != 1){ + // cout << "The diff between goal and noisy state is: " << diff + // << " The velocity vector is: " << v << endl; + // } + cout << "The diff between goal and noisy state is: " << diff << " The velocity vector is: " << v << endl; - } - + cout << "---------------------------------------------------------" << endl; + double heading = Angles::angle_2pi(atan2(v(1), v(0))); vars_.output(desired_alt_idx_, goal_(2)); vars_.output(desired_speed_idx_, v.norm()); @@ -240,6 +278,19 @@ bool Formation::step_autonomy(double t, double dt) { // follower_track_pub_->publish(follower_track_msg); // } + if (show_shapes_) { + // Draw the sphere of influence + if (circle_shape_ == nullptr) { + circle_shape_ = sc::shape::make_sphere( + state_->pos(), 2, + Eigen::Vector3d(255, 0, 0), 0.2); + } if(!leader_) { // When followers are known by the leader, can change the leader color to represent a new formation + sc::set(circle_shape_->mutable_color(), Eigen::Vector3d(0, 255, 0)); + } + sc::set(circle_shape_->mutable_sphere()->mutable_center(), state_->pos()); + draw_shape(circle_shape_); + } + noisy_state_set_ = false; return true; } diff --git a/src/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.cpp b/src/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.cpp index ef20409d98..9bdec70011 100644 --- a/src/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.cpp +++ b/src/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.cpp @@ -39,6 +39,11 @@ #include +#include + +using std::cout; +using std::endl; + REGISTER_PLUGIN(scrimmage::Controller, scrimmage::controller::SimpleAircraftControllerPID, SimpleAircraftControllerPID_plugin) namespace scrimmage { @@ -75,6 +80,10 @@ void SimpleAircraftControllerPID::init(std::map ¶m output_throttle_idx_ = vars_.declare(VariableIO::Type::throttle, VariableIO::Direction::Out); output_roll_rate_idx_ = vars_.declare(VariableIO::Type::roll_rate, VariableIO::Direction::Out); output_pitch_rate_idx_ = vars_.declare(VariableIO::Type::pitch_rate, VariableIO::Direction::Out); + + desired_pitch_idx_ = vars_.declare("desired_pitch", scrimmage::VariableIO::Direction::Out); + desired_roll_idx_ = vars_.declare("desired_roll", scrimmage::VariableIO::Direction::Out); + desired_speed_idx_ = vars_.declare("desired_speed", scrimmage::VariableIO::Direction::Out); } bool SimpleAircraftControllerPID::step(double t, double dt) { @@ -91,9 +100,17 @@ bool SimpleAircraftControllerPID::step(double t, double dt) { vel_pid_.set_setpoint(vars_.input(input_velocity_idx_)); double u_throttle = vel_pid_.step(dt, state_->vel().norm()); + // cout << "Throttle is: " << u_throttle << " State vel is: " << state_->vel() << " Norm of vel is: " << state_->vel().norm() << endl; + cout << "The speed idx is: " << output_throttle_idx_ << endl; + vars_.output(output_roll_rate_idx_, u_roll_rate); vars_.output(output_pitch_rate_idx_, u_pitch_rate); vars_.output(output_throttle_idx_, u_throttle); + + vars_.output(desired_pitch_idx_, u_pitch_rate); + vars_.output(desired_roll_idx_, u_roll_rate); + vars_.output(desired_speed_idx_, u_throttle); + return true; } } // namespace controller From 1a82bc93fac1e343afde45d204697dc2f7a61890 Mon Sep 17 00:00:00 2001 From: Natalie Date: Wed, 27 Sep 2023 23:56:51 -0400 Subject: [PATCH 06/13] Unsure if this builds, my parallels machine is timing out. Will test on work VM --- .../plugins/autonomy/Formation/Formation.h | 12 ++- missions/leadfollow_formation.xml | 4 +- msgs/Event.proto | 5 +- src/plugins/autonomy/Formation/Formation.cpp | 94 +++++++++---------- 4 files changed, 64 insertions(+), 51 deletions(-) diff --git a/include/scrimmage/plugins/autonomy/Formation/Formation.h b/include/scrimmage/plugins/autonomy/Formation/Formation.h index 23bcf4961e..9090c8a756 100644 --- a/include/scrimmage/plugins/autonomy/Formation/Formation.h +++ b/include/scrimmage/plugins/autonomy/Formation/Formation.h @@ -57,6 +57,7 @@ class Formation : public scrimmage::Autonomy{ protected: bool leader_; double leader_speed_; + int leader_id; float leader_x_pos; float leader_y_pos; float leader_z_pos; @@ -75,7 +76,16 @@ class Formation : public scrimmage::Autonomy{ scrimmage::PublisherPtr follower_track_pub_; int ent_id; - std::map follower_track; //ent_id, follower number + //std::map follower_track; //ent_id, follower number + + struct FollowerData { + double x_pos; + double y_pos; + double z_pos; + int ent_id; + }; + + std::map follower_map; Eigen::Vector3d goal_; diff --git a/missions/leadfollow_formation.xml b/missions/leadfollow_formation.xml index 80ce2eae41..c184cb94a3 100644 --- a/missions/leadfollow_formation.xml +++ b/missions/leadfollow_formation.xml @@ -80,7 +80,7 @@ zephyr-blue Formation @@ -110,7 +110,7 @@ zephyr-blue follower_track = 1; + float x_pos = 1; + float y_pos = 2; + float z_pos = 3; + int32 bird_id = 4; } diff --git a/src/plugins/autonomy/Formation/Formation.cpp b/src/plugins/autonomy/Formation/Formation.cpp index 5182994443..32832a1794 100644 --- a/src/plugins/autonomy/Formation/Formation.cpp +++ b/src/plugins/autonomy/Formation/Formation.cpp @@ -128,10 +128,21 @@ void Formation::init(std::map ¶ms) { desired_speed_idx_ = vars_.declare(VariableIO::Type::desired_speed, VariableIO::Direction::Out); desired_heading_idx_ = vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::Out); + // If initialized as a follower, update track + if(!leader_){ + //follower_track[ent_id] = follower_track.size(); + FollowerData follower; + follower.x_pos = state_->pos()(0); + follower.y_pos = state_->pos()(1); + follower.z_pos = state_->pos()(2); + follower.ent_id = ent_id; + + follower_map[ent_id] = follower; + } + // Set up follower subscriber - auto follower_cb = [&](auto &msg) { // Nat - this may be good for locating entities nearby - if(msg->data.bird_id() == ent_id || msg->data.bird_id() != 1){ // Only compute euc dist. for cur vs ent 1 - // cout << "Skip calculation, entity ids are the same" << endl; + auto follower_cb = [&](auto &msg) { + if(msg->data.bird_id() == ent_id || msg->data.bird_id() != 1){ // Only compute euclidean distance for cur vs ent 1 return; } @@ -163,7 +174,7 @@ void Formation::init(std::map ¶ms) { if(euc_dist < 50 && ent_id != 1){ leader_ = false; - follower_track[ent_id] = follower_track.size(); + //follower_track[ent_id] = follower_track.size(); cout << "Entity " << ent_id << " is no longer the leader. It is now follower #" << follower_track[ent_id] << endl; } } @@ -172,12 +183,21 @@ void Formation::init(std::map ¶ms) { leader_pub_ = advertise("GlobalNetwork", "FormationLeader"); // Set up follower tracker subscriber - // auto tracker_cb = [&](auto &msg) { - // follower_track = msg->data.follower_track(); - // cout << "In the follower track call back. The map is currently the following size: " << follower_track.size() << endl; - // }; - // subscribe("GlobalNetwork", "FollowerTrack", tracker_cb); - // follower_track_pub_ = advertise("GlobalNetwork", "FollowerTrack"); + auto tracker_cb = [&](auto &msg) { + if(follower_map.count(msg->data.bird_id()) == 0){ + //follower_track[ent_id] = follower_track.size(); + cout << "Adding new follower id: " << msg->data.bird_id() << endl; + FollowerData follower; + follower.x_pos = msg->data.x_pos(); + follower.y_pos = msg->data.y_pos(); + follower.z_pos = msg->data.z_pos(); + follower.ent_id = msg->data.bird_id(); + + follower_map[msg->data.bird_id()] = follower; + } + }; + subscribe("GlobalNetwork", "FollowerTrack", tracker_cb); + follower_track_pub_ = advertise("GlobalNetwork", "FollowerTrack"); } bool Formation::step_autonomy(double t, double dt) { @@ -189,15 +209,18 @@ bool Formation::step_autonomy(double t, double dt) { // it turns around. Instead, need to have it slow and if it passes into the // displacement zone, decrease speed but never spin in a circle. Will need hard // hard set distance where it will turn around if it goes within the area + + cout << "Leader pos is: " << leader_x_pos << ", " << leader_y_pos << ", leader_z_pos" << endl; + goal_(0) = leader_x_pos - x_disp_; goal_(1) = leader_y_pos - y_disp_; goal_(2) = leader_z_pos - z_disp_; - // cout << "Updating follower goal positions for entity" << ent_id - // << " x_pos: " << goal_(0) - // << " y_pos: " << goal_(1) - // << " z_pos: " << goal_(2) - // << endl; + cout << "Updating follower goal positions: " + << " x_pos: " << goal_(0) + << " y_pos: " << goal_(1) + << " z_pos: " << goal_(2) + << endl; } // Read data from sensors... @@ -215,29 +238,14 @@ bool Formation::step_autonomy(double t, double dt) { } } - cout << "------------------------Entity " << ent_id << "------------------------" << endl; - // If the entity is the leader, the goal is in front // look at unicycle pid Eigen::Vector3d diff = goal_ - noisy_state_.pos(); Eigen::Vector3d v; - - cout << "Pre safety check diff: " << diff(0) << endl; if(!leader_){ // Calculate velocity using a PID controller // For now, just handle the vel and distance here to get it working, then can add // it to the pid once ready. Would be good to do this and then handle the follower messaging that // couldn't get working last week - - // If the diff is less than the safety distance, then need to circle back. If it is less than the displacement - // distance, then slow down and match the sign of the velocities of the leader - // if (goal_(0) > 0 && safety_dist_ > diff(0)){ // Need to check more than just the x value, having issues with edge cases - // cout << "Safety #1 dist check" << endl; - // diff(0) = safety_dist_; - // } //else if(goal_(0) < 0 && -safety_dist_ < diff(0) && diff(0) < 0.0){ - // cout << "Safety #2 dist check" << endl; - // diff(0) = -safety_dist_; - // } - v = follow_v_k_ * diff; if(v(0)>follower_speed_){ v(0) = follower_speed_; @@ -248,17 +256,6 @@ bool Formation::step_autonomy(double t, double dt) { } else { v = leader_speed_ * diff.normalized(); } - - // Straight's way of calculating speed - //Eigen::Vector3d v = speed_ * diff.normalized(); - - // if(ent_id != 1){ - // cout << "The diff between goal and noisy state is: " << diff - // << " The velocity vector is: " << v << endl; - // } - cout << "The diff between goal and noisy state is: " << diff - << " The velocity vector is: " << v << endl; - cout << "---------------------------------------------------------" << endl; double heading = Angles::angle_2pi(atan2(v(1), v(0))); vars_.output(desired_alt_idx_, goal_(2)); @@ -272,11 +269,14 @@ bool Formation::step_autonomy(double t, double dt) { leader_msg->data.set_z_pos(state_->pos()(2)); leader_msg->data.set_bird_id(ent_id); leader_pub_->publish(leader_msg); - } //else{ - // auto follower_track_msg = std::make_shared>(); - // follower_track_msg->data.mutable_follower_track()[ent_id] = follower_track.size(); - // follower_track_pub_->publish(follower_track_msg); - // } + } else{ + auto follower_track_msg = std::make_shared>(); + follower_track_msg->data.set_x_pos(state_->pos()(0)); + follower_track_msg->data.set_y_pos(state_->pos()(1)); + follower_track_msg->data.set_z_pos(state_->pos()(2)); + follower_track_msg->data.set_bird_id(ent_id); + follower_track_pub_->publish(follower_track_msg); + } if (show_shapes_) { // Draw the sphere of influence @@ -284,7 +284,7 @@ bool Formation::step_autonomy(double t, double dt) { circle_shape_ = sc::shape::make_sphere( state_->pos(), 2, Eigen::Vector3d(255, 0, 0), 0.2); - } if(!leader_) { // When followers are known by the leader, can change the leader color to represent a new formation + } if(!follower_map.empty()){ sc::set(circle_shape_->mutable_color(), Eigen::Vector3d(0, 255, 0)); } sc::set(circle_shape_->mutable_sphere()->mutable_center(), state_->pos()); From eb8cdf0d8cd83e1b644772192d0f42212912f2a7 Mon Sep 17 00:00:00 2001 From: laserjetprinter Date: Thu, 28 Sep 2023 11:46:27 -0400 Subject: [PATCH 07/13] Build works - can track all followers in a formation --- missions/leadfollow_formation.xml | 18 +++++++++++------- src/plugins/autonomy/Formation/Formation.cpp | 2 +- .../SimpleAircraftControllerPID.cpp | 1 - 3 files changed, 12 insertions(+), 9 deletions(-) diff --git a/missions/leadfollow_formation.xml b/missions/leadfollow_formation.xml index c184cb94a3..6aff933520 100644 --- a/missions/leadfollow_formation.xml +++ b/missions/leadfollow_formation.xml @@ -69,7 +69,7 @@ 1 1 - -200 + -100 0 195 0 @@ -124,7 +124,7 @@ - + diff --git a/src/plugins/autonomy/Formation/Formation.cpp b/src/plugins/autonomy/Formation/Formation.cpp index 32832a1794..4dce794be4 100644 --- a/src/plugins/autonomy/Formation/Formation.cpp +++ b/src/plugins/autonomy/Formation/Formation.cpp @@ -175,7 +175,7 @@ void Formation::init(std::map ¶ms) { if(euc_dist < 50 && ent_id != 1){ leader_ = false; //follower_track[ent_id] = follower_track.size(); - cout << "Entity " << ent_id << " is no longer the leader. It is now follower #" << follower_track[ent_id] << endl; + //cout << "Entity " << ent_id << " is no longer the leader. It is now follower #" << follower_track[ent_id] << endl; } } }; diff --git a/src/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.cpp b/src/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.cpp index 9bdec70011..f28c1725f9 100644 --- a/src/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.cpp +++ b/src/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.cpp @@ -101,7 +101,6 @@ bool SimpleAircraftControllerPID::step(double t, double dt) { double u_throttle = vel_pid_.step(dt, state_->vel().norm()); // cout << "Throttle is: " << u_throttle << " State vel is: " << state_->vel() << " Norm of vel is: " << state_->vel().norm() << endl; - cout << "The speed idx is: " << output_throttle_idx_ << endl; vars_.output(output_roll_rate_idx_, u_roll_rate); vars_.output(output_pitch_rate_idx_, u_pitch_rate); From 43e59dae68376805e0a3804e6a025d284e72f21a Mon Sep 17 00:00:00 2001 From: Natalie Date: Wed, 18 Oct 2023 11:01:21 -0400 Subject: [PATCH 08/13] Added entity avoidance, additional formation scenario mission files --- .../plugins/autonomy/Formation/Formation.h | 9 + missions/leadfollow_formation.xml | 46 ++- missions/leadfollow_headon.xml | 132 ++++++ missions/leadfollow_line_formation.xml | 227 +++++++++++ missions/leadfollow_v_formation.xml | 237 +++++++++++ missions/leadfollow_x_formation.xml | 377 ++++++++++++++++++ .../autonomy/AvoidEntityMS/AvoidEntityMS.cpp | 2 +- src/plugins/autonomy/Formation/Formation.cpp | 140 +++++-- 8 files changed, 1139 insertions(+), 31 deletions(-) create mode 100644 missions/leadfollow_headon.xml create mode 100644 missions/leadfollow_line_formation.xml create mode 100644 missions/leadfollow_v_formation.xml create mode 100644 missions/leadfollow_x_formation.xml diff --git a/include/scrimmage/plugins/autonomy/Formation/Formation.h b/include/scrimmage/plugins/autonomy/Formation/Formation.h index 9090c8a756..1b01f72447 100644 --- a/include/scrimmage/plugins/autonomy/Formation/Formation.h +++ b/include/scrimmage/plugins/autonomy/Formation/Formation.h @@ -69,6 +69,12 @@ class Formation : public scrimmage::Autonomy{ double y_disp_; double z_disp_; + // Entity avoidance + bool avoid_non_team_; + double sphere_of_influence_; + double minimum_range_; + Eigen::Vector3d desired_vector_; + bool show_shapes_; scrimmage_proto::ShapePtr circle_shape_; @@ -109,6 +115,9 @@ class Formation : public scrimmage::Autonomy{ double desired_z_ = 0; double prev_gen_time_ = -1.0; + + void avoidance_vectors(ContactMap &contacts, + std::vector &O_vecs); }; } // namespace autonomy } // namespace scrimmage diff --git a/missions/leadfollow_formation.xml b/missions/leadfollow_formation.xml index 6aff933520..e2423b1af2 100644 --- a/missions/leadfollow_formation.xml +++ b/missions/leadfollow_formation.xml @@ -79,9 +79,12 @@ zephyr-blue + NoisyContacts + Formation + show_shapes="true" + leader_speed="8">Formation 35.721112 @@ -109,6 +112,8 @@ zephyr-blue + NoisyContacts + - ========================== Entity 3 ========================= + leader_3 3 @@ -142,6 +147,8 @@ zephyr-blue + NoisyContacts + + + + leader_4 + 4 + 1 + 1 + 1 + + -5 + 100 + 195 + 270 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + diff --git a/missions/leadfollow_headon.xml b/missions/leadfollow_headon.xml new file mode 100644 index 0000000000..a8ee1e6af5 --- /dev/null +++ b/missions/leadfollow_headon.xml @@ -0,0 +1,132 @@ + + + + + + + false + 50051 + localhost + + all_dead + + 10 + 1000 + + + 191 191 191 + 10 + + true + all + false + false + + SimpleCollisionMetrics + + ~/.scrimmage/logs + true + + 35.721025 + -120.767925 + 300 + true + 10 + + Boundary + SimpleCollision + GroundCollision + + false + + GlobalNetwork + LocalNetwork + + + 2147483648 + + + + + + + + leader_1 + 1 + 1 + 1 + 1 + + -100 + -2 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_2 + 2 + 1 + 1 + 1 + + 100 + 2 + 195 + 180 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + diff --git a/missions/leadfollow_line_formation.xml b/missions/leadfollow_line_formation.xml new file mode 100644 index 0000000000..7d593cc62a --- /dev/null +++ b/missions/leadfollow_line_formation.xml @@ -0,0 +1,227 @@ + + + + + + + false + 50051 + localhost + + all_dead + + 10 + 1000 + + + 191 191 191 + 10 + + true + all + false + false + + SimpleCollisionMetrics + + ~/.scrimmage/logs + true + + 35.721025 + -120.767925 + 300 + true + 10 + + Boundary + SimpleCollision + GroundCollision + + false + + GlobalNetwork + LocalNetwork + + + 2147483648 + + + + + + + + leader_1 + 1 + 1 + 1 + 1 + + -900 + 0 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_2 + 2 + 1 + 1 + 1 + + -905 + 0 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_3 + 3 + 1 + 1 + 1 + + -910 + 0 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_4 + 4 + 1 + 1 + 1 + + -915 + 0 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_5 + 5 + 1 + 1 + 1 + + -920 + 0 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + diff --git a/missions/leadfollow_v_formation.xml b/missions/leadfollow_v_formation.xml new file mode 100644 index 0000000000..02437f74fd --- /dev/null +++ b/missions/leadfollow_v_formation.xml @@ -0,0 +1,237 @@ + + + + + + + false + 50051 + localhost + + all_dead + + 10 + 1000 + + + 191 191 191 + 10 + + true + all + false + false + + SimpleCollisionMetrics + + ~/.scrimmage/logs + true + + 35.721025 + -120.767925 + 300 + true + 10 + + Boundary + SimpleCollision + GroundCollision + + false + + GlobalNetwork + LocalNetwork + + + 2147483648 + + + + + + + + leader_1 + 1 + 1 + 1 + 1 + + -900 + 0 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_2 + 2 + 1 + 1 + 1 + + -903 + 3 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_3 + 3 + 1 + 1 + 1 + + -903 + -3 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_4 + 4 + 1 + 1 + 1 + + -906 + 6 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_5 + 5 + 1 + 1 + 1 + + -906 + -6 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + diff --git a/missions/leadfollow_x_formation.xml b/missions/leadfollow_x_formation.xml new file mode 100644 index 0000000000..ec045814f1 --- /dev/null +++ b/missions/leadfollow_x_formation.xml @@ -0,0 +1,377 @@ + + + + + + + false + 50051 + localhost + + all_dead + + 10 + 1000 + + + 191 191 191 + 10 + + true + all + false + false + + SimpleCollisionMetrics + + ~/.scrimmage/logs + true + + 35.721025 + -120.767925 + 300 + true + 10 + + Boundary + SimpleCollision + GroundCollision + + false + + GlobalNetwork + LocalNetwork + + + 2147483648 + + + + + + + + leader_1 + 1 + 1 + 1 + 1 + + -900 + 0 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_2 + 2 + 1 + 1 + 1 + + -910 + 10 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_3 + 3 + 1 + 1 + 1 + + -910 + -10 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_4 + 4 + 1 + 1 + 1 + + -890 + -10 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_5 + 5 + 1 + 1 + 1 + + -890 + 10 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_6 + 6 + 1 + 1 + 1 + + -920 + 20 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_7 + 7 + 1 + 1 + 1 + + -920 + -20 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_8 + 8 + 1 + 1 + 1 + + -880 + -20 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_9 + 9 + 1 + 1 + 1 + + -880 + 20 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + diff --git a/src/plugins/autonomy/AvoidEntityMS/AvoidEntityMS.cpp b/src/plugins/autonomy/AvoidEntityMS/AvoidEntityMS.cpp index 909e9563e0..a0ec26c6fa 100644 --- a/src/plugins/autonomy/AvoidEntityMS/AvoidEntityMS.cpp +++ b/src/plugins/autonomy/AvoidEntityMS/AvoidEntityMS.cpp @@ -122,7 +122,7 @@ bool AvoidEntityMS::step_autonomy(double t, double dt) { desired_vector_ += *it; } - desired_vector_ *= max_vector_length_; + desired_vector_ *= max_vector_length_; // Nat - max vec length is just max speed if (show_shapes_) { // Draw the sphere of influence diff --git a/src/plugins/autonomy/Formation/Formation.cpp b/src/plugins/autonomy/Formation/Formation.cpp index 4dce794be4..7253dc6725 100644 --- a/src/plugins/autonomy/Formation/Formation.cpp +++ b/src/plugins/autonomy/Formation/Formation.cpp @@ -53,6 +53,11 @@ #include +#include + +#include +#include + namespace sc = scrimmage; namespace sp = scrimmage_proto; namespace sci = scrimmage::interaction; @@ -60,8 +65,6 @@ namespace sci = scrimmage::interaction; using std::cout; using std::endl; -#include - #define BOOST_NO_CXX11_SCOPED_ENUMS #include #undef BOOST_NO_CXX11_SCOPED_ENUMS @@ -89,6 +92,13 @@ void Formation::init(std::map ¶ms) { show_shapes_ = scrimmage::get("show_shapes", params, false); + // Entity avoidance + avoid_non_team_ = sc::get("avoid_non_team", params, true); + sphere_of_influence_ = sc::get("sphere_of_influence", params, 30); + minimum_range_ = sc::get("minimum_range", params, 5); + + cout << "Sphere: " << sphere_of_influence_ << " Min range: " << minimum_range_ << endl; + // Project goal in front... Eigen::Vector3d rel_pos = Eigen::Vector3d::UnitX()*1e6; Eigen::Vector3d unit_vector = rel_pos.normalized(); @@ -146,20 +156,9 @@ void Formation::init(std::map ¶ms) { return; } - // If the bird is a follower, offsets could be put here depending on the follower number leader_x_pos = msg->data.x_pos(); leader_y_pos = msg->data.y_pos(); leader_z_pos = msg->data.z_pos(); - // cout << "Leader positions: x_pos: " << leader_x_pos - // << " y_pos: " << leader_y_pos - // << " z_pos: " << leader_z_pos - // << endl; - - // cout << "Current entity " << ent_id << "positions: x_pos: " << state_->pos()(0) - // << " y_pos: " << state_->pos()(1) - // << " z_pos: " << state_->pos()(2) - // << endl; - // If the entity is a leader, but it is also receiver other leader messages, calculate // the euclidean distance. If it is within a range, need to combine formations and define @@ -170,12 +169,10 @@ void Formation::init(std::map ¶ms) { //float z_pos_sq = (state_->pos()(2) - leader_z_pos) * (state_->pos()(2) - leader_z_pos); // IF using z, will need a diff distance threshold float euc_dist = std::sqrt(x_pos_sq + y_pos_sq); - cout << "Euclidean distance is: " << euc_dist << " and ent id is: " << ent_id << endl; + // cout << "Euclidean distance is: " << euc_dist << " and ent id is: " << ent_id << endl; if(euc_dist < 50 && ent_id != 1){ leader_ = false; - //follower_track[ent_id] = follower_track.size(); - //cout << "Entity " << ent_id << " is no longer the leader. It is now follower #" << follower_track[ent_id] << endl; } } }; @@ -185,8 +182,7 @@ void Formation::init(std::map ¶ms) { // Set up follower tracker subscriber auto tracker_cb = [&](auto &msg) { if(follower_map.count(msg->data.bird_id()) == 0){ - //follower_track[ent_id] = follower_track.size(); - cout << "Adding new follower id: " << msg->data.bird_id() << endl; + // cout << "Adding new follower id: " << msg->data.bird_id() << endl; FollowerData follower; follower.x_pos = msg->data.x_pos(); follower.y_pos = msg->data.y_pos(); @@ -200,8 +196,75 @@ void Formation::init(std::map ¶ms) { follower_track_pub_ = advertise("GlobalNetwork", "FollowerTrack"); } +void Formation::avoidance_vectors(ContactMap &contacts, + std::vector &O_vecs) { + for (auto it = contacts.begin(); it != contacts.end(); it++) { + + // Ignore own position / id + if (it->second.id().id() == parent_->id().id()) { + continue; + } + + if (!avoid_non_team_ && + it->second.id().team_id() != parent_->id().team_id()) { + continue; + } + + Eigen::Vector3d diff = it->second.state()->pos() - state_->pos(); + + double O_mag = 0; + double dist = diff.norm(); + + if (dist > sphere_of_influence_) { + O_mag = 0; + } else if (minimum_range_ < dist && dist <= sphere_of_influence_) { + O_mag = (sphere_of_influence_ - dist) / + (sphere_of_influence_ - minimum_range_); + } else if (dist <= minimum_range_) { + O_mag = 1e10; + } + + Eigen::Vector3d O_dir = -O_mag * diff.normalized(); + + // cout << "Entity id: " << ent_id + // << " and the contact vector id is: " << it->second.id().id() + // << " The avoidance vector: " << O_dir << endl; + + O_vecs.push_back(O_dir); + } +} + bool Formation::step_autonomy(double t, double dt) { + ////////////////////////// Collision Avoidance ////////////////////////// + // Compute repulsion vector from each robot contact + std::vector O_vecs; + + avoidance_vectors(noisy_contacts_, O_vecs); + + // Normalize each repulsion vector and sum + desired_vector_ << 0, 0, 0; + for (auto it = O_vecs.begin(); it != O_vecs.end(); it++) { + if (it->hasNaN()) { + continue; // ignore misbehaved vectors + } + desired_vector_ += *it; + } + + cout << "Desired vector is: " << desired_vector_ << endl; + + //////////////////////////////////////////////////////////////////////// + + ////////////////////////// Collision Avoidance vs Formation switching // + + // Need a threshold for the output vecs - monitoring the x, y and z + // output pos + + // Would be good to figure out the scaling of x and y pos in scrimmage + + + //////////////////////////////////////////////////////////////////////// + // Goal displacements if a follower if(!leader_){ // When adding this kind of offset with the way velocity is calculated, @@ -210,17 +273,17 @@ bool Formation::step_autonomy(double t, double dt) { // displacement zone, decrease speed but never spin in a circle. Will need hard // hard set distance where it will turn around if it goes within the area - cout << "Leader pos is: " << leader_x_pos << ", " << leader_y_pos << ", leader_z_pos" << endl; + // cout << "Leader pos is: " << leader_x_pos << ", " << leader_y_pos << ", leader_z_pos" << endl; goal_(0) = leader_x_pos - x_disp_; goal_(1) = leader_y_pos - y_disp_; goal_(2) = leader_z_pos - z_disp_; - cout << "Updating follower goal positions: " - << " x_pos: " << goal_(0) - << " y_pos: " << goal_(1) - << " z_pos: " << goal_(2) - << endl; + // cout << "Updating follower goal positions: " + // << " x_pos: " << goal_(0) + // << " y_pos: " << goal_(1) + // << " z_pos: " << goal_(2) + // << endl; } // Read data from sensors... @@ -246,16 +309,37 @@ bool Formation::step_autonomy(double t, double dt) { // For now, just handle the vel and distance here to get it working, then can add // it to the pid once ready. Would be good to do this and then handle the follower messaging that // couldn't get working last week - v = follow_v_k_ * diff; + + if(desired_vector_(0) == 0 && desired_vector_(1) == 0 && desired_vector_(2) == 0){ // need to add espsilon fat guards to avoid shaking + v = follow_v_k_ * diff; + cout << "Using diff for velocity: " << v(0) << ", " << v(1) << ", " << v(2) << endl; + } else { + v = desired_vector_ * follower_speed_; + cout << "Using avoidance for velocity: " << v(0) << ", " << v(1) << ", " << v(2) << endl; + } + if(v(0)>follower_speed_){ - v(0) = follower_speed_; - } else if(v(0)<-follower_speed_){ - v(0) = -follower_speed_; + v(0) = follower_speed_; + } else if(v(0)<-follower_speed_){ + v(0) = -follower_speed_; + } + + if(v(1)>follower_speed_){ + v(1) = follower_speed_; + } else if(v(1)<-follower_speed_){ + v(1) = -follower_speed_; } + if(v(2)>follower_speed_){ + v(2) = follower_speed_; + } else if(v(2)<-follower_speed_){ + v(2) = -follower_speed_; + } } else { v = leader_speed_ * diff.normalized(); } + + cout << "Output v for entity: " << ent_id << ": " << v(0) << ", " << v(1) << ", " << v(2) << endl; double heading = Angles::angle_2pi(atan2(v(1), v(0))); vars_.output(desired_alt_idx_, goal_(2)); From a936bb8aaf4b24b8b3ba1aaea5d50a2a43a783e5 Mon Sep 17 00:00:00 2001 From: Natalie Date: Wed, 18 Oct 2023 22:48:32 -0400 Subject: [PATCH 09/13] Code cleanup and updated xml params --- .../plugins/autonomy/Formation/Formation.h | 1 + .../plugins/autonomy/Formation/Formation.xml | 8 + src/plugins/autonomy/Formation/Formation.cpp | 145 ++++++++---------- 3 files changed, 76 insertions(+), 78 deletions(-) diff --git a/include/scrimmage/plugins/autonomy/Formation/Formation.h b/include/scrimmage/plugins/autonomy/Formation/Formation.h index 1b01f72447..f003c27c89 100644 --- a/include/scrimmage/plugins/autonomy/Formation/Formation.h +++ b/include/scrimmage/plugins/autonomy/Formation/Formation.h @@ -68,6 +68,7 @@ class Formation : public scrimmage::Autonomy{ double x_disp_; double y_disp_; double z_disp_; + double lead_to_follow_dist_; // Entity avoidance bool avoid_non_team_; diff --git a/include/scrimmage/plugins/autonomy/Formation/Formation.xml b/include/scrimmage/plugins/autonomy/Formation/Formation.xml index 13b7e7797f..ad8300137e 100644 --- a/include/scrimmage/plugins/autonomy/Formation/Formation.xml +++ b/include/scrimmage/plugins/autonomy/Formation/Formation.xml @@ -5,16 +5,24 @@ 15 false + 10 + add an assert that checks if the avoidance dist, safety dist, follow dist etc make sense together + 30 0.55 0 0 0 + 50 false + true + 30 + 5 + false diff --git a/src/plugins/autonomy/Formation/Formation.cpp b/src/plugins/autonomy/Formation/Formation.cpp index 7253dc6725..d23028be92 100644 --- a/src/plugins/autonomy/Formation/Formation.cpp +++ b/src/plugins/autonomy/Formation/Formation.cpp @@ -20,14 +20,25 @@ * 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 + * @author Natalie Davis + * @date 01 September 2023 * @version 0.1.0 * @brief Brief file description. * @section DESCRIPTION * A Long description goes here. * + * To do: + * + * 1. Change how follower tracks the leader for the follower call back. It should be a param in the + * struct that tracks the entity id of the leader if this is allowed information to track. It seems + * we only know the position, trajectory, and desired formation - so entity ID might not be allowed. + * + * 2. Handle the z position for euclidean distance calculations + * + * 3. Update the follower track to update the x, y, and z positions of the followers. Currently it + * only adds them from the first time the message is sent + * + * 4. Update color tracking of the spheres. Currently only changes to green if the follow track is not empty */ #include @@ -78,51 +89,43 @@ namespace scrimmage { namespace autonomy { void Formation::init(std::map ¶ms) { + + // Set up entity data ent_id = parent_->id().id(); + // Leader information leader_speed_ = scrimmage::get("leader_speed", params, 0.0); leader_ = scrimmage::get("leader", params, false); - safety_dist_ = scrimmage::get("safety_dist", params, 0.0); + // Follower information follower_speed_ = scrimmage::get("follower_speed", params, 0.0); follow_v_k_ = scrimmage::get("follow_v_k", params, 0.0); x_disp_ = scrimmage::get("x_disp", params, 0.0); y_disp_ = scrimmage::get("y_disp", params, 0.0); z_disp_ = scrimmage::get("z_disp", params, 0.0); + lead_to_follow_dist_ = scrimmage::get("lead_to_follow_dist", params, 0.0); + // Toggles spheres around the entity show_shapes_ = scrimmage::get("show_shapes", params, false); - // Entity avoidance - avoid_non_team_ = sc::get("avoid_non_team", params, true); - sphere_of_influence_ = sc::get("sphere_of_influence", params, 30); - minimum_range_ = sc::get("minimum_range", params, 5); + // Entity avoidance parameters + avoid_non_team_ = scrimmage::get("avoid_non_team", params, true); + sphere_of_influence_ = scrimmage::get("sphere_of_influence", params, 0.0); + minimum_range_ = scrimmage::get("minimum_range", params, 0.0); - cout << "Sphere: " << sphere_of_influence_ << " Min range: " << minimum_range_ << endl; - - // Project goal in front... + // Project initial goal in front of entity 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); - // 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); - - frame_number_ = 0; - + // Handles entities hitting the edge of the simulation map 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); + // Handles noise for state and contacts auto state_cb = [&](auto &msg) { noisy_state_set_ = true; noisy_state_ = msg->data; @@ -134,13 +137,14 @@ void Formation::init(std::map ¶ms) { }; subscribe("LocalNetwork", "ContactsWithCovariances", cnt_cb); + // Setting up output vars - includes altitude, speed, and heading 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); - // If initialized as a follower, update track + // If initialized as a follower, update tracking + // follower_map holds a struct for each follower in the formation if(!leader_){ - //follower_track[ent_id] = follower_track.size(); FollowerData follower; follower.x_pos = state_->pos()(0); follower.y_pos = state_->pos()(1); @@ -150,9 +154,14 @@ void Formation::init(std::map ¶ms) { follower_map[ent_id] = follower; } - // Set up follower subscriber + // Set up follower subscriber. This listens to leader data that is used to update the + // leader position tracking, which acts as goal positions for the follower auto follower_cb = [&](auto &msg) { - if(msg->data.bird_id() == ent_id || msg->data.bird_id() != 1){ // Only compute euclidean distance for cur vs ent 1 + + // Only compute euclidean distance if the message is not from the current entity + // and if it is from the leader - assumed to be entity #1 for now. Will need to + // later identify ways a leader should be specified + if(msg->data.bird_id() == ent_id || msg->data.bird_id() != 1){ // Another instance where leader is defined as ent id 1 return; } @@ -162,16 +171,14 @@ void Formation::init(std::map ¶ms) { // If the entity is a leader, but it is also receiver other leader messages, calculate // the euclidean distance. If it is within a range, need to combine formations and define - // a new leader. For the time being, make id 1 the leader. + // a leader. if(leader_){ float x_pos_sq = (state_->pos()(0) - leader_x_pos) * (state_->pos()(0) - leader_x_pos); float y_pos_sq = (state_->pos()(1) - leader_y_pos) * (state_->pos()(1) - leader_y_pos); //float z_pos_sq = (state_->pos()(2) - leader_z_pos) * (state_->pos()(2) - leader_z_pos); // IF using z, will need a diff distance threshold float euc_dist = std::sqrt(x_pos_sq + y_pos_sq); - // cout << "Euclidean distance is: " << euc_dist << " and ent id is: " << ent_id << endl; - - if(euc_dist < 50 && ent_id != 1){ + if(euc_dist < lead_to_follow_dist_ && ent_id != 1){ // Another instance where leader is defined as ent id 1 leader_ = false; } } @@ -179,7 +186,8 @@ void Formation::init(std::map ¶ms) { subscribe("GlobalNetwork", "FormationLeader", follower_cb); leader_pub_ = advertise("GlobalNetwork", "FormationLeader"); - // Set up follower tracker subscriber + // Set up follower tracker subscriber. This updates tracking + // follower_map holds a struct for each follower in the formation auto tracker_cb = [&](auto &msg) { if(follower_map.count(msg->data.bird_id()) == 0){ // cout << "Adding new follower id: " << msg->data.bird_id() << endl; @@ -196,10 +204,10 @@ void Formation::init(std::map ¶ms) { follower_track_pub_ = advertise("GlobalNetwork", "FollowerTrack"); } +// Handles entity avoidance void Formation::avoidance_vectors(ContactMap &contacts, std::vector &O_vecs) { for (auto it = contacts.begin(); it != contacts.end(); it++) { - // Ignore own position / id if (it->second.id().id() == parent_->id().id()) { continue; @@ -237,6 +245,7 @@ void Formation::avoidance_vectors(ContactMap &contacts, bool Formation::step_autonomy(double t, double dt) { ////////////////////////// Collision Avoidance ////////////////////////// + // Compute repulsion vector from each robot contact std::vector O_vecs; @@ -251,30 +260,25 @@ bool Formation::step_autonomy(double t, double dt) { desired_vector_ += *it; } - cout << "Desired vector is: " << desired_vector_ << endl; - - //////////////////////////////////////////////////////////////////////// - - ////////////////////////// Collision Avoidance vs Formation switching // - - // Need a threshold for the output vecs - monitoring the x, y and z - // output pos - - // Would be good to figure out the scaling of x and y pos in scrimmage + ////////////////////////// Boundary Control ////////////////////////// + + 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; + } + } - //////////////////////////////////////////////////////////////////////// + //////////////////////// Velocity & Goal Calc //////////////////////// - // Goal displacements if a follower if(!leader_){ - // When adding this kind of offset with the way velocity is calculated, - // if the drone enters a zone that is closer than the displacement, - // it turns around. Instead, need to have it slow and if it passes into the - // displacement zone, decrease speed but never spin in a circle. Will need hard - // hard set distance where it will turn around if it goes within the area - - // cout << "Leader pos is: " << leader_x_pos << ", " << leader_y_pos << ", leader_z_pos" << endl; - goal_(0) = leader_x_pos - x_disp_; goal_(1) = leader_y_pos - y_disp_; goal_(2) = leader_z_pos - z_disp_; @@ -286,30 +290,10 @@ bool Formation::step_autonomy(double t, double dt) { // << endl; } - // 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 = goal_ - noisy_state_.pos(); Eigen::Vector3d v; if(!leader_){ - // Calculate velocity using a PID controller - // For now, just handle the vel and distance here to get it working, then can add - // it to the pid once ready. Would be good to do this and then handle the follower messaging that - // couldn't get working last week - if(desired_vector_(0) == 0 && desired_vector_(1) == 0 && desired_vector_(2) == 0){ // need to add espsilon fat guards to avoid shaking v = follow_v_k_ * diff; cout << "Using diff for velocity: " << v(0) << ", " << v(1) << ", " << v(2) << endl; @@ -320,9 +304,9 @@ bool Formation::step_autonomy(double t, double dt) { if(v(0)>follower_speed_){ v(0) = follower_speed_; - } else if(v(0)<-follower_speed_){ - v(0) = -follower_speed_; - } + } else if(v(0)<-follower_speed_){ + v(0) = -follower_speed_; + } if(v(1)>follower_speed_){ v(1) = follower_speed_; @@ -339,13 +323,15 @@ bool Formation::step_autonomy(double t, double dt) { v = leader_speed_ * diff.normalized(); } - cout << "Output v for entity: " << ent_id << ": " << v(0) << ", " << v(1) << ", " << v(2) << endl; + //cout << "Output v for entity: " << ent_id << ": " << v(0) << ", " << v(1) << ", " << v(2) << endl; double heading = Angles::angle_2pi(atan2(v(1), v(0))); vars_.output(desired_alt_idx_, goal_(2)); vars_.output(desired_speed_idx_, v.norm()); vars_.output(desired_heading_idx_, heading); + //////////////////////// Update leader & follower output messages //////////////////////// + if(leader_){ auto leader_msg = std::make_shared>(); leader_msg->data.set_x_pos(state_->pos()(0)); @@ -362,6 +348,8 @@ bool Formation::step_autonomy(double t, double dt) { follower_track_pub_->publish(follower_track_msg); } + //////////////////////// Handle spheres drawn around entity //////////////////////// + if (show_shapes_) { // Draw the sphere of influence if (circle_shape_ == nullptr) { @@ -375,6 +363,7 @@ bool Formation::step_autonomy(double t, double dt) { draw_shape(circle_shape_); } + //////////////////////// Return from step function //////////////////////// noisy_state_set_ = false; return true; } From 5a822b40f2b55e7bd613a8554c91673e4b34c0f5 Mon Sep 17 00:00:00 2001 From: Natalie Date: Fri, 20 Oct 2023 00:06:46 -0400 Subject: [PATCH 10/13] Added safety checks for setting up the formation mission file --- src/parse/MissionParse.cpp | 44 +++++++++++++++++++++++++++++++++++++- 1 file changed, 43 insertions(+), 1 deletion(-) diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index 7c19f020f4..f8fe98feb1 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -518,6 +518,7 @@ bool MissionParse::parse(const std::string &filename) { node = node->next_sibling()) { std::string nm = node->name(); + if (nm == "autonomy") { nm += std::to_string(autonomy_order++); } else if (nm == "controller") { @@ -536,6 +537,10 @@ bool MissionParse::parse(const std::string &filename) { attr; attr = attr->next_attribute()) { const std::string attr_name = attr->name(); + + // cout << "Attr_name: " << attr_name << "attr value: " << attr->value() << " ent_desc_id: " + // << ent_desc_id << " NM " << nm << endl; + if (attr_name == "param_common") { for (auto &kv : param_common[attr->value()]) { entity_attributes_[ent_desc_id][nm][kv.first] = kv.second; @@ -544,6 +549,43 @@ bool MissionParse::parse(const std::string &filename) { entity_attributes_[ent_desc_id][nm][attr_name] = attr->value(); } } + + // If doing formation control, need to ensure the following autonomy attributes are defined + std::string is_formation = script_info[nm]; + if (is_formation == "Formation"){ + + // Entity avoidance parameter handling + if(entity_attributes_[ent_desc_id][nm]["sphere_of_influence"] == ""){ + cout << "Entity " << ent_desc_id+1 << "'s sphere of influence needs to be defined for entity avoidance on the Formation autonomy." << endl; + } + if(entity_attributes_[ent_desc_id][nm]["minimum_range"] == ""){ + cout << "Entity " << ent_desc_id+1 << "'s minimum range needs to be defined for entity avoidance on the Formation autonomy." << endl; + } + if(entity_attributes_[ent_desc_id][nm]["sphere_of_influence"] < entity_attributes_[ent_desc_id][nm]["minimum_range"]){ + cout << "Entity " << ent_desc_id+1 << "'s sphere of influence must be larger than the minimum range for entity avoidance in the Formation autonomy." << endl; + } + + // Leader/follower parameter handling + if(entity_attributes_[ent_desc_id][nm]["leader"] == ""){ + cout << "Entity " << ent_desc_id+1 << "'s leader bool value needs to be defined for the Formation autonomy." << endl; + } + if(entity_attributes_[ent_desc_id][nm]["leader"] == "false" && + (entity_attributes_[ent_desc_id][nm]["x_disp"] == "" || + entity_attributes_[ent_desc_id][nm]["y_disp"] == "" || + entity_attributes_[ent_desc_id][nm]["z_disp"] == "")){ + cout << "Entity " << ent_desc_id+1 << "'s follower displacements need to be complete with values for x, y, and z displacements." << endl; + } + if(entity_attributes_[ent_desc_id][nm]["leader"] == "false" && entity_attributes_[ent_desc_id][nm]["follower_speed"] == ""){ + cout << "Entity " << ent_desc_id+1 << "'s follower speed needs to be defined." << endl; + } + if(entity_attributes_[ent_desc_id][nm]["leader"] == "false" && entity_attributes_[ent_desc_id][nm]["follow_v_k"] == ""){ + cout << "Entity " << ent_desc_id+1 << "'s follower PID K value needs to be defined." << endl; + } + if(entity_attributes_[ent_desc_id][nm]["leader"] == "true" && entity_attributes_[ent_desc_id][nm]["leader_speed"] == ""){ + cout << "Entity " << ent_desc_id+1 << "'s leader speed needs to be defined." << endl; + } + + } } // For each entity, if the lat/lon are defined, use these values to @@ -578,7 +620,7 @@ bool MissionParse::parse(const std::string &filename) { // Save the initial positions (x,y,z) since we modify them for each // entity during generation if (script_info.count("x") > 0) { - script_info["x0"] = script_info["x"]; + script_info["x0"] = script_info["x"]; // Why would this be updating for only entity 1? } else { cout << "Entity missing 'x' tag." << endl; } From 28ea9fa336a005037227645f166fdc410b9aadb3 Mon Sep 17 00:00:00 2001 From: Natalie Date: Fri, 20 Oct 2023 16:14:40 -0400 Subject: [PATCH 11/13] Entity avoidance refinement, working headon x and v formation examples --- .../plugins/autonomy/Formation/Formation.h | 2 + .../plugins/autonomy/Formation/Formation.xml | 3 +- missions/leadfollow_headon.xml | 67 ++- missions/leadfollow_headon_v.xml | 273 +++++++++++ missions/leadfollow_headon_x.xml | 449 ++++++++++++++++++ missions/leadfollow_v_formation.xml | 4 +- src/parse/MissionParse.cpp | 3 + src/plugins/autonomy/Formation/Formation.cpp | 39 +- 8 files changed, 827 insertions(+), 13 deletions(-) create mode 100644 missions/leadfollow_headon_v.xml create mode 100644 missions/leadfollow_headon_x.xml diff --git a/include/scrimmage/plugins/autonomy/Formation/Formation.h b/include/scrimmage/plugins/autonomy/Formation/Formation.h index f003c27c89..e934ddc84e 100644 --- a/include/scrimmage/plugins/autonomy/Formation/Formation.h +++ b/include/scrimmage/plugins/autonomy/Formation/Formation.h @@ -75,6 +75,8 @@ class Formation : public scrimmage::Autonomy{ double sphere_of_influence_; double minimum_range_; Eigen::Vector3d desired_vector_; + double fat_guard_; + bool avoid_state; bool show_shapes_; scrimmage_proto::ShapePtr circle_shape_; diff --git a/include/scrimmage/plugins/autonomy/Formation/Formation.xml b/include/scrimmage/plugins/autonomy/Formation/Formation.xml index ad8300137e..1d08870944 100644 --- a/include/scrimmage/plugins/autonomy/Formation/Formation.xml +++ b/include/scrimmage/plugins/autonomy/Formation/Formation.xml @@ -11,7 +11,7 @@ add an assert that checks if the avoidance dist, safety dist, follow dist etc make sense together - 30 + 15 0.55 0 0 @@ -23,6 +23,7 @@ true 30 5 + 5 false diff --git a/missions/leadfollow_headon.xml b/missions/leadfollow_headon.xml index a8ee1e6af5..3459527a18 100644 --- a/missions/leadfollow_headon.xml +++ b/missions/leadfollow_headon.xml @@ -75,7 +75,7 @@ 0 SimpleAircraftControllerPID - SimpleAircraft + SimpleAircraft zephyr-blue @@ -84,7 +84,7 @@ Formation + leader_speed="5">Formation 35.721112 @@ -108,18 +108,71 @@ 180 SimpleAircraftControllerPID - SimpleAircraft + SimpleAircraft zephyr-blue NoisyContacts Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_3 + 3 + 1 + 1 + 1 + + 0 + -100 + 195 + 90 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + show_shapes="true" + + sphere_of_influence="8" + minimum_range="2" + fat_guard="3">Formation 35.721112 diff --git a/missions/leadfollow_headon_v.xml b/missions/leadfollow_headon_v.xml new file mode 100644 index 0000000000..61f2a414ea --- /dev/null +++ b/missions/leadfollow_headon_v.xml @@ -0,0 +1,273 @@ + + + + + + + false + 50051 + localhost + + all_dead + + 10 + 1000 + + + 191 191 191 + 10 + + true + all + false + false + + SimpleCollisionMetrics + + ~/.scrimmage/logs + true + + 35.721025 + -120.767925 + 300 + true + 10 + + Boundary + SimpleCollision + GroundCollision + + false + + GlobalNetwork + LocalNetwork + + + 2147483648 + + + + + + + + leader_1 + 1 + 1 + 1 + 1 + + -100 + -2 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_2 + 2 + 1 + 1 + 1 + + 100 + 2 + 195 + 180 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_3 + 3 + 1 + 1 + 1 + + 0 + -100 + 195 + 90 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_4 + 4 + 1 + 1 + 1 + + 0 + -75 + 195 + 90 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_5 + 5 + 1 + 1 + 1 + + 0 + 75 + 195 + 270 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + diff --git a/missions/leadfollow_headon_x.xml b/missions/leadfollow_headon_x.xml new file mode 100644 index 0000000000..9eae264877 --- /dev/null +++ b/missions/leadfollow_headon_x.xml @@ -0,0 +1,449 @@ + + + + + + + false + 50051 + localhost + + all_dead + + 10 + 1000 + + + 191 191 191 + 10 + + true + all + false + false + + SimpleCollisionMetrics + + ~/.scrimmage/logs + true + + 35.721025 + -120.767925 + 300 + true + 10 + + Boundary + SimpleCollision + GroundCollision + + false + + GlobalNetwork + LocalNetwork + + + 2147483648 + + + + + + + + leader_1 + 1 + 1 + 1 + 1 + + -100 + -2 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_2 + 2 + 1 + 1 + 1 + + -100 + 15 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_3 + 3 + 1 + 1 + 1 + + -120 + 5 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_4 + 4 + 1 + 1 + 1 + + 0 + -75 + 195 + 90 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_5 + 5 + 1 + 1 + 1 + + 0 + -95 + 195 + 90 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_6 + 6 + 1 + 1 + 1 + + 15 + -80 + 195 + 90 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_7 + 7 + 1 + 1 + 1 + + -90 + 25 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_8 + 8 + 1 + 1 + 1 + + -60 + 25 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_9 + 9 + 1 + 1 + 1 + + -75 + -15 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + diff --git a/missions/leadfollow_v_formation.xml b/missions/leadfollow_v_formation.xml index 02437f74fd..1e568810cc 100644 --- a/missions/leadfollow_v_formation.xml +++ b/missions/leadfollow_v_formation.xml @@ -154,7 +154,9 @@ x_disp = "3" y_disp = "3" z_disp = "0" - show_shapes="true" >Formation + show_shapes="true" + sphere_of_influence="33" + minimum_range="5">Formation 35.721112 diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index f8fe98feb1..41cc3aff1d 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -561,6 +561,9 @@ bool MissionParse::parse(const std::string &filename) { if(entity_attributes_[ent_desc_id][nm]["minimum_range"] == ""){ cout << "Entity " << ent_desc_id+1 << "'s minimum range needs to be defined for entity avoidance on the Formation autonomy." << endl; } + if(entity_attributes_[ent_desc_id][nm]["fat_guard"] == ""){ + cout << "Entity " << ent_desc_id+1 << "'s fat guard value needs to be defined for entity avoidance on the Formation autonomy." << endl; + } if(entity_attributes_[ent_desc_id][nm]["sphere_of_influence"] < entity_attributes_[ent_desc_id][nm]["minimum_range"]){ cout << "Entity " << ent_desc_id+1 << "'s sphere of influence must be larger than the minimum range for entity avoidance in the Formation autonomy." << endl; } diff --git a/src/plugins/autonomy/Formation/Formation.cpp b/src/plugins/autonomy/Formation/Formation.cpp index d23028be92..541922db36 100644 --- a/src/plugins/autonomy/Formation/Formation.cpp +++ b/src/plugins/autonomy/Formation/Formation.cpp @@ -112,6 +112,8 @@ void Formation::init(std::map ¶ms) { avoid_non_team_ = scrimmage::get("avoid_non_team", params, true); sphere_of_influence_ = scrimmage::get("sphere_of_influence", params, 0.0); minimum_range_ = scrimmage::get("minimum_range", params, 0.0); + fat_guard_ = scrimmage::get("fat_guard", params, 0.0); + avoid_state = false; // Project initial goal in front of entity Eigen::Vector3d rel_pos = Eigen::Vector3d::UnitX()*1e6; @@ -207,6 +209,11 @@ void Formation::init(std::map ¶ms) { // Handles entity avoidance void Formation::avoidance_vectors(ContactMap &contacts, std::vector &O_vecs) { + // cout << "-------------Avoidance Calculation-------------" << endl; + + // Minimum distance used for state switching between avoidance and follower velocity calculations + double min_dist = sphere_of_influence_ + fat_guard_; + for (auto it = contacts.begin(); it != contacts.end(); it++) { // Ignore own position / id if (it->second.id().id() == parent_->id().id()) { @@ -223,6 +230,11 @@ void Formation::avoidance_vectors(ContactMap &contacts, double O_mag = 0; double dist = diff.norm(); + // State machine distance track + if(dist < min_dist){ + min_dist = dist; + } + if (dist > sphere_of_influence_) { O_mag = 0; } else if (minimum_range_ < dist && dist <= sphere_of_influence_) { @@ -233,6 +245,8 @@ void Formation::avoidance_vectors(ContactMap &contacts, } Eigen::Vector3d O_dir = -O_mag * diff.normalized(); + // cout << "Sphere val: " << sphere_of_influence_ << " Dist val: " << dist << " Min range: " << minimum_range_ + // << " O_mag: " << O_mag << endl; // cout << "Entity id: " << ent_id // << " and the contact vector id is: " << it->second.id().id() @@ -240,6 +254,14 @@ void Formation::avoidance_vectors(ContactMap &contacts, O_vecs.push_back(O_dir); } + + if(min_dist < (sphere_of_influence_ + fat_guard_)){ + avoid_state = true; + } else{ + avoid_state = false; + } + + // cout << "-----------------------------------------------" << endl; } bool Formation::step_autonomy(double t, double dt) { @@ -294,12 +316,21 @@ bool Formation::step_autonomy(double t, double dt) { Eigen::Vector3d v; if(!leader_){ - if(desired_vector_(0) == 0 && desired_vector_(1) == 0 && desired_vector_(2) == 0){ // need to add espsilon fat guards to avoid shaking + // Needs to be more like a state machine, not just checking avoidance every time + if(!avoid_state){ // avoid state determiner should be based on the min dist value from avoidance function v = follow_v_k_ * diff; - cout << "Using diff for velocity: " << v(0) << ", " << v(1) << ", " << v(2) << endl; - } else { + // cout << "--------------------------DIFF VEL--------------------------" << endl; + // cout << "Using diff for velocity: " << v(0) << ", " << v(1) << ", " << v(2) << endl; + // cout << "Desired vector: " << desired_vector_(0) << " , " << desired_vector_(1) << " , " << desired_vector_(2) << endl; + // cout << "Diff is: " << diff(0) << " ," << v(1) << " ," << v(2) << endl; + // cout << "-------------------------------------------------------------" << endl; + } else { // only come out of avoidance if you are within a certain min distance + fat guard v = desired_vector_ * follower_speed_; + cout << "--------------------------AVOIDANCE VEL--------------------------" << endl; cout << "Using avoidance for velocity: " << v(0) << ", " << v(1) << ", " << v(2) << endl; + cout << "Desired vector: " << desired_vector_(0) << " , " << desired_vector_(1) << " , " << desired_vector_(2) << endl; + cout << "Diff is: " << diff(0) << " ," << v(1) << " ," << v(2) << endl; + cout << "-------------------------------------------------------------" << endl; } if(v(0)>follower_speed_){ @@ -323,7 +354,7 @@ bool Formation::step_autonomy(double t, double dt) { v = leader_speed_ * diff.normalized(); } - //cout << "Output v for entity: " << ent_id << ": " << v(0) << ", " << v(1) << ", " << v(2) << endl; + cout << "Output v for entity: " << ent_id << ": " << v(0) << ", " << v(1) << ", " << v(2) << endl; double heading = Angles::angle_2pi(atan2(v(1), v(0))); vars_.output(desired_alt_idx_, goal_(2)); From f7866198b8c7f54748d7eb3b7348315c5c5f67fa Mon Sep 17 00:00:00 2001 From: Natalie Date: Wed, 29 Nov 2023 19:36:50 -0500 Subject: [PATCH 12/13] Bowling pin formation --- missions/leadfollow_bowlingpin_formation.xml | 441 +++++++++++++++++++ 1 file changed, 441 insertions(+) create mode 100644 missions/leadfollow_bowlingpin_formation.xml diff --git a/missions/leadfollow_bowlingpin_formation.xml b/missions/leadfollow_bowlingpin_formation.xml new file mode 100644 index 0000000000..209771292b --- /dev/null +++ b/missions/leadfollow_bowlingpin_formation.xml @@ -0,0 +1,441 @@ + + + + + + + false + 50051 + localhost + + all_dead + + 10 + 1000 + + + 191 191 191 + 10 + + true + all + false + false + + SimpleCollisionMetrics + + ~/.scrimmage/logs + true + + 35.721025 + -120.767925 + 300 + true + 10 + + Boundary + SimpleCollision + GroundCollision + + false + + GlobalNetwork + LocalNetwork + + + 2147483648 + + + + + + + + leader_1 + 1 + 1 + 1 + 1 + + -900 + 0 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_2 + 2 + 1 + 1 + 1 + + -910 + 10 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_3 + 3 + 1 + 1 + 1 + + -910 + -10 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_4 + 4 + 1 + 1 + 1 + + -910 + 0 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_5 + 5 + 1 + 1 + 1 + + -920 + 20 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_6 + 6 + 1 + 1 + 1 + + -920 + -20 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_7 + 7 + 1 + 1 + 1 + + -920 + 0 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_8 + 8 + 1 + 1 + 1 + + -920 + -10 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_9 + 9 + 1 + 1 + 1 + + -920 + 10 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + From 2cabe12e9e52e1a5e8835cb197610edaaf7337fb Mon Sep 17 00:00:00 2001 From: Natalie Date: Wed, 29 Nov 2023 19:44:36 -0500 Subject: [PATCH 13/13] Headon bowling pin --- missions/leadfollow_headon_bowlinpin.xml | 361 +++++++++++++++++++++++ 1 file changed, 361 insertions(+) create mode 100644 missions/leadfollow_headon_bowlinpin.xml diff --git a/missions/leadfollow_headon_bowlinpin.xml b/missions/leadfollow_headon_bowlinpin.xml new file mode 100644 index 0000000000..2146944121 --- /dev/null +++ b/missions/leadfollow_headon_bowlinpin.xml @@ -0,0 +1,361 @@ + + + + + + + false + 50051 + localhost + + all_dead + + 10 + 1000 + + + 191 191 191 + 10 + + true + all + false + false + + SimpleCollisionMetrics + + ~/.scrimmage/logs + true + + 35.721025 + -120.767925 + 300 + true + 10 + + Boundary + SimpleCollision + GroundCollision + + false + + GlobalNetwork + LocalNetwork + + + 2147483648 + + + + + + + + leader_1 + 1 + 1 + 1 + 1 + + -100 + -2 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_2 + 2 + 1 + 1 + 1 + + -100 + 15 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_3 + 3 + 1 + 1 + 1 + + -120 + 5 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_4 + 4 + 1 + 1 + 1 + + 0 + -75 + 195 + 90 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_5 + 5 + 1 + 1 + 1 + + 0 + -95 + 195 + 90 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_6 + 6 + 1 + 1 + 1 + + 15 + -80 + 195 + 90 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + + + + leader_7 + 7 + 1 + 1 + 1 + + -60 + 25 + 195 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + + zephyr-blue + + NoisyContacts + + Formation + + + 35.721112 + -120.770305 + 300 + 25 + + + +