Saves and reads the total mileage from the datacentre.

odometry_mileage/CMakeLists.txt

@@ -9,6 +9,7 @@ find_package(catkin REQUIRED COMPONENTS
   std_msgs
   nav_msgs
   geometry_msgs
+  strands_datacentre
 )
 
 ## System dependencies are found with CMake's conventions
@@ -108,7 +109,7 @@ add_executable(odometry_mileage src/odometry_mileage.cpp)
 
 ## Add cmake target dependencies of the executable/library
 ## as an example, message headers may need to be generated before nodes
-# add_dependencies(odometry_mileage_node odometry_mileage_generate_messages_cpp)
+add_dependencies(odometry_mileage strands_datacentre_generate_messages_cpp)
 
 ## Specify libraries to link a library or executable target against
 target_link_libraries(odometry_mileage

odometry_mileage/README.md

@@ -4,3 +4,12 @@ This very simple node subscribes to `/odom` and calculates the travelled distanc
 The result is publish on `/odom_mileage`.
 
 Start with `rosrun odomoetry_mileage odometry_mileage`
+
+If the parametere mileage is present on the rosparam server (e.g. via the datacentre), then the node will use the stored mileage to intialise itself.
+While the node is running the mileage parameter is constantly set to the current value and saved to the datacentre. The default save interval
+is every 500 odometry messages, which is every ~10 seconds. This can be change via the `save_interval` parametre.
+
+To initialise your node with the current mileage on the robot do: *(Only do this before you launche the node for the first time!)*
+* `rostopic echo -n 1 /mileage` and copy that value
+* `rosparam set mileage <your mileage value>`
+* `rosservice call /config_manager/save_param mileage`

odometry_mileage/package.xml

@@ -42,10 +42,16 @@
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
   <build_depend>tf</build_depend>
+  <build_depend>strands_datacentre</build_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>std_msgs</run_depend>
+  <run_depend>nav_msgs</run_depend>
+  <run_depend>geometry_msgs</run_depend>
   <run_depend>tf</run_depend>
+  <run_depend>strands_datacentre</run_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->
@@ -56,4 +62,4 @@
     <!-- Other tools can request additional information be placed here -->
 
   </export>
-</package>
+</package>

odometry_mileage/src/odometry_mileage.cpp

@@ -3,14 +3,19 @@
 #include <std_msgs/Float64.h>
 #include <nav_msgs/Odometry.h>
 #include <geometry_msgs/Point.h>
+#include <strands_datacentre/SetParam.h>
 
 #include <math.h>
 
 using namespace std;
+int save;
 
 ros::Publisher mileage_pub;
 std_msgs::Float64 total_distance;
 geometry_msgs::Point last_point;
+ros::ServiceClient client;
+strands_datacentre::SetParam srv;
+int save_interval;
 
 void callback(const nav_msgs::Odometry::ConstPtr &odom)
 {
@@ -26,6 +31,16 @@ void callback(const nav_msgs::Odometry::ConstPtr &odom)
 
     last_point = odom->pose.pose.position;
 
+    if(save % save_interval == 0) {
+        ros::param::set("/mileage",total_distance.data);
+        srv.request.param = "mileage";
+        if (client.call(srv))
+            ROS_DEBUG("Save mileage: success");
+        else
+            ROS_WARN("Save mileage: failed");
+        save = 0;
+    }
+    save++;
 }
 
 int main(int argc, char **argv)
@@ -38,7 +53,7 @@ int main(int argc, char **argv)
     last_point.y = 0;
     last_point.z = 0;
 
-    total_distance.data = 0;
+    save = 1;
 
     // Declare variables that can be modified by launch file or command line.
     string mileage_topic;
@@ -50,8 +65,13 @@ int main(int argc, char **argv)
     ros::NodeHandle private_node_handle_("~");
     private_node_handle_.param("mileage_topic", mileage_topic, string("/odom_mileage"));
     private_node_handle_.param("odom_topic", odom_topic, string("/odom"));
+    private_node_handle_.param("save_interval", save_interval, 500);
+    n.param("/mileage", total_distance.data, 0.0);
+
+    client = n.serviceClient<strands_datacentre::SetParam>("/config_manager/save_param");
 
-    ros::Subscriber odom_sub = n.subscribe(odom_topic.c_str(), 10, &callback);
+    //Create a subscriber
+    ros::Subscriber odom_sub = n.subscribe(odom_topic.c_str(), 50, &callback);
 
     // Create a publisher
     mileage_pub = n.advertise<std_msgs::Float64>(mileage_topic.c_str(), 10);