Save/restore mowing progress across service/container restarts (#62)

* Save/restore state between restarts

* Dock when giving up on first point

If stuck then skipping the path will just proceed to the end and dock,
but lose progress, so just dock when first point fails

* Fix trimming logic

src/mower_logic/CMakeLists.txt

@@ -14,17 +14,20 @@ find_package(catkin REQUIRED COMPONENTS
   mower_map
   mower_msgs
   nav_msgs
+  rosbag
   roscpp
   slic3r_coverage_planner
   tf2
   tf2_geometry_msgs
   tf2_ros
-        sensor_msgs
-        robot_localization
-        xbot_msgs
-        xbot_positioning
+  sensor_msgs
+  robot_localization
+  xbot_msgs
+  xbot_positioning
 )
 
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(CRYPTOPP REQUIRED libcrypto++)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -60,11 +63,10 @@ find_package(catkin REQUIRED COMPONENTS
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
+add_message_files(
+  FILES
+  CheckPoint.msg
+)
 
 ## Generate services in the 'srv' folder
 # add_service_files(
@@ -81,10 +83,10 @@ find_package(catkin REQUIRED COMPONENTS
 # )
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
+generate_messages(
 #   DEPENDENCIES
 #   mbf_msgs#   mower_msgs#   nav_msgs#   tf2_geometry_msgs
-# )
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
@@ -131,6 +133,7 @@ catkin_package(
 include_directories(
 # include
   ${catkin_INCLUDE_DIRS}
+  ${CRYPTOPP_INCLUDE_DIRS}
 )
 
 ## Declare a C++ library
@@ -180,7 +183,7 @@ add_executable(mower_logic
         src/mower_logic/behaviors/AreaRecordingBehavior.cpp
         )
 add_dependencies(mower_logic ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS})
-target_link_libraries(mower_logic ${catkin_LIBRARIES})
+target_link_libraries(mower_logic ${catkin_LIBRARIES} ${CRYPTOPP_LIBRARIES})
 
 add_executable(mower_odometry src/mower_odometry/mower_odometry.cpp)
 add_dependencies(mower_odometry ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS})

src/mower_logic/cfg/MowerLogic.cfg

@@ -19,7 +19,6 @@ gen.add("mow_angle_increment", double_t, 0, "Mowing angle automatic increment. W
 gen.add("tool_width", double_t, 0, "Width of the mower", 0.14, 0.1, 2)
 gen.add("enable_mower", bool_t, 0, "True to enable mow motor", False)
 gen.add("manual_pause_mowing", bool_t, 0, "True to disable mowing automatically", False)
-gen.add("current_area", int_t, 0, "Current Mow Area", 0, 0, 99)
 gen.add("battery_empty_voltage", double_t, 0, "Voltage to return to docking station", 25.0, 20.0, 32.0)
 gen.add("battery_full_voltage", double_t, 0, "Voltage to start mowing again", 29.0, 20.0, 32.0)
 gen.add("motor_hot_temperature", double_t, 0, "Motor temperature to pause mowing", 70.0, 20.0, 150.0)

src/mower_logic/msg/CheckPoint.msg

@@ -0,0 +1,5 @@
+int32 currentMowingPath
+int32 currentMowingArea
+int32 currentMowingPathIndex
+string currentMowingPlanDigest
+float32 currentMowingAngleIncrementSum

src/mower_logic/package.xml

@@ -24,6 +24,7 @@
   <build_depend>visualization_msgs</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>xbot_msgs</build_depend>
+  <build_depend>crypto++</build_depend>
   <depend>xbot_positioning</depend>
   <build_export_depend>dynamic_reconfigure</build_export_depend>
   <build_export_depend>ftc_local_planner</build_export_depend>