From 1866bc6b4d0c325e5084fa6825c40e0349e1c9b1 Mon Sep 17 00:00:00 2001
From: Marina Moreira <67443181+marinagmoreira@users.noreply.github.com>
Date: Mon, 13 Dec 2021 10:48:15 -0800
Subject: [PATCH] changing picture taking command to new name (#359)

* changing picture taking command to new name

* Changed continuous picture taking command.

* Removed old code.

Co-authored-by: Katie Browne <kathryn.browne@nasa.gov>
---
 simulation/CMakeLists.txt                     |  7 ++
 simulation/package.xml                        |  2 +
 .../gazebo_sensor_plugin_sci_cam.cc           | 67 +++++++++----------
 3 files changed, 42 insertions(+), 34 deletions(-)

diff --git a/simulation/CMakeLists.txt b/simulation/CMakeLists.txt
index 09f919d6d5..4a1f602e7a 100644
--- a/simulation/CMakeLists.txt
+++ b/simulation/CMakeLists.txt
@@ -37,11 +37,16 @@ find_package(catkin2 REQUIRED COMPONENTS
   ff_util
   gnc_autocode
   camera
+  jsonloader
 )
 
 # Find packages
 find_package(gazebo REQUIRED)
 
+# Find jsoncpp
+SET(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/../cmake")
+find_package(JsonCpp REQUIRED)
+
 # Add gazebo to the link directorries
 link_directories(${GAZEBO_LIBRARY_DIRS})
 
@@ -88,6 +93,7 @@ catkin_package(
     ff_util
     gnc_autocode
     camera
+    jsonloader
 )
 
 
@@ -100,6 +106,7 @@ include_directories(
   include
   ${catkin_INCLUDE_DIRS}
   ${GAZEBO_INCLUDE_DIRS}
+  ${JSONCPP_INCLUDE_DIRS}
 )
 
 link_directories(${GAZEBO_LIBRARY_DIRS})
diff --git a/simulation/package.xml b/simulation/package.xml
index f1dbeaa26c..c1e44a8aa5 100644
--- a/simulation/package.xml
+++ b/simulation/package.xml
@@ -29,6 +29,7 @@
   <build_depend>ff_util</build_depend>
   <build_depend>gnc_autocode</build_depend>
   <build_depend>camera</build_depend>
+  <build_depend>jsonloader</build_depend>
   <run_depend>visualization_msgs</run_depend>
   <run_depend>tf2</run_depend>
   <run_depend>tf2_ros</run_depend>
@@ -42,6 +43,7 @@
   <run_depend>ff_util</run_depend>
   <run_depend>gnc_autocode</run_depend>
   <run_depend>camera</run_depend>
+  <run_depend>jsonloader</run_depend>
   <export>
     <gazebo_ros plugin_path="${prefix}/lib"/>
     <gazebo_ros gazebo_model_path="${prefix}/../description/media"/>
diff --git a/simulation/src/gazebo_sensor_plugin_sci_cam/gazebo_sensor_plugin_sci_cam.cc b/simulation/src/gazebo_sensor_plugin_sci_cam/gazebo_sensor_plugin_sci_cam.cc
index f6e13492a4..334ba4653d 100644
--- a/simulation/src/gazebo_sensor_plugin_sci_cam/gazebo_sensor_plugin_sci_cam.cc
+++ b/simulation/src/gazebo_sensor_plugin_sci_cam/gazebo_sensor_plugin_sci_cam.cc
@@ -22,6 +22,11 @@
 // FSW includes
 #include <config_reader/config_reader.h>
 
+// JSON includes
+#include <json/json.h>
+#include <json/value.h>
+#include <jsonloader/planio.h>
+
 // Sensor plugin interface
 #include <astrobee_gazebo/astrobee_gazebo.h>
 
@@ -156,30 +161,6 @@ class GazeboSensorPluginSciCam : public FreeFlyerSensorPlugin {
     }
   }
 
-  // Out of string: "{"name": "turnOnContinuousPictureTaking"}"
-  // collect the part in the second set of quotes.
-  // Some honest json parsing could be used here.
-  std::string parseJsonStr(std::string const& json_str) {
-    size_t start = 0;
-    size_t colon_pos = json_str.find(":", start);
-    if (colon_pos == std::string::npos) {
-      return "";
-    }
-    size_t quote1_pos = json_str.find("\"", colon_pos + 1);
-    if (quote1_pos == std::string::npos) {
-      return "";
-    }
-    size_t quote2_pos = json_str.find("\"", quote1_pos + 1);
-
-    if (quote2_pos == std::string::npos) {
-      return "";
-    }
-
-    std::string parsed = json_str.substr(quote1_pos + 1, quote2_pos - quote1_pos - 1);
-
-    return parsed;
-  }
-
   // Called when a dds command is received. Process only guest science
   // sci cam control commands.
   void CmdCallback(ff_msgs::CommandStamped const& cmd) {
@@ -199,7 +180,7 @@ class GazeboSensorPluginSciCam : public FreeFlyerSensorPlugin {
       return;
     }
 
-    std::string app_name   = cmd.args[0].s.data();
+    std::string app_name = cmd.args[0].s.data();
 
     // Process only sci cam commands
     if (app_name != "gov.nasa.arc.irg.astrobee.sci_cam_image") {
@@ -209,22 +190,40 @@ class GazeboSensorPluginSciCam : public FreeFlyerSensorPlugin {
     std::string json_str = cmd.args[1].s.data();
     ROS_INFO_STREAM("Received command: " << json_str);
 
-    std::string action = parseJsonStr(json_str);
-    if (action == "")
+    // Convert string into a json object
+    Json::Value cmd_obj;
+    if (!jsonloader::LoadData(json_str, &cmd_obj)) {
+      ROS_ERROR_STREAM("Unable to convert command " << json_str << " to json.");
+      return;
+    }
+
+    // Check to make sure command name exists
+    if (!cmd_obj.isMember("name") || !cmd_obj["name"].isString()) {
+      ROS_ERROR_STREAM(json_str << " doesn't contain name for the command.");
       return;
+    }
+
+    std::string action = cmd_obj["name"].asString();
 
     // Record the desired intention. Use a lock.
     {
       const std::lock_guard<std::mutex> lock(sci_cam_image_lock);
-      if (action == "takeSinglePicture") {
+      if (action == "takePicture") {
         takeSinglePicture_ = true;
         continuousPictureTaking_ = false;
-      } else if (action == "turnOnContinuousPictureTaking") {
-        takeSinglePicture_ = false;
-        continuousPictureTaking_ = true;
-      } else if (action == "turnOffContinuousPictureTaking") {
-        takeSinglePicture_ = false;
-        continuousPictureTaking_ = false;
+      } else if (action == "setContinuousPictureTaking") {
+        if (cmd_obj.isMember("continuous") && cmd_obj["continuous"].isBool()) {
+          if (cmd_obj["continuous"].asBool()) {
+            takeSinglePicture_ = false;
+            continuousPictureTaking_ = true;
+          } else {
+            takeSinglePicture_ = false;
+            continuousPictureTaking_ = false;
+          }
+        } else {
+          ROS_ERROR_STREAM("Got set continuous picture taking command but it" <<
+                           " didn't contain the continuous argument.");
+        }
       } else {
         ROS_FATAL_STREAM("Unknown sci_cam command: " << action);
       }