Merge pull request #78 from nasa/OCEANWATER-886-use-new-discard-deliver

PLEXIL uses new Discard/Deliver actions

ow_plexil/src/plans/CollectAndTransfer.plp

@@ -110,9 +110,7 @@ CollectAndTransfer:
     {
       SkipCondition LastComplete >= 5;
 
-      // NOTE: the given coordinates are the predetermined location of the
-      // receptacle into which the sample is dumped.
-      LibraryCall Deliver (X = 0.55, Y = -0.3, Z = 0.84);
+      LibraryCall Deliver ();
       Wait 1;
       set_checkpoint(OurName,true,Lookup(ToString(5,"_",CollectMore)));
       SynchronousCommand flush_checkpoints();

ow_plexil/src/plans/CollectSample.plp

@@ -19,7 +19,5 @@ CollectSample:
                      Parallel = Parallel, GroundPos = GroundPos);
   LibraryCall DigCircular (X = X, Y = Y, Depth = Depth,
                            GroundPos = GroundPos, Parallel = Parallel);
-  // NOTE: the given coordinates are the predetermined location of the
-  // receptacle into which the sample is dumped.
-  LibraryCall Deliver (X = 0.55, Y = -0.3, Z = 0.84);
+  LibraryCall Deliver ();
 }

ow_plexil/src/plans/Deliver.plp

@@ -10,10 +10,6 @@
 
 Deliver:
 {
-  In Real X;
-  In Real Y;
-  In Real Z;
-
   Boolean FaultDetected = false;
 
   PostCondition !Lookup(ArmFault);
@@ -33,7 +29,7 @@ Deliver:
       log_info ("Arm fault(s) resolved, sending deliver command to lander...");
     }
 
-    SynchronousCommand deliver (X, Y, Z);
+    SynchronousCommand deliver ();
   }
 
 }

ow_plexil/src/plans/Discard.plp

@@ -0,0 +1,39 @@
+// 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.
+
+// Discard material in the scoop to the given point.  This is used for both
+// discarding the sample to its receptacle (which has specific coordinates) and
+// dumping tailings from trench digging.
+
+#include "plan-interface.h"
+
+Discard:
+{
+  In Real X;
+  In Real Y;
+  In Real Z;
+
+  Boolean FaultDetected = false;
+
+  PostCondition !Lookup(ArmFault);
+
+  if Lookup(ArmFault)
+  { 
+    log_error ("Command Discard not sent to lander due to active arm fault(s).");
+    FaultDetected = true;
+  }
+
+  SendDiscard:
+  {
+    Start !Lookup(ArmFault);
+
+    if FaultDetected
+    {
+      log_info ("Arm fault(s) resolved, sending Discard command to lander...");
+    }
+
+    SynchronousCommand discard (X, Y, Z);
+  }
+
+}

ow_plexil/src/plans/RemoveTailings.plp

@@ -23,9 +23,11 @@ RemoveTailings:
     log_info ("Removing tailings, pass ", pass, " of ", num_passes, "...");
     LibraryCall DigCircular (X = X, Y = Y, Depth = bite_depth * pass,
                              GroundPos = GroundPos, Parallel = Parallel);
-    // NOTE: the X/Y/Z values are known coordinates of a predetermined place to
-    // dump debris.  A real mission procedure would need to find an appropriate
-    // dump location.
-    LibraryCall Deliver (X = 1.5, Y = 0.8, Z = 0.65);
+
+    // NOTE: these X/Y/Z values specify a predecided dump location
+    // that works in any of the provided simulation worlds.  A real
+    // mission procedure would need to find an appropriate dump
+    // location.
+    LibraryCall Discard (X = 1.5, Y = 0.8, Z = 0.65);
   }
 }

ow_plexil/src/plans/TestActions.plp

@@ -13,7 +13,7 @@ TestActions:
 
   LibraryCall Unstow();
 
-	LibraryCall GuardedMove (X=1.75, Y=0.1, Z=0.2, DirX=0.1, DirY=0.1, DirZ=0.9,
+  LibraryCall GuardedMove (X=1.75, Y=0.1, Z=0.2, DirX=0.1, DirY=0.1, DirZ=0.9,
                            SearchDistance=0.7);
 
   LibraryCall Grind (X = 1.75, Y = 0.1, Depth = 0.045,
@@ -22,17 +22,17 @@ TestActions:
 
   LibraryCall DigCircular (X = 1.75, Y = 0.1, Depth = 0.045,
                            GroundPos = GP, Parallel = false);
-
-  LibraryCall Grind (X = 1.75, Y = 0.1, Depth = 0.045,
+
+  LibraryCall Discard (X = 1.5, Y = 0.8, Z = 0.65);
+
+  LibraryCall Grind (X = 1.75, Y = 0.1, Depth = 0.05,
                      Length = 0.5, Parallel = true,
                      GroundPos = GP);
 
-  LibraryCall DigLinear (X = 1.75, Y = 0.1, Depth = 0.045, Length = 0.1,
+  LibraryCall DigLinear (X = 1.75, Y = 0.1, Depth = 0.05, Length = 0.1,
                          GroundPos = GP);
 
-  // Receptacle coordinates
-  LibraryCall Deliver (X = 0.55, Y = -0.3, Z = 0.84);
-
+  LibraryCall Deliver ();
   LibraryCall Unstow(); // For safety in stowing
   LibraryCall Stow();
 

ow_plexil/src/plans/TestDiscardDeliver.plp

@@ -0,0 +1,29 @@
+// 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.
+
+// A simple test that collects a sample, discards it, collects again,
+// and delivers it.
+
+#include "plan-interface.h"
+
+TestDiscardDeliver:
+{
+  Real GP = -0.155;  // Ground position
+  log_info ("Starting TestDiscardDeliver...");
+  LibraryCall Unstow();
+  LibraryCall GuardedMove (X=1.75, Y=0.1, Z=0.2, DirX=0.1, DirY=0.1, DirZ=0.9,
+                           SearchDistance=0.7);
+  LibraryCall Grind (X = 1.75, Y = 0.1, Depth = 0.045,
+                     Length = 0.5, Parallel = false,
+                     GroundPos = GP);
+  LibraryCall DigCircular (X = 1.75, Y = 0.1, Depth = 0.045,
+                           GroundPos = GP, Parallel = false);
+  LibraryCall Discard (X = 1.5, Y = 0.8, Z = 0.65);
+  LibraryCall DigCircular (X = 1.75, Y = 0.1, Depth = 0.045,
+                           GroundPos = GP, Parallel = false);
+  LibraryCall Deliver ();
+  LibraryCall Unstow(); // For safety in stowing
+  LibraryCall Stow();
+  log_info ("TestDiscardDeliver finished.");
+}

ow_plexil/src/plans/TestLanderLights.plp

@@ -1,3 +1,7 @@
+// 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.
+
 #include "plan-interface.h"
 
 TestLanderLights:

ow_plexil/src/plans/lander-commands.h

@@ -30,9 +30,9 @@ Command dig_linear (Real x,
                     Real length,
                     Real ground_pos);
 
-// This dumps the scoop at the given location; can be used for removing tailings
-// as well as delivering a sample to the receptacle.
-Command deliver (Real x,
+Command deliver ();
+
+Command discard (Real x,
                  Real y,
                  Real z);
 

ow_plexil/src/plans/plan-interface.h

@@ -50,7 +50,9 @@ LibraryAction DigLinear (In Real X,
                          In Real Length,
                          In Real GroundPos);
 
-LibraryAction Deliver (In Real X,
+LibraryAction Deliver ();
+
+LibraryAction Discard (In Real X,
                        In Real Y,
                        In Real Z);
 

ow_plexil/src/plexil-adapter/OwAdapter.cpp

@@ -217,14 +217,21 @@ static void dig_linear (Command* cmd, AdapterExecInterface* intf)
 }
 
 static void deliver (Command* cmd, AdapterExecInterface* intf)
+{
+  unique_ptr<CommandRecord>& cr = new_command_record(cmd, intf);
+  OwInterface::instance()->deliver (CommandId);
+  acknowledge_command_sent(*cr);
+}
+
+static void discard (Command* cmd, AdapterExecInterface* intf)
 {
   double x, y, z;
   const vector<Value>& args = cmd->getArgValues();
   args[0].getValue(x);
   args[1].getValue(y);
   args[2].getValue(z);
   unique_ptr<CommandRecord>& cr = new_command_record(cmd, intf);
-  OwInterface::instance()->deliver (x, y, z, CommandId);
+  OwInterface::instance()->discard (x, y, z, CommandId);
   acknowledge_command_sent(*cr);
 }
 
@@ -324,6 +331,7 @@ bool OwAdapter::initialize()
   g_configuration->registerCommandHandler("dig_circular", dig_circular);
   g_configuration->registerCommandHandler("dig_linear", dig_linear);
   g_configuration->registerCommandHandler("deliver", deliver);
+  g_configuration->registerCommandHandler("discard", discard);
   g_configuration->registerCommandHandler("tilt_antenna", tilt_antenna);
   g_configuration->registerCommandHandler("pan_antenna", pan_antenna);
   g_configuration->registerCommandHandler("identify_sample_location",

ow_plexil/src/plexil-adapter/OwInterface.cpp

@@ -100,6 +100,7 @@ const string Op_GuardedMove       = "GuardedMove";
 const string Op_DigCircular       = "DigCircular";
 const string Op_DigLinear         = "DigLinear";
 const string Op_Deliver           = "Deliver";
+const string Op_Discard           = "Discard";
 const string Op_PanAntenna        = "PanAntenna";
 const string Op_TiltAntenna       = "TiltAntenna";
 const string Op_Grind             = "Grind";
@@ -117,6 +118,7 @@ enum LanderOps {
   DigCircular,
   DigLinear,
   Deliver,
+  Discard,
   Pan,
   Tilt,
   Grind,
@@ -128,7 +130,7 @@ enum LanderOps {
 };
 
 static vector<string> LanderOpNames = {
-  Op_GuardedMove, Op_DigCircular, Op_DigLinear, Op_Deliver,
+  Op_GuardedMove, Op_DigCircular, Op_DigLinear, Op_Deliver, Op_Discard,
   Op_PanAntenna, Op_TiltAntenna, Op_Grind, Op_Stow, Op_Unstow, Op_TakePicture,
   Op_IdentifySampleLocation, Op_SetLightIntensity
 };
@@ -556,6 +558,7 @@ void OwInterface::initialize()
     m_digCircularClient = make_unique<DigCircularActionClient>(Op_DigCircular, true);
     m_digLinearClient = make_unique<DigLinearActionClient>(Op_DigLinear, true);
     m_deliverClient = make_unique<DeliverActionClient>(Op_Deliver, true);
+    m_discardClient = make_unique<DiscardActionClient>(Op_Discard, true);
     m_identifySampleLocationClient = make_unique<IdentifySampleLocationActionClient>
       (Op_IdentifySampleLocation, true);
 
@@ -579,6 +582,10 @@ void OwInterface::initialize()
         waitForServer(ros::Duration(ACTION_SERVER_TIMEOUT_SECS))) {
       ROS_ERROR ("Deliver action server did not connect!");
     }
+    if (! m_discardClient->
+        waitForServer(ros::Duration(ACTION_SERVER_TIMEOUT_SECS))) {
+      ROS_ERROR ("Discard action server did not connect!");
+    }
     if (! m_guardedMoveClient->
         waitForServer(ros::Duration(ACTION_SERVER_TIMEOUT_SECS))) {
       ROS_ERROR ("GuardedMove action server did not connect!");
@@ -625,22 +632,26 @@ void OwInterface::takePicture (int id)
   m_leftImageTriggerPublisher->publish (msg);
 }
 
-void OwInterface::deliver (double x, double y, double z, int id)
+void OwInterface::deliver (int id)
 {
   if (! markOperationRunning (Op_Deliver, id)) return;
-  thread action_thread (&OwInterface::deliverAction, this, x, y, z, id);
+  thread action_thread (&OwInterface::deliverAction, this, id);
+  action_thread.detach();
+}
+
+void OwInterface::discard (double x, double y, double z, int id)
+{
+  if (! markOperationRunning (Op_Discard, id)) return;
+  thread action_thread (&OwInterface::discardAction, this, x, y, z, id);
   action_thread.detach();
 }
 
 
-void OwInterface::deliverAction (double x, double y, double z, int id)
+void OwInterface::deliverAction (int id)
 {
   DeliverGoal goal;
-  goal.delivery.x = x;
-  goal.delivery.y = y;
-  goal.delivery.z = z;
 
-  ROS_INFO ("Starting Deliver(x=%.2f, y=%.2f, z=%.2f)", x, y, z);
+  ROS_INFO ("Starting Deliver()");
 
   runAction<actionlib::SimpleActionClient<DeliverAction>,
             DeliverGoal,
@@ -652,6 +663,25 @@ void OwInterface::deliverAction (double x, double y, double z, int id)
      default_action_done_cb<DeliverResultConstPtr> (Op_Deliver));
 }
 
+void OwInterface::discardAction (double x, double y, double z, int id)
+{
+  DiscardGoal goal;
+  goal.discard.x = x;
+  goal.discard.y = y;
+  goal.discard.z = z;
+
+  ROS_INFO ("Starting Discard(x=%.2f, y=%.2f, z=%.2f)", x, y, z);
+
+  runAction<actionlib::SimpleActionClient<DiscardAction>,
+            DiscardGoal,
+            DiscardResultConstPtr,
+            DiscardFeedbackConstPtr>
+    (Op_Discard, m_discardClient, goal, id,
+     default_action_active_cb (Op_Discard),
+     default_action_feedback_cb<DiscardFeedbackConstPtr> (Op_Discard),
+     default_action_done_cb<DiscardResultConstPtr> (Op_Discard));
+}
+
 void OwInterface::digLinear (double x, double y,
                              double depth, double length, double ground_pos,
                              int id)