Merge pull request #717 from nasa/develop

Release 0.17.1
nasa · Apr 4, 2023 · ea41de9 · ea41de9
2 parents c46f8b1 + 36f5104
commit ea41de9
Show file tree

Hide file tree

Showing 137 changed files with 511 additions and 251 deletions.
diff --git a/.isort.cfg b/.isort.cfg
@@ -0,0 +1,18 @@
+[settings]
+
+# The src_paths setting should be a comma-separated list of all the
+# folders in the astrobee repo that contain *.py files. Python files
+# found in these folders count as "first party" so they should be in a
+# separate group from the "third party" imports. If src_paths is not
+# specified, isort will treat only the files it's asked to check as
+# first party. Therefore, if we specify src_paths broadly in this way,
+# isort behavior should be more repeatable between (1) manually
+# running isort on a single file in your local dev machine, vs. (2)
+# running the git pre-commit hook locally on your dev machine, which
+# historically ran isort only on the files that changed since the last
+# commit, or (3) running the CI workflow, which always runs isort on
+# all files. If src_paths needs to be updated, like if *.py files are
+# added to a new folder, you can auto-update it by running
+# scripts/git/configure_isort_paths.sh.
+
+src_paths = doc/scripts,hardware/eps_driver/tools,hardware/pico_driver/scripts,hardware/pmc_actuator/tools,localization/localization_common,localization/localization_common/scripts/localization_common,localization/marker_tracking/tools/marker_tracking_node,localization/sparse_mapping/scripts,localization/sparse_mapping/tools,scripts/build,scripts/calibrate,scripts/debug,scripts/git,scripts/postprocessing/coverage_analysis,tools/bag_processing/scripts,tools/bag_processing/scripts/utilities,tools/bag_processing/test,tools/calibration/scripts,tools/gds_helper/src,tools/gnc_visualizer/dds,tools/gnc_visualizer/scripts,tools/gnc_visualizer/scripts/communications,tools/localization_analysis/scripts,tools/performance_tester/scripts
diff --git a/RELEASE.md b/RELEASE.md
@@ -1,5 +1,9 @@
 # Releases
 
+# Release 0.17.1
+
+  * Multiple bug fixes
+
 # Release 0.17.0
 
   * Full compatibility and debians built for Ubuntu 20

diff --git a/astrobee.doxyfile b/astrobee.doxyfile
@@ -39,7 +39,7 @@ PROJECT_NAME           = "NASA Astrobee Robot Software"
 # control system is used.
 
 
-PROJECT_NUMBER         = 0.17.0
+PROJECT_NUMBER         = 0.17.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

diff --git a/astrobee/CMakeLists.txt b/astrobee/CMakeLists.txt
@@ -18,7 +18,7 @@
 cmake_minimum_required(VERSION 3.0)
 project(astrobee)
 
-set(ASTROBEE_VERSION 0.17.0)
+set(ASTROBEE_VERSION 0.17.1)
 
 ## Compile as C++14, supported in ROS Kinetic and newer
 add_compile_options(-std=c++14)

diff --git a/astrobee/launch/robot/MLP.launch b/astrobee/launch/robot/MLP.launch
@@ -89,7 +89,7 @@
   <include file="$(find ff_util)/launch/ff_nodelet.launch">
     <arg name="class" value="depth_odometry/DepthOdometryNodelet" />
     <arg name="name" value="depth_odometry_nodelet" />
-    <arg name="manager" value="mlp_vision" />
+    <arg name="manager" value="mlp_depth_cam" />
     <arg name="spurn" value="$(arg spurn)" />
     <arg name="nodes" value="$(arg nodes)" />
     <arg name="extra" value="$(arg extra)" />

diff --git a/behaviors/arm/tools/arm_tool.cc b/behaviors/arm/tools/arm_tool.cc
@@ -23,7 +23,7 @@
 // Include RPOS
 #include <ros/ros.h>
 // FSW includes
-#include <ff_util/ff_names.h>
+#include <ff_common/ff_names.h>
 #include <ff_util/ff_action.h>
 #include <ff_util/config_client.h>
 

diff --git a/behaviors/dock/tools/dock_tool.cc b/behaviors/dock/tools/dock_tool.cc
@@ -24,7 +24,7 @@
 #include <ros/ros.h>
 
 // FSW includes
-#include <ff_util/ff_names.h>
+#include <ff_common/ff_names.h>
 #include <ff_util/ff_action.h>
 
 // Action

diff --git a/behaviors/light_flow/include/light_flow.h b/behaviors/light_flow/include/light_flow.h
@@ -20,7 +20,7 @@
 #define LIGHT_FLOW_H_
 
 #include <ff_hw_msgs/ConfigureLED.h>
-#include <ff_util/ff_names.h>
+#include <ff_common/ff_names.h>
 #include <jsoncpp/json/value.h>
 #include <ros/ros.h>
 #include <stdio.h>

diff --git a/behaviors/light_flow/src/light_flow/light_flow.cc b/behaviors/light_flow/src/light_flow/light_flow.cc
@@ -20,7 +20,7 @@
 #include <assert.h>
 #include <ff_hw_msgs/ConfigureLED.h>
 #include <ff_hw_msgs/ConfigureLEDGroup.h>
-#include <ff_util/ff_names.h>
+#include <ff_common/ff_names.h>
 #include <jsoncpp/json/allocator.h>
 #include <jsoncpp/json/json.h>
 #include <jsoncpp/json/value.h>

diff --git a/behaviors/light_flow/src/light_flow_nodelet/light_flow_nodelet.cc b/behaviors/light_flow/src/light_flow_nodelet/light_flow_nodelet.cc
@@ -32,7 +32,7 @@
 #include <config_reader/config_reader.h>
 
 // FSW nodelet
-#include <ff_util/ff_names.h>
+#include <ff_common/ff_names.h>
 #include <ff_util/ff_nodelet.h>
 
 // Services

diff --git a/behaviors/light_flow/tools/light_flow_tool.cc b/behaviors/light_flow/tools/light_flow_tool.cc
@@ -18,7 +18,7 @@
 
 #include <ff_hw_msgs/ConfigureLED.h>
 #include <ff_hw_msgs/ConfigureLEDGroup.h>
-#include <ff_util/ff_names.h>
+#include <ff_common/ff_names.h>
 #include <jsoncpp/json/allocator.h>
 #include <jsoncpp/json/json.h>
 #include <jsoncpp/json/value.h>

diff --git a/behaviors/perch/tools/perch_tool.cc b/behaviors/perch/tools/perch_tool.cc
@@ -24,7 +24,7 @@
 #include <ros/ros.h>
 
 // FSW includes
-#include <ff_util/ff_names.h>
+#include <ff_common/ff_names.h>
 #include <ff_util/ff_action.h>
 
 // Action

diff --git a/behaviors/states/src/states_nodelet/states_nodelet.cc b/behaviors/states/src/states_nodelet/states_nodelet.cc
@@ -25,7 +25,7 @@
 #include <config_reader/config_reader.h>
 
 // FSW nodelet
-#include <ff_util/ff_names.h>
+#include <ff_common/ff_names.h>
 #include <ff_util/ff_nodelet.h>
 
 // Services

diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/astrobee_astrobee_bridge.h b/communications/dds_ros_bridge/include/dds_ros_bridge/astrobee_astrobee_bridge.h
@@ -39,7 +39,7 @@
 
 #include "ff_msgs/ResponseOnly.h"
 
-#include "ff_util/ff_names.h"
+#include "ff_common/ff_names.h"
 #include "ff_util/ff_nodelet.h"
 
 // SoraCore Includes

diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/dds_ros_bridge.h b/communications/dds_ros_bridge/include/dds_ros_bridge/dds_ros_bridge.h
@@ -65,7 +65,7 @@
 
 #include "ff_msgs/SetRate.h"
 
-#include "ff_util/ff_names.h"
+#include "ff_common/ff_names.h"
 #include "ff_util/ff_nodelet.h"
 
 // SoraCore Includes

diff --git a/communications/ff_msgs/bmr/rosbag_rewrite_types_rules.json b/communications/ff_msgs/bmr/rosbag_rewrite_types_rules.json
@@ -3,10 +3,11 @@
     ],
     "fix_message_definition_topic_patterns": [
         "/gs/*",
-	"/command",
+        "/command",
         "/hw/cam_sci/compressed",
         "/hw/cam_sci_info",
         "/mgt/disk_monitor/state",
-        "/mgt/cpu_monitor/state"
+        "/mgt/cpu_monitor/state",
+        "/signals"
     ]
 }
diff --git a/communications/ground_dds_ros_bridge/include/ground_dds_ros_bridge/ground_dds_ros_bridge.h b/communications/ground_dds_ros_bridge/include/ground_dds_ros_bridge/ground_dds_ros_bridge.h
@@ -36,7 +36,7 @@
 #include "ground_dds_ros_bridge/ros_command.h"
 #include "ground_dds_ros_bridge/ros_sub_rapid_pub.h"
 
-#include "ff_util/ff_names.h"
+#include "ff_common/ff_names.h"
 
 // SoraCore Includes
 #include "knDds/DdsSupport.h"

diff --git a/debian/changelog b/debian/changelog
@@ -1,9 +1,15 @@
+astrobee (0.17.1) testing; urgency=medium
+
+  * Multiple bug fixes
+
+ -- Astrobee Flight Software <[email protected]>  Mon, 03 Apr 2023 10:59:33 -0700
+
 astrobee (0.17.0) testing; urgency=medium
 
   * Full compatibility and debians built for Ubuntu 20
   * Deleted all simulink autocode for ctl and fam
 
- -- Marina Moreira <mgouveia@ubuntu>  Thu, 16 Feb 2023 09:43:41 -0800
+ -- Astrobee Flight Software <[email protected]>  Thu, 16 Feb 2023 09:43:41 -0800
 
 astrobee (0.16.7) testing; urgency=medium
 

diff --git a/debian/control b/debian/control
@@ -49,7 +49,7 @@ Depends: ${misc:Depends}
   ${ros-python},
   astrobee-config (>= ${binary:Version}),
   astrobee-comms (>= ${binary:Version}),
-  libalvar2 (>=2.0), libdbow21 (>=0.1), libgtsam (>=4.0), libopenmvg1 (>=1.0), libroyale1 (>=1.0),
+  libalvar2 (>=2.0), libdbow21 (>=0.1-6), libgtsam (>=4.0), libopenmvg1 (>=1.0), libroyale1 (>=1.0),
   libceres1 (>=1.0), rti (>=1.0), libmiro0 (>=0.1), libsoracore1 (>=1.0),
   libdecomputil0 (>=0.1), libjps3d0 (>=0.1),
   libluajit-5.1-2,

diff --git a/doc/general_documentation/NASA_INSTALL.md b/doc/general_documentation/NASA_INSTALL.md
@@ -32,9 +32,9 @@ This is the typical case for all wired computers in ARC TI, and simplifies
 your life greatly.
 
 Verify that you are in this situation with the command below should succeed
-(remove the Release.gpg file after being fetched).
+(certificate will be added later; remove the Release.gpg file after being fetched).
 
-    wget -v http://astrobee.ndc.nasa.gov/software/dists/xenial/Release.gpg
+    wget -v --no-check-certificate http://astrobee.ndc.nasa.gov/software/dists/xenial/Release.gpg
 
 Before running the scripts in `scripts/setup` below, set this variable:
 
@@ -234,7 +234,7 @@ will copy all products into this directory.
 Once the installation has completed, copy the install directory to the robot.
 This script assumes that you are connected to the Astrobee network, as it uses
 rsync to copy the install directory to `~/armhf` on the two processors. It 
-takes the robot name as an argument. Here we use `p4d'.
+takes the robot name as an argument. Here we use `p4d`.
 
     pushd $ASTROBEE_WS
     ./src/scripts/install_to_astrobee.sh $INSTALL_PATH p4d

diff --git a/gnc/ctl/src/ctl_ros.cc b/gnc/ctl/src/ctl_ros.cc
@@ -25,7 +25,7 @@
 #include <ff_msgs/Segment.h>
 #include <msg_conversions/msg_conversions.h>
 
-#include <ff_util/ff_names.h>
+#include <ff_common/ff_names.h>
 
 #include <Eigen/Dense>
 #include <Eigen/Geometry>

diff --git a/gnc/ctl/test/test_ctl_nominal.cc b/gnc/ctl/test/test_ctl_nominal.cc
@@ -28,7 +28,7 @@
 #include <ros/ros.h>
 #include <ros/console.h>
 
-#include <ff_util/ff_names.h>
+#include <ff_common/ff_names.h>
 #include <actionlib/client/simple_action_client.h>
 #include <ff_msgs/ControlAction.h>
 #include <ff_msgs/EkfState.h>

diff --git a/gnc/fam/include/fam/fam.h b/gnc/fam/include/fam/fam.h
@@ -26,7 +26,7 @@
 #include <ff_msgs/FamCommand.h>
 #include <ff_msgs/FlightMode.h>
 
-#include <ff_util/ff_names.h>
+#include <ff_common/ff_names.h>
 #include <ff_util/perf_timer.h>
 
 #include <Eigen/Dense>

diff --git a/gnc/fam/src/fam.cc b/gnc/fam/src/fam.cc
@@ -18,7 +18,7 @@
 
 #include <fam/fam.h>
 #include <msg_conversions/msg_conversions.h>
-#include <ff_util/ff_names.h>
+#include <ff_common/ff_names.h>
 #include <ff_hw_msgs/PmcCommand.h>
 
 #include <Eigen/QR>

diff --git a/gnc/pmc/include/pmc/shared.h b/gnc/pmc/include/pmc/shared.h
@@ -20,6 +20,7 @@
 #define PMC_SHARED_H_
 
 #include <Eigen/Dense>
+#include <limits>
 
 namespace pmc {
 
@@ -74,6 +75,24 @@ class PMCConstants {
 
 float Lookup(int table_size, const float lookup[], const float breakpoints[], float value);
 
+template <class MatT>
+Eigen::Matrix<typename MatT::Scalar, MatT::ColsAtCompileTime, MatT::RowsAtCompileTime> pseudoInverse(
+  const MatT& mat, typename MatT::Scalar tolerance = typename MatT::Scalar{std::numeric_limits<double>::epsilon()}) {
+  typedef typename MatT::Scalar Scalar;
+  auto svd = mat.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
+  const auto& singularValues = svd.singularValues();
+  Eigen::Matrix<Scalar, MatT::ColsAtCompileTime, MatT::RowsAtCompileTime> singularValuesInv(mat.cols(), mat.rows());
+  singularValuesInv.setZero();
+  for (unsigned int i = 0; i < singularValues.size(); ++i) {
+    if (singularValues(i) > tolerance) {
+      singularValuesInv(i, i) = Scalar {1} / singularValues(i);
+    } else {
+      singularValuesInv(i, i) = Scalar {0};
+    }
+  }
+  return svd.matrixV() * singularValuesInv * svd.matrixU().adjoint();
+}
+
 }  // end namespace pmc
 
 #endif  // PMC_SHARED_H_
diff --git a/gnc/pmc/src/fam.cc b/gnc/pmc/src/fam.cc
@@ -120,8 +120,7 @@ void Fam::UpdateCOM(const Eigen::Vector3f & com) {
   Eigen::Matrix<float, 6, 12> thrust2forcetorque;
   thrust2forcetorque << thrust2force_, thrust2torque_;
   forcetorque2thrust_ = Eigen::MatrixXf::Identity(12, 6);
-  Eigen::JacobiSVD<Eigen::MatrixXf> svd(thrust2forcetorque, Eigen::ComputeThinU | Eigen::ComputeThinV);
-  forcetorque2thrust_ = svd.solve(forcetorque2thrust_);
+  forcetorque2thrust_ = pseudoInverse(thrust2forcetorque);
 }
 
 void Fam::CalcThrustMatrices(const Eigen::Vector3f & force, const Eigen::Vector3f & torque,

diff --git a/hardware/eps_driver/tools/eps_simulator/eps_simulator.cc b/hardware/eps_driver/tools/eps_simulator/eps_simulator.cc
@@ -20,7 +20,7 @@
 #include <ros/ros.h>
 
 // FSW standard naming
-#include <ff_util/ff_names.h>
+#include <ff_common/ff_names.h>
 
 // Standard messages
 #include <ff_hw_msgs/EpsBatteryLocation.h>

diff --git a/hardware/flashlight/include/flashlight/ros/flashlight_nodelet.h b/hardware/flashlight/include/flashlight/ros/flashlight_nodelet.h
@@ -28,7 +28,7 @@
 
 #include <config_reader/config_reader.h>
 #include <ff_hw_msgs/SetFlashlight.h>
-#include <ff_util/ff_names.h>
+#include <ff_common/ff_names.h>
 #include <ff_util/ff_nodelet.h>
 
 #include <flashlight/flashlight.h>

diff --git a/hardware/is_camera/src/camera.cc b/hardware/is_camera/src/camera.cc
@@ -403,7 +403,7 @@ namespace is_camera {
     if (!thread_running_ || ((pub_.getNumSubscribers() == 0) && (getNumBayerSubscribers() == 0))) return;
 
     // Get last generated image from buffer (index incremented after image completed)
-    sensor_msgs::ImagePtr& grey_image = img_msg_buffer_[(img_msg_buffer_idx_ - 1) % kImageMsgBuffer];
+    sensor_msgs::ImagePtr& grey_image = img_msg_buffer_[(img_msg_buffer_idx_ + kImageMsgBuffer - 1) % kImageMsgBuffer];
 
     cv::Mat input(kImageHeight, kImageWidth,
                      cv::DataType<uint8_t>::type,

diff --git a/hardware/is_camera/src/debayer.cc b/hardware/is_camera/src/debayer.cc
@@ -22,7 +22,7 @@
 #include <pluginlib/class_list_macros.h>
 
 // Shared libraries
-#include <ff_util/ff_names.h>
+#include <ff_common/ff_names.h>
 #include <ff_util/ff_nodelet.h>
 
 #include <image_transport/image_transport.h>

diff --git a/hardware/laser/include/laser/ros/laser_nodelet.h b/hardware/laser/include/laser/ros/laser_nodelet.h
@@ -29,7 +29,7 @@
 
 #include <config_reader/config_reader.h>
 #include <ff_hw_msgs/SetEnabled.h>
-#include <ff_util/ff_names.h>
+#include <ff_common/ff_names.h>
 #include <ff_util/ff_nodelet.h>
 
 #include <i2c/i2c_new.h>

diff --git a/hardware/pico_driver/scripts/debug_pico_utils.py b/hardware/pico_driver/scripts/debug_pico_utils.py
@@ -31,11 +31,12 @@
 import logging
 
 import numpy as np
-import pico_utils as pico
 import rosbag
 from matplotlib import collections as mc
 from matplotlib import pyplot as plt
 
+import pico_utils as pico
+
 
 def plot_xy_grid_many(inbag_path, verbose=False, fast=False, cam=pico.DEFAULT_CAM):
     """

diff --git a/hardware/pico_driver/scripts/pico_check_split_extended.py b/hardware/pico_driver/scripts/pico_check_split_extended.py
@@ -29,6 +29,7 @@
 import logging
 
 import numpy as np
+
 import pico_utils as pico
 
 

diff --git a/hardware/pico_driver/scripts/pico_split_extended.py b/hardware/pico_driver/scripts/pico_split_extended.py
@@ -28,9 +28,10 @@
 import shutil
 
 import numpy as np
-import pico_utils as pico
 import rosbag
 
+import pico_utils as pico
+
 
 class PrintEveryK:
     def __init__(self, k):

diff --git a/hardware/pico_driver/scripts/pico_write_xyz_coeff.py b/hardware/pico_driver/scripts/pico_write_xyz_coeff.py
@@ -26,6 +26,7 @@
 import logging
 
 import numpy as np
+
 import pico_utils as pico