Removed ekf (#314)

* added gt creation script

* updated create groundtruth script

* updates

* updateS

* fixed path issue

* move run command to utils

* added run graph bag and plot results script

* added option for map name to make groundtruth map

* fixed typo in gt map script

* fixed brisk typo

* added missing gt path arg

* changed run graph and plot img option

* started make gt script

* updated run graph and plot command

* updates run graph bag command

* update make gt script

* uncommented make groundtruth map

* fixed bag creation in make groundtruth

* added gt sweep file

* formated scripts

* updated make gt with basename call

* formatted scripts

* passing scripts through black and isort

* updated descriptions for each groundtruth script

* passing code through linter (ubuntu20 only)

* apply black formating diff

* improved script documentation

* updated readme with new scripts

* removed ekf dir

* removed ekf_bag

* removed some more matlab, auto gen code, and ekf_bag

* tmp removed ekf_video

* removing ekf dependencies in gnc_autocode and sim_wrapper

* make gnc_autocode compile

* wip getting code to autogenerate without est

* remove ekf from being generated

* reverse ekf matlab delete

Co-authored-by: Marina Moreira <[email protected]>

doc/diagrams/architecture.puml

@@ -65,7 +65,6 @@ frame localization {
 frame gnc {
   [CTL]
   [FAM]
-  [EKF]
   () "/gnc/control\nff_msgs::ControlAction" <<action>> as ControlAction
   () "/gnc/ekf/set_input\nff_msgs::SetEkfInput" <<service>> as EkfSetInput
   () "/gnc/ekf\nff_msgs::EkfState" <<msg>> as EkfState
@@ -94,7 +93,7 @@ EXECUTIVE <<-->> ArmAction
 EXECUTIVE --> InertiaStamped
 InertiaStamped --> FAM
 InertiaStamped --> CTL
-InertiaStamped --> EKF
+InertiaStamped --> LOC 
 
 
 DockAction -- DOCK
@@ -103,14 +102,14 @@ DOCK <<-->> SwitchAction
 PERCH <<-->> SwitchAction
 SwitchAction -- LOCALIZATION_MANAGER
 LOCALIZATION_MANAGER --> EkfSetInput
-EkfSetInput --|> EKF
+EkfSetInput --|> LOC 
 DOCK <<-->> MotionAction
 PERCH <<-->> MotionAction
 PERCH <<-->> ArmAction
 MotionAction -- CHOREOGRAPHER
 CHOREOGRAPHER <<-->> ControlAction
 CHOREOGRAPHER --> SpeedGain
-SpeedGain --> EKF
+SpeedGain --> LOC 
 SpeedGain --> FAM
 ControlAction -- CTL
 CTL --> WrenchStamped
@@ -123,9 +122,9 @@ ArmAction -- ARM
 WrenchStamped --> FAM
 FAM --> PmcCommand
 ARM --> JointGoals
-EKF --> PoseStamped
-EKF --> TwistStamped
-EKF --> EkfState
+LOC --> PoseStamped
+LOC --> TwistStamped
+LOC --> EkfState
 TwistStamped --> CTL
 PoseStamped --> CTL
 JointStates --> ARM

doc/diagrams/gnc_overview.puml

@@ -11,7 +11,6 @@ frame mob {
 }
 
 frame gnc {
-  gnc_node(EKF)
   gnc_node(CTL)
   gnc_node(FAM)
   action(Control, ff_msgs::ControlAction, /gnc/control)
@@ -31,20 +30,20 @@ message(Landmarks, ff_msgs::VisualLandmarks, "/loc/{ml,ar,of}/features")
 latching_msg(Inertia, geometry_msgs::InertiaStamped, /mob/inertia)
 latching_msg(FlightMode, ff_msgs::FlightMode, /mob/flight_mode)
 
-Landmarks -r-> EKF : "                "
+Landmarks -r-> LOC : "                "
 
 FlightMode --> FAM
 FlightMode --> choregrapher
-Inertia -up-> EKF
+Inertia -up-> LOC
 Inertia -up-> CTL
 Inertia -up-> FAM
 
-'EKF -r-> Pose
-'EKF -r-> Vels
+'LOC -r-> Pose
+'LOC -r-> Vels
 'Pose -r-> CTL
 'Vels -r-> CTL
 
-EKF -r-> CTL : "/loc/{pose,twist}\n[geometry_msgs::{Pose,Twist}Stamped]"
+LOC -r-> CTL : "/loc/{pose,twist}\n[geometry_msgs::{Pose,Twist}Stamped]"
 CTL -r-> FAM : "/gnc/ctl/command\n[ff_msgs::FamCommand]"
 'CTL -r-> Wrench : "                "
 'Wrench -r-> FAM : "                "

doc/diagrams/nodes.puml

@@ -110,7 +110,6 @@ node LLP {
   end_manager()
 !endif
   start_manager("GN&C")
-    gnc_node([ekf])
     gnc_node([ctl])
     gnc_node([fam])
   end_manager()

doc/diagrams/notations.puml

@@ -27,10 +27,8 @@ node "Processor (LLP,MLP,HLP)" {
 
 folder "Interfaces (Messages, Services, Actions) Legend" as l2 {
 
-  gnc_node(EKF)
   gnc_node(CTL)
   message(EkfState, ff_msgs::EkfState, /gnc/ekf)
-  EKF --> EkfState
   EkfState --> CTL
 
   action(DockAction, ff_msgs::DockAction, /proc/dock)

doc/diagrams/sim_concepts_robot.puml

@@ -27,13 +27,12 @@ folder "Astrobee Robot" {
 
   node LLP <<CPU>> COLOR_CPU {
     'frame GNC {
-      gnc_node(EKF)
       gnc_node(CTL)
       gnc_node(FAM)
     '}
     drv_node(epson_imu)
     drv_node(pmc_actuator)
-    EKF -> CTL
+    LOC -> CTL
     CTL -> FAM
     message(PmcCommand, ff_hw_msgs::PmcCommand, /hw/pmc/command)
     message(ImuTelem, sensor_msgs::Imu, /hw/imu)
@@ -52,12 +51,12 @@ folder "Astrobee Robot" {
   NavImage --> image_sampler
 
   sparse_mapping --> Features
-  Features ---> EKF
+  Features ---> LOC
 
   image_sampler --> ImgSample
 
   epson_imu --> ImuTelem
-  ImuTelem -> EKF
+  ImuTelem -> LOC
 
   FAM -> PmcCommand
   PmcCommand --> pmc_actuator

doc/diagrams/sim_concepts_simulator.puml

@@ -25,11 +25,10 @@ folder "Development Computer" {
     mgt_node(image_sampler)
     loc_node("real robot\nsparse_mapping\nnode not running")
     'frame GNC {
-      gnc_node(EKF)
       gnc_node(CTL)
       gnc_node(FAM)
     '}
-    EKF -> CTL
+    LOC -> CTL
     CTL -> FAM
   }
 
@@ -43,12 +42,12 @@ folder "Development Computer" {
   NavImage --> image_sampler
 
   sparse_mapping --> Features
-  Features ---> EKF
+  Features ---> LOC
 
   image_sampler --> ImgSample
 
   IMU --> ImuTelem
-  ImuTelem --> EKF
+  ImuTelem --> LOC
 
   FAM -up-> PmcCommand
   PmcCommand -up-> PMC

doc/general_documentation/subsystems.md

@@ -25,7 +25,7 @@ Definition of topic names are maintained in \subpage shared:<br>
   - \subpage localization
   - directory: `localization`
   - topic prefix: `loc`
-  - We keep this name, everything necessary for localization, pose estimation is performed in the GNC EKF...
+  - Everything necessary for localization
   - include:
     - Sparse Mapping Features
     - AR Tags
@@ -52,8 +52,6 @@ Definition of topic names are maintained in \subpage shared:<br>
   - directory: `gnc`
   - topic prefix: `gnc`
   - include:
-    - EKF
-    - Visual Odometry
     - Control
 
 ### Behaviors