Merge pull request #41 from robocup-logistics/thofmann/simple-storage-station

Implement a simple storage station that dispenses a single C0

Author: morxa

cfg/config.yaml

@@ -57,6 +57,12 @@ llsfrb:
         port: !tcp-port 4840
         type: DS
         connection: mockup
+      C-SS:
+        active: true
+        host: !ipv4 192.168.2.22
+        port: !tcp-port 4840
+        type: SS
+        connection: mockup
       M-BS:
         active: true
         host: !ipv4 192.168.2.33
@@ -93,6 +99,12 @@ llsfrb:
         port: !tcp-port 4840
         type: DS
         connection: mockup
+      M-SS:
+        active: true
+        host: !ipv4 192.168.2.35
+        port: !tcp-port 4840
+        type: SS
+        connection: mockup
 
   clips:
     # Timer interval, in milliseconds

src/games/rcll/facts.clp

@@ -76,7 +76,8 @@
   (slot ds-order (type INTEGER))
 
   (slot ss-operation (type SYMBOL) (allowed-values STORE RETRIEVE))
-  (multislot ss-slot (type INTEGER) (cardinality 3 3)) ; meaning defined in llsf_msgs.SSSlot
+  ;(multislot ss-slot (type INTEGER) (cardinality 3 3)) ; meaning defined in llsf_msgs.SSSlot
+	(slot ss-holding (type SYMBOL) (allowed-values TRUE FALSE) (default TRUE))
 
   (slot rs-ring-color (type SYMBOL)
 	(allowed-values RING_BLUE RING_GREEN RING_ORANGE RING_YELLOW))

src/games/rcll/globals.clp

@@ -53,6 +53,7 @@
   ; How long to wait before resetting a machine that is processing
   ?*PROCESSING-WAIT-TILL-RESET* = 90
   ?*PREPARE-WAIT-TILL-PROCESSING* = 5
+  ?*PROCESS-TIME-SS* = 5
 
   ; number of points for specific actions
   ?*EXPLORATION-CORRECT-REPORT-ROTATION-POINTS* = 1
@@ -76,6 +77,7 @@
   ?*PRODUCTION-DELIVER-MAX-LATENESS-TIME* = 10
   ?*PRODUCTION-POINTS-COMPETITIVE-FIRST-BONUS* = 10
   ?*PRODUCTION-POINTS-COMPETITIVE-SECOND-DEDUCTION* = 10
+  ?*PRODUCTION-POINTS-SS-RETRIEVAL* = -10
 	; Workpiece ranges
 	?*WORKPIECE-RANGE-RED* = (create$ 1001 1999)
 	?*WORKPIECE-RANGE-BLACK* = (create$ 2001 2999)

src/games/rcll/net.clp

@@ -523,17 +523,8 @@
             (pb-set-field ?m "instruction_ds" ?pm)
 	  )
 	  (case SS then
-	    (bind ?pssm (pb-create "llsf_msgs.SSSlot"))
-	    (pb-set-field ?pssm "x" (nth$ 1 (fact-slot-value ?mf ss-slot)))
-	    (pb-set-field ?pssm "y" (nth$ 2 (fact-slot-value ?mf ss-slot)))
-	    (pb-set-field ?pssm "z" (nth$ 3 (fact-slot-value ?mf ss-slot)))
-
-	    (bind ?psm (pb-create "llsf_msgs.SSTask"))
-	    (pb-set-field ?psm "operation" (fact-slot-value ?mf ss-operation))
-	    (pb-set-field ?psm "shelf" ?pssm)
-
 	    (bind ?pm (pb-create "llsf_msgs.PrepareInstructionSS"))
-	    (pb-set-field ?pm "task" ?psm)
+	    (pb-set-field ?pm "operation" (fact-slot-value ?mf ss-operation))
       (pb-set-field ?m "instruction_ss" ?pm)
 	  )
 	  (case RS then

src/games/rcll/production.clp

@@ -109,36 +109,24 @@
 	      (if (pb-has-field ?p "instruction_ss")
 	       then
 	        (bind ?prepmsg (pb-field-value ?p "instruction_ss"))
-	        (bind ?task (pb-field-value ?prepmsg "task"))
-		(bind ?operation (sym-cat (pb-field-value ?task "operation")))
-		(bind ?slot (pb-field-value ?task "shelf"))
-                (bind ?slot-x (pb-field-value ?slot "x"))
-                (bind ?slot-y (pb-field-value ?slot "y"))
-                (bind ?slot-z (pb-field-value ?slot "z"))
-
+	        (bind ?operation (sym-cat (pb-field-value ?prepmsg "operation")))
                 (if (eq ?operation RETRIEVE)
                  then
-                  ; check if slot is filled
-                  (if (any-factp ((?ss-slot machine-ss-filled)) (and (eq ?ss-slot:name ?mname)
-                                                                     (and (eq (nth$ 1 ?ss-slot:slot) ?slot-x)
-                                                                          (and (eq (nth$ 2 ?ss-slot:slot) ?slot-y)
-                                                                               (eq (nth$ 3 ?ss-slot:slot) ?slot-z)
-                                                                          )
-                                                                     )
-                                                                )
-                      )
+                  (if ?m:ss-holding
                    then
-                    (printout t "Prepared " ?mname " (RETRIVE: (" ?slot-x ", " ?slot-y ", " ?slot-z ") )" crlf)
-                    (modify ?m (state PREPARED) (ss-operation ?operation) (ss-slot ?slot-x ?slot-y ?slot-z) (wait-for-product-since ?gt))
+                    (printout t "Prepared " ?mname crlf)
+                    (modify ?m (state PREPARED) (ss-operation ?operation) (wait-for-product-since ?gt))
                    else
-		    (modify ?m (state BROKEN) (broken-reason (str-cat "Prepare received for " ?mname " with RETRIVE (" ?slot-x ", " ?slot-y ", " ?slot-z ") but this is empty")))
+                    (modify ?m (state BROKEN)
+                               (broken-reason
+                                 (str-cat "Prepare received for " ?mname ", but station is empty")))
                   )
                  else
                   (if (eq ?operation STORE)
                    then
-		    (modify ?m (state BROKEN) (broken-reason (str-cat "Prepare received for " ?mname " with STORE-operation")))
+                     (modify ?m (state BROKEN) (broken-reason (str-cat "Prepare received for " ?mname " with STORE operation")))
                    else
-		    (modify ?m (state BROKEN) (broken-reason (str-cat "Prepare received for " ?mname " with unknown operation")))
+                     (modify ?m (state BROKEN) (broken-reason (str-cat "Prepare received for " ?mname " with unknown operation " ?operation)))
                   )
                 )
                else
@@ -450,10 +438,10 @@
 	(mps-move-conveyor (str-cat ?n) "OUTPUT" "FORWARD")
 )
 
-(defrule production-bs-cs-rs-ready-at-output
+(defrule production-bs-cs-rs-ss-ready-at-output
 	"Workpiece is in output, switch to READY-AT-OUTPUT"
 	(gamestate (state RUNNING) (phase PRODUCTION) (game-time ?gt))
-	?m <- (machine (name ?n) (mtype BS|CS|RS) (state PROCESSED) (task MOVE-OUT)
+	?m <- (machine (name ?n) (mtype BS|CS|RS|SS) (state PROCESSED) (task MOVE-OUT)
 	               (mps-busy FALSE) (mps-ready TRUE))
 	=>
 	(modify ?m (state READY-AT-OUTPUT) (task nil))
@@ -504,6 +492,28 @@
   (modify ?m (state WAIT-IDLE) (idle-since ?gt))
 )
 
+(defrule production-ss-start-retrieval
+	"SS is prepared, move the workpiece to the output."
+	(gamestate (state RUNNING) (phase PRODUCTION) (game-time ?gt))
+	?m <- (machine (name ?n) (mtype SS) (state PREPARED)
+                 (ss-operation RETRIEVE) (task nil))
+	=>
+	(modify ?m (state PROCESSING) (proc-start ?gt))
+)
+
+(defrule production-ss-processed-retrieval
+	"The conveyor started moving, go to processed."
+	(gamestate (state RUNNING) (phase PRODUCTION) (game-time ?gt))
+	?m <- (machine (name ?n) (mtype SS) (team ?team) (state PROCESSING) (ss-operation RETRIEVE)
+	               (proc-start ?t&:(timeout-sec ?gt ?t ?*PROCESS-TIME-SS*)))
+	=>
+	(assert (points (game-time ?gt) (team ?team) (phase PRODUCTION)
+	                (points ?*PRODUCTION-POINTS-SS-RETRIEVAL*)
+	                (reason (str-cat "Retrieved product from " ?n))))
+	(mps-move-conveyor (str-cat ?n) "OUTPUT" "FORWARD")
+	(modify ?m (state PROCESSED) (task MOVE-OUT) (mps-busy WAIT) (ss-holding FALSE))
+)
+
 (defrule production-mps-idle
 	"The machine has been in WAIT-IDLE for the specified time, switch to IDLE."
 	(gamestate (state RUNNING) (phase PRODUCTION) (game-time ?gt))

src/libs/mps_comm/storage_station.cpp

@@ -21,11 +21,6 @@ StorageStation::StorageStation(std::string    name,
 
 StorageStation::~StorageStation() {}
 
-void StorageStation::get_product(int slot) {
-  //lock_guard<mutex> g(lock_);
-  send_command(machine_type_ + Operation::OPERATION_GET_F_PRODUCT, slot);
-}
-
 void StorageStation::identify() {
   send_command(Command::COMMAND_SET_TYPE, StationType::STATION_TYPE_SS);
 }

src/libs/mps_comm/storage_station.h

@@ -19,11 +19,6 @@ class StorageStation: public Machine {
     StorageStation(std::string name, std::string ip, unsigned short port, ConnectionMode mode);
     virtual ~StorageStation();
 
-    // Send command to deliver a product
-    // slot is between 1 and 3
-    // deprecated
-    void get_product(int slot);
-
     // identify: tell the PLC, which machine it is controlling
     virtual void identify();
 };

src/msgs/MachineInstructions.proto

@@ -50,11 +50,6 @@ enum MachineSide {
   OUTPUT = 2;
 }
 
-message SSTask {
-  required SSOp   operation = 2;
-  required SSSlot shelf     = 3;
-}
-
 message PrepareInstructionBS {
   required MachineSide side  = 1;
   required BaseColor   color = 2;
@@ -67,7 +62,7 @@ message PrepareInstructionDS {
 }
 
 message PrepareInstructionSS {
-  required SSTask task = 1;
+  required SSOp   operation = 1;
 }
 
 message PrepareInstructionRS {

src/refbox/refbox.cpp

@@ -234,6 +234,10 @@ LLSFRefBox::LLSFRefBox(int argc, char **argv)
 	      logger_->log_info("RefBox", "Adding DS %s:%u", mpsip.c_str(), port);
 	      mps = new DeliveryStation(cfg_name, mpsip, port, connection_mode);
 	    }
+      else if (mpstype == "SS") {
+	      logger_->log_info("RefBox", "Adding SS %s:%u", mpsip.c_str(), port);
+	      mps = new StorageStation(cfg_name, mpsip, port, connection_mode);
+      }
 	    else {
 	      throw fawkes::Exception("this type wont match");
 	    }

src/tools/rcll-prepare-machine.cpp

@@ -173,14 +173,7 @@ handle_message(boost::asio::ip::udp::endpoint &sender,
 	prep_ds->set_order_id(ds_order_id_);
       } else if (machine_type_ == "SS") {
 	llsf_msgs::PrepareInstructionSS *prep_ss = prep.mutable_instruction_ss();
-        
-        llsf_msgs::SSTask *ss_task = prep_ss->mutable_task();
-        ss_task->set_operation( ss_op_ );
-        
-        llsf_msgs::SSSlot *ss_slot = ss_task->mutable_shelf();
-        ss_slot->set_x(ss_slot_x_);
-        ss_slot->set_y(ss_slot_y_);
-        ss_slot->set_z(ss_slot_z_);
+	prep_ss->set_operation(ss_op_);
       } else if (machine_type_ == "RS") {
 	llsf_msgs::PrepareInstructionRS *prep_rs = prep.mutable_instruction_rs();
 	prep_rs->set_ring_color(rs_ring_color_);
@@ -201,7 +194,7 @@ usage(const char *progname)
 	 "instructions are specific for the machine type:\n"
 	 "BS:  (INPUT|OUTPUT) (BASE_RED|BASE_BLACK|BASE_SILVER)\n"
 	 "DS:  <order_id>\n"
-	 "SS:  (RETRIEVE|STORE) <slot-x> <slot-y> <slot-z>\n"
+	 "SS:  (RETRIEVE|STORE)\n"
 	 "RS:  (RING_BLUE|RING_GREEN|RING_ORANGE|RING_YELLOW)\n"
 	 "CS:  (RETRIEVE_CAP|MOUNT_CAP)\n",
 	 progname);
@@ -242,17 +235,14 @@ main(int argc, char **argv)
     }
     ds_order_id_ = argp.parse_item_int(2);
   } else if (machine_type_ == "SS") {
-    if (argp.num_items() < 6) {
-      printf("SS machine requires operation and x, y, z arguments %zu\n", argp.num_items());
+    if (argp.num_items() != 3) {
+      printf("Wrong number of arguments. Expected 3, got %zu\n", argp.num_items());
       usage(argv[0]);
       exit(-1);
     }
     if (! llsf_msgs::SSOp_Parse(argp.items()[2], &ss_op_)) {
       printf("Invalid operation\n"); exit(-2);
     }
-    ss_slot_x_  = argp.parse_item_int(3);
-    ss_slot_y_  = argp.parse_item_int(4);
-    ss_slot_z_  = argp.parse_item_int(5);
   } else if (machine_type_ == "RS") {
     if (! llsf_msgs::RingColor_Parse(argp.items()[2], &rs_ring_color_)) {
       printf("Invalid ring color\n"); exit(-2);