Release 0.14.1 (nasa#142)

* Fixing IMU filtering
* Other small fixes from last activity

CMakeLists.txt

@@ -18,7 +18,7 @@
 cmake_minimum_required(VERSION 3.0)
 project(Astrobee)
 
-set(ASTROBEE_VERSION 0.14.0)
+set(ASTROBEE_VERSION 0.14.1)
 
 # Define our options
 option(USE_CCACHE

RELEASE.md

@@ -1,5 +1,13 @@
 # Releases
 
+## Release 0.14.1
+
+  * Dynamic IMU Filtering
+  * Added measurement spacing for visual odometry
+  * Tuning configurations
+  * Bug fixes
+  Note: Perching does not work in this release, use another version for this.
+
 ## Release 0.14.0
 
   * Added graph_localizer package

astrobee.doxyfile

@@ -39,7 +39,7 @@ PROJECT_NAME           = "NASA Astrobee Robot Software"
 # control system is used.
 
 
-PROJECT_NUMBER         = 0.14.0
+PROJECT_NUMBER         = 0.14.1
 
 # Using the PROJECT_BRIEF tag one can provide an optional one line description
 # for a project that appears at the top of each page and should give viewer a

astrobee/config/graph_localizer.config

@@ -17,7 +17,7 @@
 require "context"
 
 -- Graph Value Options
-ideal_duration = 2 
+ideal_duration = 3.5 
 --  Don't leave less than min_num_states in window if possible
 min_num_states = 3
 max_num_states = 20 
@@ -36,13 +36,13 @@ loc_nav_cam_noise_stddev = 0.1
 optical_flow_nav_cam_noise_stddev = 0.1
 loc_dock_cam_noise_stddev = 0.1
 huber_k = world_huber_k
-max_standstill_feature_track_avg_distance_from_mean = 0.075
-standstill_min_num_points_per_track = 5
 -- Only applicable for non zero gravity, causes gravity to be detected as an accelerometer bias instead of 
 -- subtracted from accelerometer measurements which requires the use of global orientation estimates.
 -- Only works when robot is always gravity aligned.  Recommended when testing in the lab.
 ignore_gravity = true 
 estimate_world_T_dock_using_loc = true
+-- Applies to factor adders but not standstill detection
+optical_flow_measurement_spacing = 2 
 -- Sanity Checker options
 -- TODO(rsoussan): Create seperate config for sanity checker?
 check_pose_difference = true 
@@ -62,8 +62,11 @@ starting_prior_gyro_bias_stddev = 0.001
 position_cov_log_det_lost_threshold = 0
 orientation_cov_log_det_lost_threshold = 0
 -- Feature Tracker
-feature_tracker_sliding_window_duration = 0.5 
+feature_tracker_sliding_window_duration = 3.5 
 -- Standstill
+max_standstill_feature_track_avg_distance_from_mean = 0.075
+standstill_min_num_points_per_track = 4 
+standstill_feature_tracker_sliding_window_duration = 1 
 standstill_adder_add_velocity_prior = true 
 standstill_adder_add_pose_between_factor = true 
 standstill_adder_prior_velocity_stddev = 0.01
@@ -81,6 +84,7 @@ smart_projection_adder_verbose_cheirality = false
 smart_projection_adder_robust = true 
 smart_projection_adder_max_num_factors = 10 
 smart_projection_adder_min_num_points = 2 
+smart_projection_adder_max_num_points_per_factor = 5 
 smart_projection_adder_rotation_only_fallback = false 
 smart_projection_adder_splitting = true 
 smart_projection_adder_scale_noise_with_num_points = true 
@@ -100,6 +104,7 @@ loc_adder_add_projections = true
 loc_adder_prior_translation_stddev = 0.01
 loc_adder_prior_quaternion_stddev = 0.01
 loc_adder_min_num_matches = 5
+loc_adder_max_num_factors = 50  
 loc_adder_scale_pose_noise_with_num_landmarks = true 
 loc_adder_scale_projection_noise_with_num_landmarks = false 
 loc_adder_pose_noise_scale = 7 
@@ -114,6 +119,7 @@ ar_tag_loc_adder_add_projections = true
 ar_tag_loc_adder_prior_translation_stddev = 0.01
 ar_tag_loc_adder_prior_quaternion_stddev = 0.01
 ar_tag_loc_adder_min_num_matches = 4 
+ar_tag_loc_adder_max_num_factors = 50
 ar_tag_loc_adder_scale_pose_noise_with_num_landmarks = false 
 ar_tag_loc_adder_scale_projection_noise_with_num_landmarks = false 
 ar_tag_loc_adder_pose_noise_scale = 1 
@@ -126,19 +132,32 @@ rotation_adder_enabled = false
 rotation_adder_min_avg_disparity = 0.2
 rotation_adder_rotation_stddev = 0.1
 rotation_adder_max_percent_outliers = 0.13 
--- Imu integration
--- butter, butter3, butter5, butter5_1, butter5_05, none
-imu_filter = "butter"
-imu_augmentor_standstill = true
--- From gtsam: Angular and velocity random walk expressed in degrees respectively m/s per sqrt(hr)
--- (0.5 * M_PI / 180) / 60;  // 0.5 degree ARW
-gyro_sigma = 0.005    
--- 0.1 / 60; // 10 cm VRW
-accel_sigma = 0.03 
-accel_bias_sigma = 0.01
-gyro_bias_sigma = 0.001
-integration_variance = 0.05
-bias_acc_omega_int = 0.0001
+-- IMU integration
+-- none, ButterOxSxLpxNx, where O is order, S is sample rate, Lp is low pass cutoff, N is notch 
+-- Note that notch depends on the sample rate and aliasing
+imu_filter_quiet_accel = "ButterO3S125Lp3N33_33"
+imu_filter_quiet_ang_vel = "ButterO1S125Lp3N33_33"
+imu_filter_nominal_accel = "ButterO3S125Lp3N41_66"
+imu_filter_nominal_ang_vel = "ButterO1S125Lp3N41_66"
+imu_filter_aggressive_accel = "ButterO3S125Lp3N46_66"
+imu_filter_aggressive_ang_vel = "ButterO1S125Lp3N46_66"
+imu_augmentor_standstill = true 
+gyro_sigma = world_gyro_sigma 
+accel_sigma = world_accel_sigma 
+accel_bias_sigma = world_accel_bias_sigma
+gyro_bias_sigma = world_gyro_bias_sigma
+integration_variance = world_integration_variance
+bias_acc_omega_int = world_bias_acc_omega_int
+-- Graph Localizer Nodelet
+-- Buffer max should be at least 10 and a max of 2 seconds of data
+-- Imu ~62.5 Hz
+max_imu_buffer_size = 125
+-- OF ~10 Hz
+max_optical_flow_buffer_size = 20
+-- Vl ~1 Hz
+max_vl_buffer_size = 10
+-- AR ~1 Hz
+max_ar_buffer_size = 10
 -- Other
 verbose = false 
 fatal_failures = true

astrobee/config/hw/epson_imu.config

@@ -25,7 +25,7 @@ epson_imu = {
   gpio_num_data_ready = robot_imu_drdy_pin,
   gpio_num_reset = 24,
   gpio_num_cs = 62,
-  sampling_rate = 0x06,
+  sampling_rate = 0x05,
   filter = 0x06,
   calibration_sampling_rate = 0x04,
   calibration_filter = 0x04,

astrobee/config/localization/localization_manager.config

@@ -108,13 +108,13 @@ pipelines = {
   { id = "ml", name = "Sparse map", ekf_input = 0,
     needs_filter = true, optical_flow = true, timeout = 1.0, max_confidence = 0,
     enable_topic = "loc/ml/enable", enable_timeout = 20.0,
-    reg_topic = "loc/ml/registration", reg_timeout = 2.0,
+    reg_topic = "loc/ml/registration", reg_timeout = 3.0,
     feat_topic = "loc/ml/features", feat_timeout = 600.0, feat_threshold = 3    },
   -- AR Tags
   { id = "ar", name = "AR Tags", ekf_input = 1,
     needs_filter = true, optical_flow = true, timeout = 1.0, max_confidence = 0,
     enable_topic = "loc/ar/enable", enable_timeout = 10.0,
-    reg_topic = "loc/ar/registration", reg_timeout = 1.0,
+    reg_topic = "loc/ar/registration", reg_timeout = 3.0,
     feat_topic = "loc/ar/features", feat_timeout = 1.0, feat_threshold = 3    },
   -- Handrail
   { id = "hr", name = "Handrail", ekf_input = 2,

astrobee/config/tools/graph_bag.config

@@ -18,6 +18,9 @@
 -- imu
 imu_msg_delay = 0
 imu_min_msg_spacing = 0
+-- flight_mode 
+flight_mode_msg_delay = 0
+flight_mode_min_msg_spacing = 0
 -- optical flow
 of_msg_delay = 0
 of_min_msg_spacing = 0

astrobee/config/worlds/granite.config

@@ -48,6 +48,12 @@ world_vision_ar_tag_filename = "dock_markers_world.config"
 world_handrail_wall_min_gap = 0.11
 -- Huber loss function cutoff point to switch from quadratic to linear
 world_huber_k = 0.5 
+world_gyro_sigma = 0.005    
+world_accel_sigma = 0.03 
+world_accel_bias_sigma = 0.01
+world_gyro_bias_sigma = 0.001
+world_integration_variance = 0.05
+world_bias_acc_omega_int = 0.0001
 
 --------------------------
 --       PHYSICS        --

astrobee/config/worlds/iss.config

@@ -54,6 +54,15 @@ world_vision_ar_tag_filename = "dock_markers_world.config"
 world_handrail_wall_min_gap = 0.06
 -- Huber loss function cutoff point to switch from quadratic to linear
 world_huber_k = 1.345
+-- From gtsam: Angular and velocity random walk expressed in degrees respectively m/s per sqrt(hr)
+-- (0.5 * M_PI / 180) / 60;  // 0.5 degree ARW
+world_gyro_sigma = 0.00001;    
+-- 0.1 / 60; // 10 cm VRW
+world_accel_sigma = 0.00015
+world_accel_bias_sigma = 0.0077
+world_gyro_bias_sigma = 0.0001
+world_integration_variance = 0.0001
+world_bias_acc_omega_int = 0.000015
 
 --------------------------
 --       PHYSICS        --

astrobee/launch/offline_localization/replay_localization.launch

@@ -29,5 +29,5 @@
   <remap from="/mgt/img_sampler/nav_cam/image_record" to="/hw/cam_nav"/>
   <remap from="/gnc/ekf" to="/bag/gnc/ekf"/>
 
-  <node pkg="rosbag" type="play" name="player" output="screen" launch-prefix="$(arg terminal)" args="--clock $(arg bagfile) --topics /hw/imu /hw/cam_nav /mgt/img_sampler/nav_cam/image_record /loc/ar/features $(arg image_feature_topics)" />
+  <node pkg="rosbag" type="play" name="player" output="screen" launch-prefix="$(arg terminal)" args="--clock $(arg bagfile) --topics /hw/imu /hw/cam_nav /mgt/img_sampler/nav_cam/image_record /mob/flight_mode /loc/ar/features $(arg image_feature_topics)" />
 </launch>

behaviors/dock/src/dock_nodelet.cc

@@ -43,6 +43,7 @@
 // Services
 #include <ff_hw_msgs/Undock.h>
 #include <ff_msgs/SetState.h>
+#include <ff_msgs/GetPipelines.h>
 
 // Actions
 #include <ff_msgs/MotionAction.h>
@@ -133,6 +134,14 @@ class DockNodelet : public ff_util::FreeFlyerNodelet {
     // [3]
     fsm_.Add(STATE::UNDOCKED,
       GOAL_DOCK, [this](FSM::Event const& event) -> FSM::State {
+        ff_msgs::GetPipelines srv;
+        if (client_l_.Call(srv)) {
+          if (!srv.response.pipelines.empty() && srv.response.pipelines[0].id == "ar") {
+            // Move to the approach pose using AR localization
+            Move(APPROACH_POSE, ff_msgs::MotionGoal::NOMINAL);
+            return STATE::DOCKING_MOVING_TO_APPROACH_POSE;
+          }
+        }
         Switch(LOCALIZATION_MAPPED_LANDMARKS);
         return STATE::DOCKING_SWITCHING_TO_ML_LOC;
       });
@@ -422,6 +431,9 @@ class DockNodelet : public ff_util::FreeFlyerNodelet {
     server_set_state_ = nh->advertiseService(SERVICE_BEHAVIORS_DOCK_SET_STATE,
       &DockNodelet::SetStateCallback, this);
 
+    // Query the current localization pipeline
+    client_l_.Create(nh, SERVICE_LOCALIZATION_MANAGER_GET_CURR_PIPELINE);
+
     // Contact EPS service for undocking
     client_u_.SetConnectedTimeout(cfg_.Get<double>("timeout_undock_connected"));
     client_u_.SetConnectedCallback(std::bind(
@@ -941,6 +953,7 @@ class DockNodelet : public ff_util::FreeFlyerNodelet {
   ff_util::FSM fsm_;
   ff_util::FreeFlyerActionClient<ff_msgs::MotionAction> client_m_;
   ff_util::FreeFlyerActionClient<ff_msgs::LocalizationAction> client_s_;
+  ff_util::FreeFlyerServiceClient<ff_msgs::GetPipelines>  client_l_;
   ff_util::FreeFlyerServiceClient<ff_hw_msgs::Undock> client_u_;
   ff_util::FreeFlyerActionServer<ff_msgs::DockAction> server_;
   ff_util::ConfigServer cfg_;

cmake/CreateToolTargets.cmake

@@ -66,16 +66,7 @@ function(create_tool_targets)
     if (tool_DEPS)
       add_dependencies(${execname} ${tool_DEPS})
     endif (tool_DEPS)
+    install(TARGETS ${execname} DESTINATION share/${PROJECT_NAME})
     install(TARGETS ${execname} DESTINATION bin)
-
-    # Create symlinks so our executables can be found with rosrun. This is done
-    # inside an 'install' so that it happens after the installation.
-    install(CODE "execute_process(
-      COMMAND mkdir -p share/${PROJECT_NAME}
-      COMMAND ln -s ../../bin/${execname} share/${PROJECT_NAME}/${execname}
-      WORKING_DIRECTORY ${CMAKE_INSTALL_PREFIX}
-      OUTPUT_QUIET
-      ERROR_QUIET
-      )")
   endforeach()
 endfunction()

communications/ff_msgs/msg/GraphState.msg

@@ -32,13 +32,20 @@ uint8 CONFIDENCE_GOOD = 0	# Tracking well
 uint8 CONFIDENCE_POOR = 1	# Tracking poorly
 uint8 CONFIDENCE_LOST = 2	# We are lost
 # Stats
-uint8 of_count # optical flow feature count  
-uint8 ml_count # ml feature count 
+uint32 num_detected_of_features  
+uint32 num_detected_ar_features 
+uint32 num_detected_ml_features 
 uint32 iterations # Optimization iterations
 float32 optimization_time
 float32 update_time # Include optimization_time and other operations to add data to graph
+float32 callbacks_time # Includes processing msgs and their callbacks
+float32 nodelet_runtime # Total runtime of nodelet iteration.  Includes update and callback time
 uint32 num_factors
+uint32 num_of_factors
+uint32 num_ml_projection_factors
+uint32 num_ml_pose_factors
 uint32 num_states
 # Status
 bool standstill
 bool estimating_bias # Are we busy estimating the bias?
+uint8 fan_speed_mode # Used for imu filtering

debian/changelog

@@ -1,3 +1,13 @@
+astrobee (0.14.1) testing; urgency=medium
+
+  * Dynamic IMU Filtering
+  * Added measurement spacing for visual odometry
+  * Tuning configurations
+  * Bug fixes
+  * Note, Perching does not work in this release, use another version for this.
+
+  -- Astrobee Flight Software <[email protected]>  Wed, 03 Mar 2021 14:55:02 -0800
+
 astrobee (0.14.0) testing; urgency=medium
 
   * Added graph_localizer package

localization/graph_localizer/include/graph_localizer/feature_counts.h

@@ -25,10 +25,12 @@ struct FeatureCounts {
   void Reset() {
     vl = 0;
     of = 0;
+    ar = 0;
   }
 
   int vl = 0;
   int of = 0;
+  int ar = 0;
 };
 }  // namespace graph_localizer
 

localization/graph_localizer/include/graph_localizer/graph_localizer.h

@@ -36,6 +36,7 @@
 #include <localization_common/combined_nav_state.h>
 #include <localization_common/combined_nav_state_covariances.h>
 #include <localization_common/time.h>
+#include <localization_measurements/fan_speed_mode.h>
 #include <localization_measurements/feature_points_measurement.h>
 #include <localization_measurements/matched_projections_measurement.h>
 
@@ -85,7 +86,8 @@ class GraphLocalizer {
   bool LocProjectionNoiseScaling(FactorsToAdd& factors_to_add);
   bool ARProjectionNoiseScaling(FactorsToAdd& factors_to_add);
   bool MapProjectionNoiseScaling(const LocFactorAdderParams& params, FactorsToAdd& factors_to_add);
-  void CheckForStandstill();
+  void CheckForStandstill(
+    const localization_measurements::FeaturePointsMeasurement& optical_flow_feature_points_measurement);
   void AddARTagMeasurement(
     const localization_measurements::MatchedProjectionsMeasurement& matched_projections_measurement);
   void AddSparseMappingMeasurement(
@@ -134,6 +136,10 @@ class GraphLocalizer {
 
   const GraphStats& graph_stats() const;
 
+  void SetFanSpeedMode(const localization_measurements::FanSpeedMode fan_speed_mode);
+
+  const localization_measurements::FanSpeedMode fan_speed_mode() const;
+
  private:
   gtsam::NonlinearFactorGraph MarginalFactors(const gtsam::NonlinearFactorGraph& old_factors,
                                               const gtsam::KeyVector& old_keys,
@@ -224,6 +230,7 @@ class GraphLocalizer {
   }
 
   std::shared_ptr<FeatureTracker> feature_tracker_;
+  std::shared_ptr<FeatureTracker> standstill_feature_tracker_;
   imu_integration::LatestImuIntegrator latest_imu_integrator_;
   std::shared_ptr<GraphValues> graph_values_;
   bool log_on_destruction_;