From c70414f10ccfb16e09be44571272bfe878d239ba Mon Sep 17 00:00:00 2001
From: Aaron Miller <acmiller273@gmail.com>
Date: Sun, 4 Jun 2017 17:46:43 +0000
Subject: [PATCH] Added option to turn battery readings off

---
 CMakeLists.txt                           |  1 +
 dependencies.rosinstall                  |  3 +++
 include/iarc7_fc_comms/CommonFcComms.hpp | 30 +++++++++++++++---------
 launch/fc_comms_msp.launch               |  5 +++-
 package.xml                              |  4 +++-
 params/fc_comms.yaml                     |  1 +
 6 files changed, 31 insertions(+), 13 deletions(-)
 create mode 100644 params/fc_comms.yaml

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0525925..1924a14 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -11,6 +11,7 @@ find_package(catkin REQUIRED COMPONENTS
   std_msgs
   std_srvs
   roscpp
+  ros_utils
   message_generation
   serial
   tf2
diff --git a/dependencies.rosinstall b/dependencies.rosinstall
index e9a8038..340a290 100644
--- a/dependencies.rosinstall
+++ b/dependencies.rosinstall
@@ -7,3 +7,6 @@
 - git:
     local-name: iarc7_safety
     uri: https://github.com/Pitt-RAS/iarc7_safety.git
+- git:
+    local-name: ros_utils
+    uri: https://github.com/Pitt-RAS/ros_utils.git
diff --git a/include/iarc7_fc_comms/CommonFcComms.hpp b/include/iarc7_fc_comms/CommonFcComms.hpp
index 63892ca..3c9e10b 100644
--- a/include/iarc7_fc_comms/CommonFcComms.hpp
+++ b/include/iarc7_fc_comms/CommonFcComms.hpp
@@ -12,16 +12,20 @@
 #include <ros/ros.h>
 #include <tf2/LinearMath/Quaternion.h>
 #include <tf2_ros/transform_broadcaster.h>
-#include <geometry_msgs/TransformStamped.h>
-#include <sensor_msgs/Imu.h>
-#include "CommonConf.hpp"
+#include <vector>
+
 #include "iarc7_safety/SafetyClient.hpp"
 #include "iarc7_msgs/BoolStamped.h"
 #include "iarc7_msgs/FlightControllerStatus.h"
 #include "iarc7_msgs/Float64Stamped.h"
 #include "iarc7_msgs/OrientationThrottleStamped.h"
+#include <ros_utils/ParamUtils.hpp>
+
+#include <geometry_msgs/TransformStamped.h>
+#include <sensor_msgs/Imu.h>
 #include "std_srvs/SetBool.h"
 
+#include "CommonConf.hpp"
 
 //mspfccomms actually handles the serial communication
 namespace FcComms{
@@ -98,6 +102,9 @@ namespace FcComms{
         // This node's handle
         ros::NodeHandle nh_;
 
+        // NodeHandle in this node's namespace
+        ros::NodeHandle private_nh_;
+
         // Safety client
         Iarc7Safety::SafetyClient safety_client_;
 
@@ -127,12 +134,10 @@ namespace FcComms{
 
         typedef void (CommonFcComms::*CommonFcCommsMemFn)();
 
-        CommonFcCommsMemFn sequenced_updates[3] = {&CommonFcComms::updateArmed,
-                                                   &CommonFcComms::updateAutoPilotEnabled,
-                                                   &CommonFcComms::updateBattery
-                                                  };
-
-        uint32_t num_sequenced_updates = sizeof(sequenced_updates) / sizeof(CommonFcCommsMemFn);
+        std::vector<CommonFcCommsMemFn> sequenced_updates = {
+                &CommonFcComms::updateArmed,
+                &CommonFcComms::updateAutoPilotEnabled
+            };
 
         uint32_t current_sequenced_update = 0;
 
@@ -149,6 +154,7 @@ using namespace FcComms;
 template<class T>
 CommonFcComms<T>::CommonFcComms() :
 nh_(),
+private_nh_("~"),
 safety_client_(nh_, "fc_comms_msp"),
 battery_publisher(),
 status_publisher(),
@@ -160,7 +166,9 @@ uav_throttle_subscriber(),
 uav_arm_service(),
 last_direction_command_message_ptr_()
 {
-
+    if (ros_utils::ParamUtils::getParam<bool>(private_nh_, "publish_fc_battery")) {
+        sequenced_updates.push_back(&CommonFcComms::updateBattery);
+    }
 }
 
 template<class T>
@@ -478,7 +486,7 @@ void CommonFcComms<T>::update()
             }
 
             (this->*sequenced_updates[current_sequenced_update])();
-            current_sequenced_update = (current_sequenced_update + 1) % num_sequenced_updates;
+            current_sequenced_update = (current_sequenced_update + 1) % sequenced_updates.size();
 
             publishFcStatus();
 
diff --git a/launch/fc_comms_msp.launch b/launch/fc_comms_msp.launch
index 52f3c55..0c9b1ec 100644
--- a/launch/fc_comms_msp.launch
+++ b/launch/fc_comms_msp.launch
@@ -1,7 +1,10 @@
 <launch>
     <arg name="bond_id_namespace" default="safety_bonds" />
 
-    <node name="fc_comms_msp" pkg="iarc7_fc_comms" type="fc_comms_msp"/>
+    <node name="fc_comms_msp" pkg="iarc7_fc_comms" type="fc_comms_msp">
+        <rosparam command="load"
+            file="$(find iarc7_fc_comms)/params/fc_comms.yaml" />
+    </node>
     <param name="$(arg bond_id_namespace)/fc_comms_msp/form_bond"
         value="true" />
 </launch>
diff --git a/package.xml b/package.xml
index 2d14623..2c23ed1 100644
--- a/package.xml
+++ b/package.xml
@@ -48,6 +48,7 @@
   <build_depend>sensor_msgs</build_depend>
   <build_depend>message_generation</build_depend>
   <build_depend>serial</build_depend>
+  <build_depend>ros_utils</build_depend>
   <build_depend>tf2</build_depend>
   <build_depend>tf2_ros</build_depend>
   <build_depend>iarc7_safety</build_depend>
@@ -56,6 +57,7 @@
   <run_depend>message_runtime</run_depend>
   <run_depend>tf2</run_depend>
   <run_depend>tf2_ros</run_depend>
+  <run_depend>ros_utils</run_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->
@@ -63,4 +65,4 @@
     <!-- Other tools can request additional information be placed here -->
 
   </export>
-</package>
\ No newline at end of file
+</package>
diff --git a/params/fc_comms.yaml b/params/fc_comms.yaml
new file mode 100644
index 0000000..2225458
--- /dev/null
+++ b/params/fc_comms.yaml
@@ -0,0 +1 @@
+publish_fc_battery: false