Release 0.14.0

* Added graph_localizer package
* Added supporting packages for graph localizer including imu_augmetor, imu_integation, localization_common, localization_measurements
* Added tools for graph localizer including graph_bag and imu_bias_tester
* Added rviz plugins for graph localizer and associated packages in localization_rviz_plugins
* Added gtsam debian
Please Note: Perching does not work in this release, please use 0.13.0 for this.

CMakeLists.txt

@@ -18,7 +18,7 @@
 cmake_minimum_required(VERSION 3.0)
 project(Astrobee)
 
-set(ASTROBEE_VERSION 0.13.0)
+set(ASTROBEE_VERSION 0.14.0)
 
 # Define our options
 option(USE_CCACHE
@@ -69,6 +69,10 @@ option(ENABLE_VIVE_SOLVER
 option(ENABLE_INTEGRATION_TESTING
   "Build the integration tests if tests are active."
   ON)
+option(BUILD_LOC_RVIZ_PLUGINS
+  "Build the localization rviz plugins."
+  ON)
+
 
 if ( "${CMAKE_VERSION}" VERSION_GREATER 3.0.0 )
   cmake_policy(SET CMP0045 OLD)
@@ -212,7 +216,7 @@ endif (NOT PROTOBUF_PROTOC_EXECUTABLE)
 
 find_package(Ceres REQUIRED)
 
-find_package(Boost 1.54.0 QUIET REQUIRED COMPONENTS filesystem system iostreams thread program_options)
+find_package(Boost 1.54.0 QUIET REQUIRED COMPONENTS filesystem system iostreams thread program_options timer)
 find_package(Eigen3 REQUIRED)
 find_package(ZLIB)
 find_package(Luajit20 REQUIRED)
@@ -239,6 +243,7 @@ set(OpenCV_LIBRARIES ${OpenCV_LIBS})
 
 find_package(dbow2 REQUIRED)
 find_package(Alvar REQUIRED)
+find_package(GTSAM REQUIRED)
 find_package(OpenMVG QUIET REQUIRED)
 find_package(JsonCpp REQUIRED)
 find_package(FFMPEG QUIET)    # Optional
@@ -261,6 +266,15 @@ if (NOT USE_CTC)  # pkg-config needs extra work to be used for cross compile
   endif (YAMLCPP_FOUND)
 endif (NOT USE_CTC)
 
+if(ENABLE_GOOGLE_PROF)
+  SET(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,--no-as-needed -lprofiler -Wl,--as-needed")
+  SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DDEBUG")
+endif()
+
+if(USE_CTC) 
+  set(BUILD_LOC_RVIZ_PLUGINS OFF)
+endif()
+
 set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Wno-unused-local-typedefs -Wno-packed-bitfield-compat -std=c++11")
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wno-unused-local-typedefs -Wno-packed-bitfield-compat -std=c++11")
 set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--no-undefined")
@@ -294,7 +308,8 @@ if (USE_CTC)
 endif (USE_CTC)
 if (USE_ROS)
   set(ENV{ROS_LANG_DISABLE} "genlisp:gennodejs:geneus:$ENV{ROS_LANG_DISABLE}")
-  find_package(catkin2 COMPONENTS roscpp message_generation std_msgs geometry_msgs sensor_msgs cv_bridge image_transport tf tf2 tf2_ros rosbag nodelet)
+  find_package(catkin2 COMPONENTS roscpp message_generation std_msgs geometry_msgs sensor_msgs cv_bridge image_transport tf tf2 tf2_ros rosbag nodelet rviz)
+  find_package(Qt5 ${rviz_QT_VERSION} REQUIRED Core Widgets)
 
   if (IS_BAMBOO_BUILD)
     include(BambooFix)

RELEASE.md

@@ -1,5 +1,14 @@
 # Releases
 
+## Release 0.14.0
+
+  * Added graph_localizer package
+  * Added supporting packages for graph localizer including imu_augmetor, imu_integation, localization_common, localization_measurements
+  * Added tools for graph localizer including graph_bag and imu_bias_tester
+  * Added rviz plugins for graph localizer and associated packages in localization_rviz_plugins
+  * Added gtsam debian
+  Note: Perching does not work in this release, use another version for this.
+
 ## Release 0.13.0
 
   * Multiple updates to documentation, doxygen is exported to github pages

astrobee.doxyfile

@@ -39,7 +39,7 @@ PROJECT_NAME           = "NASA Astrobee Robot Software"
 # control system is used.
 
 
-PROJECT_NUMBER         = 0.13.0
+PROJECT_NUMBER         = 0.14.0
 
 # 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/commands/freeFlyerPlanSchema.json

@@ -112,7 +112,8 @@
         ["EkfState", "EkfState"],
         ["GncState", "GncState"],
         ["PmcCmdState", "PmcCmdState"],
-        ["Position", "Position"]
+        ["Position", "Position"],
+        ["SparseMappingPose", "SparseMappingPose"]
       ]
     },
 

astrobee/config/communications/dds_ros_bridge.config

@@ -148,6 +148,16 @@ pub_topic_RLRLS = ""
 use_RORP = true
 pub_topic_RORP = ""
 
+-- ros_sparse_mapping_pose_rapid_position => RSMPRP --
+use_RSMPRP = true
+pub_topic_RSMPRP = "-sparse"
+use_rate_RSMPRP = true
+
+-- ros_vive_pose_rapid_position => RVPRP --
+use_RVPRP = false 
+pub_topic_RVPRP = "-vive"
+use_rate_RVPRP = false
+
 -- ros_payload_power_state_rapid_payload_power_state =>  RPPSRPPS --
 use_RPPSRPPS = true
 pub_topic_RPPSRPPS = ""
@@ -175,6 +185,7 @@ ekf_state_rate = 0
 gnc_state_rate = 0
 pmc_command_rate = 0
 position_rate = 30
+sparse_mapping_pose_rate = 0
 
 battery_time_round_to_multiple = 10;
 

astrobee/config/faults.config

@@ -18,59 +18,59 @@
 -- Autogenerated from FMECA! DO NOT CHANGE!
 
 arm = {
-  {id=61, key="INITIALIZATION_FAILED", description="Arm Initialization Failed"},
+  {id=63, key="INITIALIZATION_FAILED", description="Arm Initialization Failed"},
 }
 
 all = {
-  {id=85, key="UNKNOWN_FAULT_KEY", description="Unknown Fault Key"},
+  {id=87, key="UNKNOWN_FAULT_KEY", description="Unknown Fault Key"},
 }
 
 access_control = {
-  {id=70, key="INITIALIZATION_FAILED", description="Access Control Initialization Failed"},
+  {id=72, key="INITIALIZATION_FAILED", description="Access Control Initialization Failed"},
 }
 
 executive = {
-  {id=68, key="INITIALIZATION_FAILED", description="Executive Initialization Failed"},
+  {id=70, key="INITIALIZATION_FAILED", description="Executive Initialization Failed"},
 }
 
 data_bagger = {
-  {id=83, key="INITIALIZATION_FAILED", description="Data Bagger Initialization Failed"},
+  {id=85, key="INITIALIZATION_FAILED", description="Data Bagger Initialization Failed"},
 }
 
 llp_cpu_mem_monitor = {
-  {id=72, key="INITIALIZATION_FAILED", description="LLP CPU Monitor Initialization Failed"},
-  {id=86, key="LOAD_TOO_HIGH", description="LLP CPU load too high"},
-  {id=97, key="MEMORY_USAGE_TOO_HIGH", description="LLP Memory usage too high"},
-  {id=100, key="TEMPERATURE_TOO_HIGH", description="LLP Temperature too high"},
+  {id=74, key="INITIALIZATION_FAILED", description="LLP CPU Monitor Initialization Failed"},
+  {id=88, key="LOAD_TOO_HIGH", description="LLP CPU load too high"},
+  {id=99, key="MEMORY_USAGE_TOO_HIGH", description="LLP Memory usage too high"},
+  {id=102, key="TEMPERATURE_TOO_HIGH", description="LLP Temperature too high"},
 }
 
 llp_disk_monitor = {
-  {id=75, key="INITIALIZATION_FAILED", description="LLP Disk Monitor Initialization Failed"},
-  {id=89, key="DISK_CONFIG_INVALID", description="LLP Disk conifg not valid"},
-  {id=91, key="DISK_USAGE_HIGH", description="LLP Disk usage getting high"},
-  {id=94, key="DISK_USAGE_TOO_HIGH", description="LLP Disk usage too high"},
+  {id=77, key="INITIALIZATION_FAILED", description="LLP Disk Monitor Initialization Failed"},
+  {id=91, key="DISK_CONFIG_INVALID", description="LLP Disk conifg not valid"},
+  {id=93, key="DISK_USAGE_HIGH", description="LLP Disk usage getting high"},
+  {id=96, key="DISK_USAGE_TOO_HIGH", description="LLP Disk usage too high"},
 }
 
 image_sampler = {
-  {id=81, key="INITIALIZATION_FAILED", description="Image Sampler Initialization Failed"},
+  {id=83, key="INITIALIZATION_FAILED", description="Image Sampler Initialization Failed"},
 }
 
 mlp_disk_monitor = {
-  {id=76, key="INITIALIZATION_FAILED", description="MLP Disk Monitor Initialization Failed"},
-  {id=90, key="DISK_CONFIG_INVALID", description="MLP Disk config not valid"},
-  {id=92, key="DISK_USAGE_HIGH", description="MLP Disk usage getting high"},
-  {id=95, key="DISK_USAGE_TOO_HIGH", description="MLP Disk usage too high"},
+  {id=78, key="INITIALIZATION_FAILED", description="MLP Disk Monitor Initialization Failed"},
+  {id=92, key="DISK_CONFIG_INVALID", description="MLP Disk config not valid"},
+  {id=94, key="DISK_USAGE_HIGH", description="MLP Disk usage getting high"},
+  {id=97, key="DISK_USAGE_TOO_HIGH", description="MLP Disk usage too high"},
 }
 
 mlp_cpu_mem_monitor = {
-  {id=73, key="INITIALIZATION_FAILED", description="MLP CPU Monitor Initialization Failed"},
-  {id=87, key="LOAD_TOO_HIGH", description="MLP CPU load too high"},
-  {id=98, key="MEMORY_USAGE_TOO_HIGH", description="MLP Memory usage too high"},
-  {id=101, key="TEMPERATURE_TOO_HIGH", description="MLP Temperature too high"},
+  {id=75, key="INITIALIZATION_FAILED", description="MLP CPU Monitor Initialization Failed"},
+  {id=89, key="LOAD_TOO_HIGH", description="MLP CPU load too high"},
+  {id=100, key="MEMORY_USAGE_TOO_HIGH", description="MLP Memory usage too high"},
+  {id=103, key="TEMPERATURE_TOO_HIGH", description="MLP Temperature too high"},
 }
 
 sys_monitor = {
-  {id=105, key="TIME_DIFF_TOO_HIGH", description="Time difference between mlp and llp too high"},
+  {id=107, key="TIME_DIFF_TOO_HIGH", description="Time difference between mlp and llp too high"},
 }
 
 optical_flow_nodelet = {
@@ -86,29 +86,29 @@ handrail_detect = {
 }
 
 localization_manager = {
-  {id=62, key="INITIALIZATION_FAILED", description="Localization Manager Initialization Failed"},
-  {id=117, key="LOCALIZATION_PIPELINE_UNSTABLE", description="Localization pipeline is not producing registration or features."},
-  {id=119, key="ESTIMATE_CONFIDENCE_TOO_LOW", description="Estimator confidence too low for too long"},
+  {id=64, key="INITIALIZATION_FAILED", description="Localization Manager Initialization Failed"},
+  {id=119, key="LOCALIZATION_PIPELINE_UNSTABLE", description="Localization pipeline is not producing registration or features."},
+  {id=121, key="ESTIMATE_CONFIDENCE_TOO_LOW", description="Estimator confidence too low for too long"},
 }
 
 dds_ros_bridge = {
-  {id=71, key="INITIALIZATION_FAILED", description="DDS ROS Bridge Initialization Failed"},
+  {id=73, key="INITIALIZATION_FAILED", description="DDS ROS Bridge Initialization Failed"},
 }
 
 mapper = {
-  {id=80, key="INITIALIZATION_FAILED", description="Mapper Initialization Failed"},
+  {id=82, key="INITIALIZATION_FAILED", description="Mapper Initialization Failed"},
 }
 
 planner_qp = {
-  {id=78, key="INITIALIZATION_FAILED", description="QP Planner Initialization Failed"},
+  {id=80, key="INITIALIZATION_FAILED", description="QP Planner Initialization Failed"},
 }
 
 planner_trapezoidal = {
-  {id=79, key="INITIALIZATION_FAILED", description="Trapezoidal Planner Initialization Failed"},
+  {id=81, key="INITIALIZATION_FAILED", description="Trapezoidal Planner Initialization Failed"},
 }
 
 choreographer = {
-  {id=67, key="INITIALIZATION_FAILED", description="Choreographer Initialization Failed"},
+  {id=69, key="INITIALIZATION_FAILED", description="Choreographer Initialization Failed"},
 }
 
 laser = {
@@ -118,11 +118,11 @@ eps_driver = {
 }
 
 perching_arm = {
-  {id=60, key="INITIALIZATION_FAILED", description="Perching Arm Initialization Failed"},
+  {id=62, key="INITIALIZATION_FAILED", description="Perching Arm Initialization Failed"},
 }
 
 framestore = {
-  {id=82, key="INITIALIZATION_FAILED", description="Frame Store Initialization Failed"},
+  {id=84, key="INITIALIZATION_FAILED", description="Frame Store Initialization Failed"},
 }
 
 pmc_actuator = {
@@ -138,45 +138,48 @@ flashlight_aft = {
 }
 
 pico_driver = {
-  {id=59, key="INITIALIZATION_FAILED", description="PicoFlexx Initialization Failed"},
-  {id=124, key="NO_COMMUNICATION_HAZCAM", description="Unable to communicate with the Hazcam"},
+  {id=61, key="INITIALIZATION_FAILED", description="PicoFlexx Initialization Failed"},
+  {id=126, key="NO_COMMUNICATION_HAZCAM", description="Unable to communicate with the Hazcam"},
 }
 
 dock_cam = {
 }
 
 ctl = {
-  {id=53, key="INITIALIZATION_FAILED", description="Control Initialization Failed"},
-  {id=133, key="KINETIC_ENERGY_INCREASING", description="Kinetic energy increasing in stopping mode"},
+  {id=54, key="INITIALIZATION_FAILED", description="Control Initialization Failed"},
+  {id=135, key="KINETIC_ENERGY_INCREASING", description="Kinetic energy increasing in stopping mode"},
 }
 
-ekf = {
-  {id=54, key="INITIALIZATION_FAILED", description="EKF Initialization Failed"},
-  {id=134, key="CONTROL_CYCLE_OVERRUN", description="Control Cycle Overrun"},
-  {id=136, key="POSE_ESTIMATE_NON_PHYSICAL", description="Pose estimate non-physical"},
+imu_aug = {
+  {id=56, key="INITIALIZATION_FAILED", description="IMU Augmentor Initialization Failed"},
+}
+
+graph_loc = {
+  {id=55, key="INITIALIZATION_FAILED", description="Graph Localizer Initialization Failed"},
+  {id=137, key="POSE_ESTIMATE_NON_PHYSICAL", description="Pose estimate non-physical"},
 }
 
 fam = {
-  {id=55, key="INITIALIZATION_FAILED", description="FAM Initialization Failed"},
+  {id=57, key="INITIALIZATION_FAILED", description="FAM Initialization Failed"},
 }
 
 speed_cam = {
   {id=10, key="VELOCITY_TOO_HIGH", description="Velocity too high"},
-  {id=56, key="INITIALIZATION_FAILED", description="Speedcam Initialization Failed"},
-  {id=126, key="NO_COMMUNICATION_SPEEDCAM", description="Unable to communicate with the Speedcam"},
+  {id=58, key="INITIALIZATION_FAILED", description="Speedcam Initialization Failed"},
+  {id=128, key="NO_COMMUNICATION_SPEEDCAM", description="Unable to communicate with the Speedcam"},
 }
 
 epson_imu = {
-  {id=112, key="IMU_ACCEL_ABOVE_LIMITS", description="IMU acceleration above limits"},
-  {id=145, key="BAD_GYRO_DATA", description="IMU returns bad gyro data"},
-  {id=146, key="IMU_OVERTEMP", description="IMU overtemp"},
-  {id=147, key="SELF_TEST_FAULT_CODE", description="IMU Fault code - Self test"},
-  {id=148, key="HARDWARE_ERROR_FAULT_CODE", description="IMU Fault code - Hardware error"},
-  {id=150, key="BAD_ACCEL_DATA", description="IMU returns bad accel data"},
-  {id=151, key="NO_IMU_DATA_PACKETS", description="Loss of data packets from IMU"},
+  {id=114, key="IMU_ACCEL_ABOVE_LIMITS", description="IMU acceleration above limits"},
+  {id=146, key="BAD_GYRO_DATA", description="IMU returns bad gyro data"},
+  {id=147, key="IMU_OVERTEMP", description="IMU overtemp"},
+  {id=148, key="SELF_TEST_FAULT_CODE", description="IMU Fault code - Self test"},
+  {id=149, key="HARDWARE_ERROR_FAULT_CODE", description="IMU Fault code - Hardware error"},
+  {id=151, key="BAD_ACCEL_DATA", description="IMU returns bad accel data"},
+  {id=152, key="NO_IMU_DATA_PACKETS", description="Loss of data packets from IMU"},
 }
 
 dock = {
-  {id=84, key="INITIALIZATION_FAILED", description="Docker Initialization Failed"},
+  {id=86, key="INITIALIZATION_FAILED", description="Docker Initialization Failed"},
 }