Merge pull request #500 from nasa/develop

Release 0.16.5

RELEASE.md

@@ -1,5 +1,9 @@
 # Releases
 
+## Release 0.16.5
+
+  * Fix multiple bugs
+
 ## Release 0.16.4
 
   * Auto-exposure

astrobee.doxyfile

@@ -39,7 +39,7 @@ PROJECT_NAME           = "NASA Astrobee Robot Software"
 # control system is used.
 
 
-PROJECT_NUMBER         = 0.16.4
+PROJECT_NUMBER         = 0.16.5
 
 # 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/CMakeLists.txt

@@ -18,7 +18,7 @@
 cmake_minimum_required(VERSION 3.0)
 project(astrobee)
 
-set(ASTROBEE_VERSION 0.16.4)
+set(ASTROBEE_VERSION 0.16.5)
 
 ## Compile as C++14, supported in ROS Kinetic and newer
 add_compile_options(-std=c++14)

behaviors/arm/src/arm_nodelet.cc

@@ -196,6 +196,11 @@ class ArmNodelet : public ff_util::FreeFlyerNodelet {
     fsm_.Add(STATE::DEPLOYING_TILTING,
       TILT_COMPLETE,
       [this](FSM::Event const& event) -> FSM::State {
+        if (!calibrated_) {
+           if (!CalibrateGripper())
+            return Result(RESPONSE::CALIBRATE_FAILED);
+          return STATE::CALIBRATING;
+        }
         return Result(RESPONSE::SUCCESS);
       });
 
@@ -252,8 +257,8 @@ class ArmNodelet : public ff_util::FreeFlyerNodelet {
             return STATE::STOWING_SETTING;
           else if (goal_set_)
             return STATE::SETTING;
-          else   // We don't know where we were, return error
-            return Result(RESPONSE::NO_GOAL);
+          else   // Finished
+            return Result(RESPONSE::SUCCESS);
         }
         if (!Arm(PAN))
           return Result(RESPONSE::PAN_FAILED);

debian/changelog

@@ -1,3 +1,9 @@
+astrobee (0.16.5) testing; urgency=medium
+
+  * Fix multiple bugs
+
+ -- Astrobee Flight Software <[email protected]>  Mon, 23 May 2022 14:15:36 -0700
+
 astrobee (0.16.4) testing; urgency=medium
 
   * Auto-exposure

hardware/is_camera/src/camera.cc

@@ -356,7 +356,7 @@ namespace is_camera {
     } else {
       return;
     }
-    enable = bayer_enable_;
+    bayer_enable_ = enable;
   }
 
   // Timer that periodically changes camera exposure based on the captured image's histogram

localization/camera/CMakeLists.txt

@@ -23,15 +23,7 @@ add_compile_options(-std=c++14)
 
 ## Find catkin macros and libraries
 find_package(catkin2 REQUIRED COMPONENTS
-  roscpp
-  std_msgs
-  sensor_msgs
-  cv_bridge
-  image_transport
-  rosbag
   ff_common
-  ff_msgs
-  nodelet
   config_reader
 )
 
@@ -40,19 +32,17 @@ find_package(cmake_modules REQUIRED)
 find_package(Boost REQUIRED COMPONENTS system)
 find_package(Eigen3 REQUIRED)
 
+# Find OpenCV
+SET(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/../../cmake")
+find_package(OpenCV331 REQUIRED)
+
+
+
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES camera
   CATKIN_DEPENDS
-    roscpp
-    std_msgs
-    sensor_msgs
-    cv_bridge
-    image_transport
-    rosbag
     ff_common
-    ff_msgs
-    nodelet
     config_reader
 )
 
@@ -65,6 +55,7 @@ include_directories(
   include
   ${catkin_INCLUDE_DIRS}
   ${EIGEN3_INCLUDE_DIR}
+  ${OpenCV_INCLUDE_DIRS}
 )
 
 # Declare C++ libraries
@@ -73,7 +64,7 @@ add_library(camera
   src/camera_params.cc
 )
 add_dependencies(camera ${catkin_EXPORTED_TARGETS})
-target_link_libraries(camera ${catkin_LIBRARIES} ${EIGEN_LIBRARIES})
+target_link_libraries(camera ${catkin_LIBRARIES} ${EIGEN_LIBRARIES} ${OpenCV_LIBRARIES})
 
 ## Declare a C++ executable: undistort_image
 add_executable(undistort_image tools/undistort_image.cc)
@@ -127,4 +118,4 @@ install(DIRECTORY include/${PROJECT_NAME}/
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
   FILES_MATCHING PATTERN "*.h"
   PATTERN ".svn" EXCLUDE
-)
+)

localization/camera/package.xml

@@ -2,7 +2,7 @@
   <name>camera</name>
   <version>1.0.0</version>
   <description>
-    This package localizes the Astrobee on a map.
+    This package provides camera utilities.
   </description>
   <license>
     Apache License, Version 2.0
@@ -14,28 +14,8 @@
     Astrobee Flight Software
   </maintainer>
   <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>message_generation</build_depend>
-  <build_depend>std_msgs</build_depend>
-  <build_depend>sensor_msgs</build_depend>
-  <build_depend>cv_bridge</build_depend>
-  <build_depend>image_transport</build_depend>
-  <build_depend>rosbag</build_depend>
   <build_depend>ff_common</build_depend>
-  <build_depend>ff_msgs</build_depend>
-  <build_depend>nodelet</build_depend>
-  <build_depend>tf2</build_depend>
   <build_depend>config_reader</build_depend>
-  <run_depend>roscpp</run_depend>
-  <run_depend>message_runtime</run_depend>
-  <run_depend>std_msgs</run_depend>
-  <run_depend>sensor_msgs</run_depend>
-  <run_depend>cv_bridge</run_depend>
-  <run_depend>image_transport</run_depend>
-  <run_depend>rosbag</run_depend>
   <run_depend>ff_common</run_depend>
-  <run_depend>ff_msgs</run_depend>
-  <run_depend>nodelet</run_depend>
-  <run_depend>tf2</run_depend>
   <run_depend>config_reader</run_depend>
 </package>

localization/depth_odometry/CMakeLists.txt

@@ -23,11 +23,13 @@ add_compile_options(-std=c++14)
 
 ## Find catkin macros and libraries
 find_package(catkin2 REQUIRED COMPONENTS
-  camera
+  cv_bridge
   ff_util
+  image_transport
   localization_common 
   localization_measurements 
   msg_conversions 
+  nodelet
   optimization_common
   point_cloud_common
   vision_common
@@ -48,7 +50,7 @@ find_package(pcl_conversions REQUIRED)
 catkin_package(
   INCLUDE_DIRS include ${CERES_INCLUDE_DIRS} ${GTSAM_INCLUDE_DIR} ${PCL_INCLUDE_DIRS} ${pcl_conversions_INCLUDE_DIRS} 
   LIBRARIES ${PROJECT_NAME} ${CERES_LIBRARIES} ${PCL_LIBRARIES} ${GTSAM_LIBRARIES}   
-  CATKIN_DEPENDS roscpp ff_msgs ff_util localization_common localization_measurements msg_conversions optimization_common point_cloud_common vision_common 
+  CATKIN_DEPENDS roscpp cv_bridge ff_msgs ff_util image_transport localization_common localization_measurements msg_conversions nodelet optimization_common point_cloud_common vision_common 
   DEPENDS gtsam pcl 
 )
 
@@ -88,24 +90,29 @@ if(CATKIN_ENABLE_TESTING)
   target_link_libraries(test_point_to_plane_icp_depth_odometry
     ${PROJECT_NAME} ${catkin_LIBRARIES} 
   )
-  add_rostest_gtest(test_image_features_with_known_correspondences_aligner_depth_odometry
-    test/test_image_features_with_known_correspondences_aligner_depth_odometry.test
-    test/test_image_features_with_known_correspondences_aligner_depth_odometry.cc
-    test/test_utilities.cc
-  )
-  target_link_libraries(test_image_features_with_known_correspondences_aligner_depth_odometry
-    ${PROJECT_NAME} ${catkin_LIBRARIES} 
-  )
 
- find_package(rostest REQUIRED)
-  add_rostest_gtest(test_depth_odometry_wrapper
-    test/test_depth_odometry_wrapper.test
-    test/test_depth_odometry_wrapper.cc
-    test/test_utilities.cc
-  )
-  target_link_libraries(test_depth_odometry_wrapper
-    ${PROJECT_NAME} ${catkin_LIBRARIES} 
-  )
+  # Ignore these tests if OpenCV 4 installed as cv_bridge will cause a runtime segfault
+  # since it uses opencv4 on newer ros versions and the rest of the code uses opencv3
+  find_package(OpenCV 4 QUIET)
+  if(NOT OpenCV_FOUND)
+    add_rostest_gtest(test_image_features_with_known_correspondences_aligner_depth_odometry
+      test/test_image_features_with_known_correspondences_aligner_depth_odometry.test
+      test/test_image_features_with_known_correspondences_aligner_depth_odometry.cc
+      test/test_utilities.cc
+    )
+    target_link_libraries(test_image_features_with_known_correspondences_aligner_depth_odometry
+      ${PROJECT_NAME} ${catkin_LIBRARIES} 
+    )
+
+    add_rostest_gtest(test_depth_odometry_wrapper
+      test/test_depth_odometry_wrapper.test
+      test/test_depth_odometry_wrapper.cc
+      test/test_utilities.cc
+    )
+    target_link_libraries(test_depth_odometry_wrapper
+      ${PROJECT_NAME} ${catkin_LIBRARIES} 
+    )
+  endif()
 
 endif()
 

localization/depth_odometry/package.xml

@@ -14,9 +14,10 @@
     Astrobee Flight Software
   </maintainer>
   <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>camera</build_depend>
+  <build_depend>cv_bridge</build_depend>
   <build_depend>ff_msgs</build_depend>
   <build_depend>ff_util</build_depend>
+  <build_depend>image_transport</build_depend>
   <build_depend>localization_common</build_depend>
   <build_depend>localization_measurements</build_depend>
   <build_depend>msg_conversions</build_depend>
@@ -27,9 +28,10 @@
   <build_depend>point_cloud_common</build_depend>
   <build_depend>vision_common</build_depend>
   <build_depend>roscpp</build_depend>
-  <run_depend>camera</run_depend>
+  <run_depend>cv_bridge</run_depend>
   <run_depend>ff_msgs</run_depend>
   <run_depend>ff_util</run_depend>
+  <run_depend>image_transport</run_depend>
   <run_depend>localization_common</run_depend>
   <run_depend>localization_measurements</run_depend>
   <run_depend>msg_conversions</run_depend>

localization/depth_odometry/src/parameter_reader.cc

@@ -16,7 +16,6 @@
  * under the License.
  */
 
-#include <camera/camera_params.h>
 #include <depth_odometry/parameter_reader.h>
 #include <localization_common/logger.h>
 #include <msg_conversions/msg_conversions.h>

localization/ground_truth_localizer/CMakeLists.txt

@@ -39,7 +39,7 @@ if (USE_ROS)
   catkin_package(
     LIBRARIES ${PROJECT_NAME} ${GLOG_LIBRARIES} 
     INCLUDE_DIRS include ${GLOG_INCLUDE_DIRS} 
-    CATKIN_DEPENDS roscpp ff_msgs ff_util localization_common
+    CATKIN_DEPENDS roscpp ff_msgs ff_util localization_common nodelet
   )
 
   ###########

localization/localization_common/CMakeLists.txt

@@ -35,6 +35,9 @@ find_package(GTSAM REQUIRED)
 # System dependencies are found with CMake's conventions
 find_package(Eigen3 REQUIRED)
 
+# Allow other packages to use python scripts from this package
+catkin_python_setup()
+
 catkin_package(
   INCLUDE_DIRS include 
   LIBRARIES ${PROJECT_NAME} ${GTSAM_LIBRARIES} gtsam

localization/localization_common/scripts/localization_common/utilities.py

@@ -0,0 +1,84 @@
+# Copyright (c) 2017, United States Government, as represented by the
+# Administrator of the National Aeronautics and Space Administration.
+#
+# All rights reserved.
+#
+# The Astrobee platform is licensed under the Apache License, Version 2.0
+# (the "License"); you may not use this file except in compliance with the
+# License. You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+# License for the specific language governing permissions and limitations
+# under the License.
+
+import datetime
+import glob
+import os
+import subprocess
+
+import pandas as pd
+
+
+# Forward errors so we can recover failures
+# even when running commands through multiprocessing
+# pooling
+def full_traceback(func):
+    import functools
+    import traceback
+
+    @functools.wraps(func)
+    def wrapper(*args, **kwargs):
+        try:
+            return func(*args, **kwargs)
+        except Exception as e:
+            msg = "{}\n\nOriginal {}".format(e, traceback.format_exc())
+            raise type(e)(msg)
+
+    return wrapper
+
+
+def get_files(directory, file_string):
+    return glob.glob(os.path.join(directory, file_string))
+
+
+def get_files_recursive(directory, file_string):
+    subdirectory_csv_files = []
+    _, subdirectories, _ = next(os.walk(directory))
+    for subdirectory in subdirectories:
+        subdirectory_path = os.path.join(directory, subdirectory)
+        for subdirectory_csv_file in get_files(subdirectory_path, file_string):
+            subdirectory_csv_files.append(subdirectory_csv_file)
+    return subdirectory_csv_files
+
+
+def create_directory(directory=None):
+    if directory == None:
+        directory = os.path.join(
+            os.getcwd(), datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S")
+        )
+    if os.path.exists(directory):
+        print((directory + " already exists!"))
+        exit()
+    os.makedirs(directory)
+    return directory
+
+
+def load_dataframe(files):
+    dataframes = [pd.read_csv(file) for file in files]
+    dataframe = pd.concat(dataframes)
+    return dataframe
+
+
+def run_command_and_save_output(command, output_filename, print_command=True):
+    if print_command:
+        print(command)
+    with open(output_filename, "w") as output_file:
+        subprocess.call(command, shell=True, stdout=output_file, stderr=output_file)
+
+
+def basename(filename):
+    return os.path.splitext(os.path.basename(filename))[0]

localization/localization_common/setup.py

@@ -0,0 +1,9 @@
+from distutils.core import setup
+
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    packages=["localization_common"], package_dir={"": "scripts"}
+)
+
+setup(**d)