Merge pull request #145 from nasa/OCEANWATER-700_activate_comms_action

PLEXIL support for ActivateComms action

Author: kmdalal

ow_plexil/src/plans/ActivateComms.plp

@@ -0,0 +1,20 @@
+// The Notices and Disclaimers for Ocean Worlds Autonomy Testbed for Exploration
+// Research and Simulation can be found in README.md in the root directory of
+// this repository.
+
+// Stubbed uplink or downlink that issues the given message an invoke
+// a dummy communications action, which only applies an additional
+// power draw for the given duration.
+
+#include "ow-interface.h"
+
+ActivateComms:
+{
+  In String Message;
+  // NOTE: "Duration" is a PLEXIL keyword and cannot be used here.
+  In Real DurationSecs;
+
+  log_info (Message, "...");
+  SynchronousCommand activate_comms (DurationSecs);
+  log_info ("Uplink/Downlink complete.");
+}

ow_plexil/src/plans/CollectAndTransfer.plp

@@ -11,7 +11,6 @@ Real Lookup CollectAndTransferTimeout;
 LibraryAction ImageSampleSite();
 LibraryAction Stub(In String desc);
 LibraryAction Image(In String desc);
-LibraryAction Downlink();
 LibraryAction GetInfoIfCrash(In String CheckpointName,
                              In Boolean IgnoreCrash,
                              InOut Boolean Crashed,
@@ -100,7 +99,8 @@ CollectAndTransfer:
     DownlinkSampleImage:
     {
       SkipCondition LastComplete >= 4;
-      LibraryCall Downlink();
+      LibraryCall ActivateComms (Message = "Downlinking sample image",
+                                 DurationSecs = 3);
       Wait 1;
       set_checkpoint(OurName,true,Lookup(ToString(4,"_",CollectMore)));
       SynchronousCommand flush_checkpoints();

ow_plexil/src/plans/Downlink.plp

@@ -25,7 +25,6 @@ LibraryAction ImageLandingSite(In String InstanceName,
 LibraryAction InterogateSurface;
 LibraryAction GetTrenchTarget;
 LibraryAction Excavation;
-LibraryAction Downlink;
 LibraryAction Image (In String desc);
 LibraryAction IdentifySampleTarget (InOut Real X,
                                     InOut Real Y,
@@ -212,7 +211,8 @@ EuropaMission: Concurrence
                                   GroundPos = ground_pos,
                                   Parallel = parallel);
       LibraryCall Image(desc="trench");
-      LibraryCall Downlink;
+      LibraryCall ActivateComms (Message = "Downlinking trench image",
+                                 DurationSecs = 3);
 
       LibraryCall CollectAndTransfer(IgnoreCrash = false,
                                      InstanceName="MissionSol1",

ow_plexil/src/plans/DownlinkImage.plp

@@ -7,42 +7,40 @@
 // interpretation of the Europa Lander reference mission, and serves mainly as a
 // PLEXIL example.
 
-Real Lookup time;
+#include "ow-interface.h"
+
+//Real Lookup time;
 Real Lookup TrenchTargetTimeout;
 Boolean Lookup TrenchIdentified;
-LibraryAction DownlinkImage();
 LibraryAction Stub(In String desc);
-Command log_error (...);
 
 GetTrenchTarget:
 {
-  Real Time;
-  Time = Lookup(time);
+  //  Real Time;
 
-  // TODO: invoke comms, reset on failure.
-  LibraryCall DownlinkImage();
+  LibraryCall ActivateComms (Message = "Downlinking workspace images",
+                             DurationSecs = 5);
 
   Try {
     GetInfoFromGround:
     {
       Integer numAttempts = 0;
       RepeatCondition AttemptGround.outcome == FAILURE;
       InvariantCondition numAttempts <= 1;
-      Time = Lookup(time);
+      //      Time = Lookup(time);
       AttemptGround:
       {
-        InvariantCondition
-          Time < (AttemptGround.EXECUTING.START +
-                  Lookup(TrenchTargetTimeout));
+        InvariantCondition (Lookup(time) < (AttemptGround.EXECUTING.START +
+                                            Lookup(TrenchTargetTimeout)));
         EndCondition Lookup(TrenchIdentified);
-        LibraryCall Stub(desc="Getting trench target from ground");
+        LibraryCall ActivateComms (Message = "Uplinking sample target location",
+                                   DurationSecs = 2);
         numAttempts = numAttempts + 1;
       }
     }
     GetTrenchTargetOnboard:
     {
-      LibraryCall Stub(desc="Getting trench target onboard");
+      LibraryCall Stub(desc="Computing sample target location onboard");
     }
-    log_error ("Failed to get trench target!");
   }
 }

ow_plexil/src/plans/EuropaMission.plp

@@ -6,13 +6,12 @@
 
 #include "ow-interface.h"
 
-LibraryAction Downlink();
-
 SampleAnalysis:
 {
   In Integer inst_id;
 
   log_info ("PLEXIL Stub: Analyzing sample using instrument ", inst_id, "...");
 
-  LibraryCall Downlink();
+  LibraryCall ActivateComms (Message = "Downlinking sample analysis",
+                             DurationSecs = 2);
 }

ow_plexil/src/plans/GetTrenchTarget.plp

@@ -13,6 +13,10 @@ TestActions: UncheckedSequence
   Real GP = -0.155;  // Ground position
   log_info ("Beginning action test...");
 
+
+  LibraryCall ActivateComms (Message = "Testing ActivateComms",
+                             DurationSecs = 2);
+
   LibraryCall ArmUnstow();
 
   LibraryCall ArmMoveCartesian (Frame = BASE_FRAME,

ow_plexil/src/plans/SampleAnalysis.plp

@@ -62,6 +62,7 @@ Command camera_set_exposure (Real seconds);
 Command dock_ingest_sample();
 Command light_set_intensity (String side,     // "left" or "right"
 			     Real intensity); // 0.0 to 1.0.  0 is off.
+Command activate_comms(Real duration_secs);
 
 // Simulate fault commands.  See TestSimulatedFaults.plp for example
 // usage. The value of 'probability' must be in the range [0..1].

ow_plexil/src/plans/TestActions.plp

@@ -37,6 +37,8 @@ LibraryAction CameraSetExposure (In Real Seconds);
 
 LibraryAction DockIngestSample ();
 
+LibraryAction ActivateComms (In String Message, In Real DurationSecs);
+
 LibraryAction GuardedMove (In Real X,
                            In Real Y,
                            In Real Z,

ow_plexil/src/plans/ow-commands.h

@@ -241,6 +241,16 @@ static void dock_ingest_sample (Command* cmd, AdapterExecInterface* intf)
   acknowledge_command_sent(*cr);
 }
 
+static void activate_comms (Command* cmd, AdapterExecInterface* intf)
+{
+  double duration;
+  const vector<Value>& args = cmd->getArgValues();
+  args[0].getValue (duration);
+  unique_ptr<CommandRecord>& cr = new_command_record(cmd, intf);
+  OwInterface::instance()->activateComms(duration, g_cmd_id);
+  acknowledge_command_sent(*cr);
+}
+
 static void light_set_intensity (Command* cmd, AdapterExecInterface* intf)
 {
   bool valid_args = true;
@@ -308,13 +318,16 @@ bool OwAdapter::initialize (AdapterConfiguration* config)
   LanderAdapter::s_interface = OwInterface::instance();
 
   // Commands
-  config->registerCommandHandlerFunction("grind", grind);
-  config->registerCommandHandlerFunction("guarded_move", guarded_move);
+  config->registerCommandHandlerFunction("activate_comms", activate_comms);
   config->registerCommandHandlerFunction("arm_move_joints", arm_move_joints);
   config->registerCommandHandlerFunction("arm_move_joints_guarded",
-                                 arm_move_joints_guarded);
+                                         arm_move_joints_guarded);
+  config->registerCommandHandlerFunction("camera_set_exposure",
+                                         camera_set_exposure);
   config->registerCommandHandlerFunction("dock_ingest_sample",
                                          dock_ingest_sample);
+  config->registerCommandHandlerFunction("grind", grind);
+  config->registerCommandHandlerFunction("guarded_move", guarded_move);
   config->registerCommandHandlerFunction("pan", pan);
   config->registerCommandHandlerFunction("tilt", tilt);
   config->registerCommandHandlerFunction("scoop_circular", scoop_circular);
@@ -323,8 +336,6 @@ bool OwAdapter::initialize (AdapterConfiguration* config)
                                          pan_tilt_cartesian);
   config->registerCommandHandlerFunction("identify_sample_location",
                                          identify_sample_location);
-  config->registerCommandHandlerFunction("camera_set_exposure",
-                                         camera_set_exposure);
   config->registerCommandHandlerFunction("light_set_intensity",
                                          light_set_intensity);
   // Note: the following two are simulation utilities and not valid

ow_plexil/src/plans/ow-interface.h

@@ -48,6 +48,7 @@ const string Name_ArmMoveJoints          = "ArmMoveJoints";
 const string Name_ArmMoveJointsGuarded   = "ArmMoveJointsGuarded";
 const string Name_CameraSetExposure      = "CameraSetExposure";
 const string Name_Ingest                 = "DockIngestSample";
+const string Name_ActivateComms          = "ActivateComms";
 const string Name_Grind                  = "TaskGrind";
 const string Name_GuardedMove            = "GuardedMove";
 const string Name_IdentifySampleLocation = "IdentifySampleLocation";
@@ -60,6 +61,7 @@ const string Name_Tilt                   = "Tilt";
 const string Name_TaskDiscardSample      = "TaskDiscardSample";
 
 static vector<string> LanderOpNames = {
+  Name_ActivateComms,
   Name_ArmFindSurface,
   Name_GuardedMove,
   Name_ArmMoveJoints,
@@ -278,16 +280,17 @@ void OwInterface::initialize()
 
   // Initialize action clients
 
+  m_activateCommsClient =
+    make_unique<ActivateCommsActionClient>(Name_ActivateComms, true);
   m_armFindSurfaceClient =
     make_unique<ArmFindSurfaceActionClient>(Name_ArmFindSurface, true);
-  m_guardedMoveClient =
-    make_unique<GuardedMoveActionClient>(Name_GuardedMove, true);
   m_armMoveJointsClient =
     make_unique<ArmMoveJointsActionClient>(Name_ArmMoveJoints, true);
   m_armMoveJointsGuardedClient =
-    make_unique<ArmMoveJointsGuardedActionClient>(Name_ArmMoveJointsGuarded,
-                                                  true);
+    make_unique<ArmMoveJointsGuardedActionClient>(Name_ArmMoveJointsGuarded, true);
   m_grindClient = make_unique<TaskGrindActionClient>(Name_Grind, true);
+  m_guardedMoveClient =
+    make_unique<GuardedMoveActionClient>(Name_GuardedMove, true);
   m_scoopCircularClient =
     make_unique<TaskScoopCircularActionClient>(Name_TaskScoopCircular, true);
   m_scoopLinearClient =
@@ -328,6 +331,7 @@ void OwInterface::initialize()
       subscribe("/joint_states", QueueSize,
                 &OwInterface::jointStatesCallback, this)));
 
+  connectActionServer (m_activateCommsClient, Name_ActivateComms);
   connectActionServer (m_armFindSurfaceClient, Name_ArmFindSurface);
   connectActionServer (m_armMoveJointsClient, Name_ArmMoveJoints);
   connectActionServer (m_armMoveJointsGuardedClient, Name_ArmMoveJointsGuarded);
@@ -486,6 +490,32 @@ void OwInterface::dockIngestSampleAction (int id)
      default_action_done_cb<DockIngestSampleResultConstPtr> (Name_Ingest));
 }
 
+void OwInterface::activateComms (double duration, int id)
+{
+  if (! markOperationRunning (Name_ActivateComms, id)) return;
+  thread action_thread (&OwInterface::activateCommsAction, this, duration, id);
+  action_thread.detach();
+}
+
+void OwInterface::activateCommsAction (double duration, int id)
+{
+  ActivateCommsGoal goal;
+  goal.duration = duration;
+
+  ROS_INFO ("Starting ActivateComms()");
+
+  runAction<actionlib::SimpleActionClient<ActivateCommsAction>,
+            ActivateCommsGoal,
+            ActivateCommsResultConstPtr,
+            ActivateCommsFeedbackConstPtr>
+    (Name_ActivateComms, m_activateCommsClient, goal, id,
+     default_action_active_cb (Name_ActivateComms),
+     default_action_feedback_cb<ActivateCommsFeedbackConstPtr> (
+      Name_ActivateComms),
+     default_action_done_cb<ActivateCommsResultConstPtr> (
+      Name_ActivateComms));
+}
+
 void OwInterface::scoopLinear (int frame, bool relative,
                                double x, double y, double z,
                                double depth, double length, int id)

ow_plexil/src/plexil-adapter/OwAdapter.cpp

@@ -56,6 +56,7 @@ using TaskDiscardSampleActionClient =
 
 #include <ow_lander/GuardedMoveAction.h>
 #include <ow_lander/DockIngestSampleAction.h>
+#include <ow_lander/ActivateCommsAction.h>
 #include <ow_lander/TiltAction.h>
 #include <ow_lander/PanAction.h>
 
@@ -65,7 +66,8 @@ using PanActionClient = actionlib::SimpleActionClient<ow_lander::PanAction>;
 using TiltActionClient = actionlib::SimpleActionClient<ow_lander::TiltAction>;
 using DockIngestSampleActionClient =
   actionlib::SimpleActionClient<ow_lander::DockIngestSampleAction>;
-
+using ActivateCommsActionClient =
+  actionlib::SimpleActionClient<ow_lander::ActivateCommsAction>;
 
 // Telemetry
 #include <owl_msgs/ArmEndEffectorForceTorque.h>
@@ -112,6 +114,7 @@ class OwInterface : public LanderInterface
   void panTiltCartesian (int frame, double x, double y, double z, int id);
   void cameraSetExposure (double exposure_secs, int id);
   void dockIngestSample (int id);
+  void activateComms (double duration, int id);
   void scoopLinear (int frame, bool relative, double x, double y, double z,
                     double depth, double length, int id);
   void scoopCircular (int frame, bool relative, double x, double y, double z,
@@ -182,6 +185,7 @@ class OwInterface : public LanderInterface
                             double depth, bool parallel, int id);
   void cameraSetExposureAction (double exposure_secs, int id);
   void dockIngestSampleAction (int id);
+  void activateCommsAction (double duration, int id);
   void lightSetIntensityAction (const std::string& side, double intensity, int id);
   void jointStatesCallback (const sensor_msgs::JointState::ConstPtr&);
   void antennaOp (const std::string& opname, double degrees,
@@ -230,6 +234,7 @@ class OwInterface : public LanderInterface
   std::unique_ptr<TaskScoopLinearActionClient> m_scoopLinearClient;
   std::unique_ptr<CameraSetExposureActionClient> m_cameraSetExposureClient;
   std::unique_ptr<DockIngestSampleActionClient> m_dockIngestSampleClient;
+  std::unique_ptr<ActivateCommsActionClient> m_activateCommsClient;
   std::unique_ptr<LightSetIntensityActionClient> m_lightSetIntensityClient;
   std::unique_ptr<IdentifySampleLocationActionClient> m_identifySampleLocationClient;
   std::unique_ptr<TaskDiscardSampleActionClient> m_taskDiscardSampleClient;