feature(#183): Stochastic flocking

- Closing in on initial cut an an implementation

- Will require injection of global swarm state each timestep, which is kind of
  icky, but the quickest/most computationally efficient way to do it.

cmake/cosm-config-summary.cmake

@@ -1,8 +1,9 @@
 function(cosm_config_summary )
-  message(STATUS "")
-  message(STATUS "")
-  message(STATUS "COSM Configuration Summary:")
-  message(STATUS "")
+  message("")
+  message("--------------------------------------------------------------------------------")
+  message("                           COSM Configuration Summary")
+  message("--------------------------------------------------------------------------------")
+  message("")
 
   message(STATUS "Build environment.....................: COSM_BUILD_ENV=${COSM_BUILD_ENV}")
   message(STATUS "Building for..........................: COSM_BUILD_FOR=${COSM_BUILD_FOR}")
@@ -17,4 +18,7 @@ function(cosm_config_summary )
     message(STATUS "Robot name prefix.....................: COSM_ROS_ROBOT_NAME_PREFIX=${COSM_ROS_ROBOT_NAME_PREFIX}")
     message(STATUS "Custom msg MD5........................: COSM_ROS_MD5=${COSM_ROS_MD5}")
   endif()
+  message("")
+  message("--------------------------------------------------------------------------------")
+
 endfunction()

cmake/project-local.cmake

@@ -143,6 +143,7 @@ string(CONCAT common_regex
   "src/init|"
   "src/subsystem|"
   "src/apf2D|"
+  "src/flocking|"
   "src/metrics"
   )
 

include/cosm/apf2D/apf_manager.hpp

@@ -12,6 +12,7 @@
  * Includes
  ******************************************************************************/
 #include <memory>
+#include <vector>
 
 #include "rcppsw/er/client.hpp"
 #include "rcppsw/rcppsw.hpp"
@@ -34,6 +35,11 @@ class polar_force;
 class wander_force;
 } /* namespace nav */
 
+namespace flocking {
+class constant_speed_force;
+class alignment_force;
+} /* namespace flocking */
+
 namespace config {
 struct apf_manager_config;
 } /* namespace config */
@@ -156,25 +162,48 @@ class apf_manager : public rer::client<apf_manager> {
   anti_phototaxis(const nav::phototaxis_force::camera_sensor_readings& readings,
                   const rutils::color& color);
 
-  void accum(const rmath::vector2d& force);
+  /**
+   * \brief Calculate the \ref flocking:alignment_force for this timestep.
+   *
+   * \param others The VELOCITIES of other agents to which the current agent
+   *               should try to align its heading to the centroid of.
+   */
+  rmath::vector2d alignment(const std::vector<rmath::vector2d> others);
+
+  /**
+   * \brief Calculate the \ref flocking:constant_speed_force for this timestep.
+   *
+   * \param others The VELOCITIES of other agents to which the current agent
+   *               should try to align its heading to the centroid of.
+   */
+  rmath::vector2d constant_speed(const std::vector<rmath::vector2d> others);
 
+  /**
+   * \brief Add a calculated force to the running total of the forces acting on
+   * the agent since the last reset. Does not apply the force(s) to the agent.
+   */
+  void accum(const rmath::vector2d& force);
 
   bool within_slowing_radius(void) const;
 
  private:
   const boid& entity(void) const { return m_entity; }
 
   /* clang-format off */
-  bool                                       m_enabled{true};
-  boid&                                      m_entity;
-  rmath::vector2d                            m_force_accum{};
-  std::unique_ptr<nav::avoidance_force>      m_avoidance;
-  std::unique_ptr<nav::arrival_force>        m_arrival;
-  std::unique_ptr<nav::wander_force>         m_wander;
-  std::unique_ptr<nav::polar_force>          m_polar;
-  std::unique_ptr<nav::phototaxis_force>     m_phototaxis;
-  std::unique_ptr<nav::path_following_force> m_path_following;
-  class tracker                              m_tracker{};
+  bool                                            m_enabled{true};
+  boid&                                           m_entity;
+  rmath::vector2d                                 m_force_accum{};
+  std::unique_ptr<nav::avoidance_force>           m_avoidance;
+  std::unique_ptr<nav::arrival_force>             m_arrival;
+  std::unique_ptr<nav::wander_force>              m_wander;
+  std::unique_ptr<nav::polar_force>               m_polar;
+  std::unique_ptr<nav::phototaxis_force>          m_phototaxis;
+  std::unique_ptr<nav::path_following_force>      m_path_following;
+
+  std::unique_ptr<flocking::alignment_force>      m_alignment;
+  std::unique_ptr<flocking::constant_speed_force> m_constant_speed;
+
+  class tracker                                   m_tracker{};
   /* clang-format on */
 };
 

include/cosm/apf2D/boid_vector.hpp

@@ -0,0 +1,28 @@
+/**
+ * \file boid_vector.hpp
+ *
+ * \copyright 2022 SIFT LLC, All rights reserved.
+ *
+ * SPDX-License Identifier: MIT
+ */
+
+#pragma once
+
+/*******************************************************************************
+ * Includes
+ ******************************************************************************/
+#include <vector>
+
+#include "cosm/apf2D/boid.hpp"
+
+/*******************************************************************************
+ * Namespaces/Decls
+ ******************************************************************************/
+namespace cosm::apf2D {
+
+/*******************************************************************************
+ * Type Definitions
+ ******************************************************************************/
+using boid_vectorro = std::vector<const boid*>;
+
+} /* namespace cosm::apf2D */

include/cosm/apf2D/config/apf_manager_config.hpp

@@ -12,6 +12,7 @@
  * Includes
  ******************************************************************************/
 #include "cosm/apf2D/nav/config/nav_config.hpp"
+#include "cosm/apf2D/flocking/config/flocking_config.hpp"
 
 /*******************************************************************************
  * Namespaces/Decls
@@ -28,6 +29,7 @@ namespace cosm::apf2D::config {
 struct apf_manager_config final : public rconfig::base_config {
   /* clang-format off */
   nav::config::nav_config nav{};
+  flocking::config::flocking_config flocking{};
   /* clang-format on */
 };
 

include/cosm/apf2D/flocking/alignment_force.hpp

@@ -0,0 +1,61 @@
+/**
+ * \file alignment_force.hpp
+ *
+ * \copyright 2018 John Harwell, All rights reserved.
+ *
+ * SPDX-License-Identifier: LGPL-2.0-or-later
+ */
+
+#pragma once
+
+/*******************************************************************************
+ * Includes
+ ******************************************************************************/
+#include <vector>
+
+#include "cosm/cosm.hpp"
+#include "cosm/apf2D/boid.hpp"
+#include "cosm/apf2D/flocking/config/alignment_force_config.hpp"
+
+/*******************************************************************************
+ * Namespaces/Decls
+ ******************************************************************************/
+namespace cosm::apf2D::flocking {
+
+/*******************************************************************************
+ * Class Definitions
+ ******************************************************************************/
+/**
+ * \class alignment_force
+ * \ingroup apf2D flocking
+ *
+ * \brief A force pulling an agent towards a single other agent, or towards the
+ * centroid of the positions of a set of agents.
+ *
+ * From \cite FLOCK:Bagarti2018-stochfov.
+ */
+class alignment_force {
+ public:
+  explicit alignment_force(const config::alignment_force_config* config);
+
+   /**
+   * \brief Calculate the force.
+   *
+   * \param agent The current agent.
+   *
+   * \param neighbors The velocities of the neighbors of \p agent.
+   *
+   * \return The heading correction the agent should take to stay aligned with
+   *         the average position of \p neighbors.
+   */
+
+  rmath::vector2d operator()(const boid& agent,
+                             const std::vector<rmath::vector2d>& others) const;
+
+ private:
+  /* clang-format off */
+  const double mc_max;
+  /* clang-format on */
+};
+
+} /* namespace cosm::apf2D::flocking */

include/cosm/apf2D/flocking/config/alignment_force_config.hpp

@@ -0,0 +1,37 @@
+/**
+ * \file alignment_force_config.hpp
+ *
+ * \copyright 2022 SIFT LLC, All rights reserved.
+ *
+ * SPDX-License Identifier: LGPL-2.0-or-later
+ */
+
+#pragma once
+
+/*******************************************************************************
+ * Includes
+ ******************************************************************************/
+#include "rcppsw/config/base_config.hpp"
+
+/*******************************************************************************
+ * Namespaces/Decls
+ ******************************************************************************/
+namespace cosm::apf2D::flocking::config {
+
+/*******************************************************************************
+ * Structure Definitions
+ ******************************************************************************/
+/**
+ * \struct alignment_force_config
+ * \ingroup apf2D flocking config
+ *
+ * \brief Configuration for \ref capf2D::flocking::alignment_force.
+ */
+struct alignment_force_config : public rconfig::base_config {
+  /**
+   * \brief The maximum strength of the force.
+   */
+  double max{-1};
+};
+
+} /* namespace cosm::apf2D::flocking::config */

include/cosm/apf2D/flocking/config/constant_speed_force_config.hpp

@@ -0,0 +1,50 @@
+/**
+ * \file constant_speed_force_config.hpp
+ *
+ * \copyright 2022 SIFT LLC, All rights reserved.
+ *
+ * SPDX-License Identifier: LGPL-2.0-or-later
+ */
+
+#pragma once
+
+/*******************************************************************************
+ * Includes
+ ******************************************************************************/
+#include "rcppsw/config/base_config.hpp"
+
+#include "rcppsw/math/radians.hpp"
+#include "rcppsw/spatial/euclidean_dist.hpp"
+
+/*******************************************************************************
+ * Namespaces/Decls
+ ******************************************************************************/
+namespace cosm::apf2D::flocking::config {
+
+/*******************************************************************************
+ * Structure Definitions
+ ******************************************************************************/
+/**
+ * \struct constant_speed_force_config
+ * \ingroup spatial strategy flocking config
+ *
+ * \brief Configuration for \ref capf2D::constant_speed_force.
+ */
+struct constant_speed_force_config : public rconfig::base_config {
+  /**
+   * \brief The maximum strength of the force.
+   */
+  double max{-1};
+
+  /**
+   * \brief The "critical" speed the the swarm is collectively trying to
+   *        maintain.
+   *
+   * \f$v_c\f$ in Bagarti2018.
+   */
+
+  double critical_speed{-1};
+};
+
+
+} /* namespace cosm::apf2D::flocking::config */

include/cosm/apf2D/flocking/config/flocking_config.hpp

@@ -0,0 +1,38 @@
+/**
+ * \file flocking_config.hpp
+ *
+ * \copyright 2022 SIFT LLC, All rights reserved.
+ *
+ * SPDX-License Identifier: LGPL-2.0-or-later
+ */
+
+#pragma once
+
+/*******************************************************************************
+ * Includes
+ ******************************************************************************/
+#include "cosm/apf2D/flocking/config/alignment_force_config.hpp"
+#include "cosm/apf2D/flocking/config/constant_speed_force_config.hpp"
+
+/*******************************************************************************
+ * Namespaces/Decls
+ ******************************************************************************/
+namespace cosm::apf2D::flocking::config {
+
+/*******************************************************************************
+ * Structure Definitions
+ ******************************************************************************/
+/**
+ * \struct flocking_config
+ * \ingroup apf2D flocking config
+ *
+ * \brief Configuration force APF forces related to flocking.
+ */
+struct flocking_config : public rconfig::base_config {
+  /* clang-format off */
+  alignment_force_config            alignment{};
+  constant_speed_force_config       constant_speed{};
+  /* clang-format on */
+};
+
+} /* namespace cosm::apf2D::flocking::config */