Release 0.7.6. Code on flight units.

.gitignore

@@ -84,6 +84,7 @@ astrobee/freeflyer/*.bin
 /freeflyer.sublime-workspace
 /description/*.tar.gz
 /description/media
+/localization/vive_localization/src/ukf
 
 # catkin
 /.catkin_tools

CMakeLists.txt

@@ -17,8 +17,8 @@
 
 cmake_minimum_required(VERSION 3.0)
 project(Astrobee)
-set(ASTROBEE_VERSION_MAJOR 0)
-set(ASTROBEE_VERSION_MINOR 5)
+
+set(ASTROBEE_VERSION 0.7.6)
 
 if ( "${CMAKE_VERSION}" VERSION_GREATER 3.0.0 )
   cmake_policy(SET CMP0045 OLD)
@@ -99,6 +99,12 @@ option(ENABLE_PICOFLEXX
 option(ENABLE_GAZEBO
   "Enable support for building the Gazebo simulator"
   ON)
+option(ENABLE_VIVE
+  "Enable support for building the Vive drivers"
+  ON)
+option(ENABLE_VIVE_SOLVER
+  "Enable support for building the Vive offline solver"
+  ON)
 
 # If the user selected static libs .. set BUILD_SHARED_LIBS accordingly
 if(USE_STATIC_LIBS)
@@ -201,10 +207,17 @@ find_package(Alvar REQUIRED)
 find_package(OpenMVG QUIET REQUIRED)
 find_package(JsonCpp REQUIRED)
 find_package(FFMPEG QUIET)    # Optional
-find_package(Deepdive QUIET)  # Optional
 if (ENABLE_PICOFLEXX)
   find_package(royale REQUIRED)
 endif (ENABLE_PICOFLEXX)
+# Find pkg-config supported libraries
+if (ENABLE_VIVE)
+  find_package(USB REQUIRED)
+  find_package(JSONC REQUIRED)
+  find_package(ARGTABLE2 REQUIRED)
+  find_package(ZLIB REQUIRED)
+endif (ENABLE_VIVE)
+
 
 if (NOT USE_CTC)  # pkg-config needs extra work to be used for cross compile
   pkg_check_modules(YAMLCPP yaml-cpp)
@@ -306,8 +319,7 @@ set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin)
 add_custom_command(OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/version.txt
   COMMAND ${CMAKE_COMMAND}
   -D OUTPUT_FILE=${CMAKE_CURRENT_BINARY_DIR}/version.txt
-  -D ASTROBEE_VERSION_MAJOR=${ASTROBEE_VERSION_MAJOR}
-  -D ASTROBEE_VERSION_MINOR=${ASTROBEE_VERSION_MINOR}
+  -D ASTROBEE_VERSION=${ASTROBEE_VERSION}
   -D GIT_MODULE_FILE="${CMAKE_SOURCE_DIR}/cmake/GetGitRevisionDescription.cmake"
   -P ${CMAKE_SOURCE_DIR}/cmake/SetVersion.cmake)
 add_custom_target(version DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/version.txt)

INSTALL.md

@@ -10,6 +10,9 @@ support some of our Gazebo plugins.
 *Note: You will need 4 GBs of RAM to compile the software. If you don't have
 that much RAM available, please use swap space.*
 
+*Note: Please ensure you install Ubuntu 16.04. At this time we do not support
+any other operating system or Ubuntu version.*
+
 *Note: Please ensure you install the 64-bit version of Ubuntu. We do not
 support running Astrobee Robot Software on 32-bit systems.*
 
@@ -58,6 +61,17 @@ Next, install all required dependencies:
     rosdep update
     popd
 
+**Important**: you can safely ignore the following error messages, as they are simply letting you know that certain libraries cannot be found. These libraries are for internal NASA use only, and are not required by public users provided that software is launched with DDS disabled.
+
+    E: Unable to locate package libroyale1
+    E: Unable to locate package rti
+    E: Unable to locate package libmiro0
+    E: Unable to locate package libsoracore1
+    E: Unable to locate package libroyale-dev
+    E: Unable to locate package rti-dev
+    E: Unable to locate package libsoracore-dev
+    E: Unable to locate package libmiro-dev
+
 ### Have System Monitor Ignore DDS ROS Bridge
 Since external users do not have access to our internal dds repository, they will not be running or compiling our DDS ROS Bridge. The system monitor needs to be told that the bridge isn't running so that it doesn't assert bridge heartbeat missed faults. In order to do this, please open $SOURCE_PATH/astrobee/config/robots/sim.config and add "dds_ros_bridge" to the nodes_not_running list the end of the file. If an external user doesn't do this, they will see a "Never received heartbeat from dds_ros_bridge" error which will not cause any issues but can be slightly annoying.
 

NASA_INSTALL.md

@@ -182,10 +182,11 @@ 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.
+rsync to copy the install directory to `~/armhf` on the two processors. It 
+takes the robot name as an argument. Here we use `p4d'.
 
     pushd $SOURCE_PATH
-    ./scripts/install_to_astrobee.sh $INSTALL_PATH
+    ./scripts/install_to_astrobee.sh $INSTALL_PATH p4d
     popd
 
 You are now ready to run the code. This code launches a visualization tool,

RELEASE.md

@@ -1,5 +1,65 @@
 # Astrobee Robot Software v1
 
+## Release 0.7.6
+
+ * Fixes to sparse mapping and localization manager.
+
+## Release 0.7.5
+
+ * Various fixes for flight units.
+ * Improvements to debians.
+
+## Release 0.7.4
+
+ * Fixed compile for vive again.
+
+## Release 0.7.3
+
+ * Fixed compile for vive.
+
+## Release 0.7.2
+
+ * Fix cmake bug.
+
+## Release 0.7.1
+
+ * Minor vive updates
+ * Bumps to submodules.
+
+## Release 0.7.0
+
+ * Infrastructure to build the flight images completed, including HLP
+    (Android) configuration methods not requiring touchscreen access
+  * Inertia and masses reflecting the Flight Units characteristics
+  * New values for flight modes optimized for the new propulsion units
+  * Support for all functional tests (including Speedcam modes)
+  * Support for all mobility tests (including plans)
+  * Improved marker tracking simulation plugin allowing to analyze marker
+    placement
+  * Fixed a jerk motion while docking (real robot) due to camera occlusion
+  * Executive:
+    - Refactored for easier maintenance
+    - Support data to disk configuration
+    - Support telemetry rate command
+    - Support localization mode switch and EKF commands
+    - Manage guest science state
+  * Real time analysis tool (gviz) ported to support DDS telemetry:
+    allows the same capabilities of inspection for flight than on the ground.
+  * New launch file feature to support backlisting / adding extra nodes
+  * Bug fixes
+
+## Release 0.6.1
+
+Added config files for new robots.
+
+## Release 0.6.0
+
+Flight packaging candidate release
+
+  - Indicator LEDs management.
+  - All DDS commands to support remote tests implemented.
+  - Adjustments and tools to support test procedures in place.
+
 ## Release 0.5.0
 
 Build 3 - CERT Release

astrobee/config/commands.config

@@ -29,6 +29,10 @@ commandConfig = {
           name="fault",
           parameters={}
         },
+        {
+          name="initializeBias",
+          parameters={}
+        },
         {
           name="loadNodelet",
           parameters={
@@ -58,10 +62,23 @@ commandConfig = {
           name="reacquirePosition",
           parameters={}
         },
+        {
+          name="resetEkf",
+          parameters={}
+        },
         {
           name="shutdown",
           parameters={}
         },
+        {
+          name="switchLocalization",
+          parameters={
+            {
+              key="mode",
+              type="RAPID_STRING"
+            }
+          }
+        },
         {
           name="unloadNodelet",
           parameters={
@@ -75,6 +92,10 @@ commandConfig = {
             }
           }
         },
+        {
+          name="unterminate",
+          parameters={}
+        },
         {
           name="wake",
           parameters={
@@ -458,6 +479,10 @@ commandConfig = {
               key="mass",
               type="RAPID_FLOAT"
             },
+            {
+              key="centerOfMass",
+              type="RAPID_VEC3d"
+            },
             {
               key="matrix",
               type="RAPID_MAT33f"

astrobee/config/communications/dds_ros_bridge.config

@@ -120,6 +120,10 @@ pub_topic_RGCTRGCT = "-traj"
 use_RGSRGS = true
 pub_topic_RGSRGS = ""
 
+-- ros_inertia_rapid_inertia => RIRI --
+use_RIRI = true
+pub_topic_RIRI = ""
+
 -- ros_log_rapid_log_sample => RLRLS --
 use_RLRLS = true
 pub_topic_RLRLS = ""
@@ -149,8 +153,8 @@ cpu_state_rate = 0
 disk_state_rate = 1
 ekf_state_rate = 0
 gnc_state_rate = 0
-position_rate = 30
 pmc_command_rate = 0
+position_rate = 30
 
 battery_time_round_to_multiple = 10;
 

astrobee/config/dock_markers_world.config

@@ -63,7 +63,8 @@ function marker_corners(m, scale)
   top_left = scale * matrix{0.0, -m['pos'][1], m['pos'][2]}
   top_right = top_left + scale * matrix{0.0, -m['size'], 0.0}
   bottom_left = top_left + scale * matrix{0.0, 0.0, m['size']}
-  return top_left, top_right, bottom_left
+  bottom_right = bottom_left + scale * matrix{0.0, -m['size'], 0.0}
+  return top_left, top_right, bottom_left, bottom_right
 end
 
 -- Units transformation
@@ -84,21 +85,24 @@ end
 markers_world = {}
 for k,v in pairs(markers) do
   marker_in_world = {}
-  top_left, top_right, bottom_left = marker_corners(v, scale_factor)
+  top_left, top_right, bottom_left, bottom_right = marker_corners(v, scale_factor)
   marker_in_world['id'] = v['id']
   -- Edited by Andrew to put dock markers in DOCK frame not WORLD frame, since
   -- the EKF now works in a local frame AND that having the features in the
   -- DOCK frame allows us to use the feature messages for etrinsic calibration.
   top_left_world = top_left
   top_right_world = top_right
   bottom_left_world = bottom_left
+  bottom_right_world = bottom_right
   -- corners results are in Matrix form, transpose them back into a table
   marker_in_world['top_left'] = {
     top_left_world[1][1], top_left_world[2][1], top_left_world[3][1] }
   marker_in_world['top_right'] = {
     top_right_world[1][1], top_right_world[2][1], top_right_world[3][1] }
   marker_in_world['bottom_left'] = {
     bottom_left_world[1][1], bottom_left_world[2][1], bottom_left_world[3][1] }
+  marker_in_world['bottom_right'] = {
+    bottom_right_world[1][1], bottom_right_world[2][1], bottom_right_world[3][1] }
   table.insert(markers_world, marker_in_world)
 end
 

astrobee/config/flight.config

@@ -18,6 +18,7 @@
 require "context"
 
 -- Inertial Parameters
+inertia_name = world_inertia_name;
 inertia_com = world_inertia_com;
 inertia_mass = world_inertia_mass;
 inertia_matrix = world_inertia_matrix;
@@ -27,4 +28,4 @@ initial_flight_mode = world_initial_flight_mode;
 default_flight_mode = world_default_flight_mode;
 
 -- All possible flight modes
-flight_modes =  world_flight_modes;
+flight_modes =  world_flight_modes;

astrobee/config/gnc.config

@@ -111,17 +111,20 @@ tun_cmd_pch_pos_k = 0*2*8;
 tun_cmd_pch_att_k = 0*20*3;
 -- nozzle_areas = matrix{{0.0027, 0.0027, 0.0015, 0.0015, 0.0015, 0.0015, 0.0027, 0.0027, 0.0015, 0.0015, 0.0015, 0.0015}};
 
-tun_cmd_pch_max_area = {0.0027, 0.0027, 0.0015, 0.0015, 0.0015, 0.0015, 0.0027, 
+tun_cmd_pch_max_area = {0.0027, 0.0027, 0.0015, 0.0015, 0.0015, 0.0015, 0.0027,
 						0.0027, 0.0015, 0.0015, 0.0015, 0.0015};
 
 -- Reset Parameters
 tun_cmd_pos_error_reset_thresh = 0.25; -- Magnitude of position error (m) before the command shaper recomputes a new trajectory
-tun_cmd_att_error_reset_thresh = 30 * math.pi / 180;-- Magnitude of attitude error (rad) before the command shaper recomputes a new trajectory
+tun_cmd_att_error_reset_thresh = 30 * math.pi / 180; -- Magnitude of attitude error (rad) before the command shaper recomputes a new trajectory
 
 
 -- Stopped Mode Parameters
 tun_ctl_stopped_pos_thresh = 0.1;
 tun_ctl_stopped_quat_thresh = 10*pi/180;
+tun_ctl_stopping_vel_thresh = .02^2; -- Linear velocity threshold squared below which the vehicle is considered to be stopped, (m/s)^2
+tun_ctl_stopping_omega_thresh = .02^2; -- Angular velocity threshold squared below which the vehicle is considered to be stopped, (rad/s)^2
+
 
 
 -- Command Shaper Gains
@@ -167,7 +170,7 @@ tun_ase_map_error = 0.005
 
 -- Mapped Landmark Navigation
 tun_ase_navcam_inv_focal_length = 1.0 / nav_cam.intrinsic_matrix[1];
-tun_ase_navcam_distortion = nav_cam.distortion_coeff; 
+tun_ase_navcam_distortion = nav_cam.distortion_coeff;
 tun_ase_vis_r_mag = 5;
 tun_ase_min_ml_meas = 3; --Minumum number of valid features to trigger a ML update
 
@@ -195,7 +198,7 @@ tun_ase_state_ic_quat_ISS2B = tun_ini_Q_ISS2B;
 tun_ase_state_ic_omega_B_ISS_B = tun_ini_omega_B_ISS_B;
 tun_ase_state_ic_V_B_ISS_ISS = tun_ini_V_B_ISS_ISS;
 
--- Calculate initial Estimator Position 
+-- Calculate initial Estimator Position
 q1 = tun_ini_Q_ISS2B[1];
 q2 = tun_ini_Q_ISS2B[2];
 q3 = tun_ini_Q_ISS2B[3];
@@ -273,9 +276,13 @@ tun_env_drag_disturb_on = false; -- Air drag
 
 tun_epson_report_truth = false;
 tun_cvs_noise_on = true;
-tun_bpm_noise_on_flag = 1.0;
+tun_bpm_noise_on_flag = 0.0;
+
+-- Sim Thrust Scale Factor, scale factor to apply to simulated thrust
+tun_bpm_PM1_thrust_error_sf  = 1.25;
+tun_bpm_PM2_thrust_error_sf  = 1.25;
 
 -- Non-GNC parameters
 min_of_observations 			= 15;
 bias_required_observations 		= 62 * 5;
-imu_bias_file 					= "imu_bias.config";
+imu_bias_file 					= resolve_resource("imu_bias.config");

astrobee/config/hw/flashlights.config

@@ -19,10 +19,10 @@ require "context"
 
 flashlight_front = {
   device = robot_i2c_bus,
-  address = 0x63
+  address = robot_flashlight_addr_front or 0x60
 };
 
 flashlight_aft = {
   device = robot_i2c_bus,
-  address = 0x60
+  address = robot_flashlight_addr_aft or 0x63
 };