Add a contact point to the contact-report when the robot stalls

robots/primotion/pilotClient.cc

@@ -55,6 +55,8 @@ void pilotMoveCallback::call(int status)
     mtp_packet_t mp;
     mtp_status_t ms;
 
+    fprintf(stderr, "callback: %d\n", status);
+
     switch (status) {
     case aGARCIA_ERRFLAG_NORMAL:
 	ms = MTP_POSITION_STATUS_COMPLETE;
@@ -316,6 +318,11 @@ bool pilotClient::handlePacket(mtp_packet_t *mp, list &notify_list)
 		points[count].y = sin(-M_PI + 0.40) * mgt->rear_ranger_right;
 		count += 1;
 	    }
+	    else if (mgt->stall_contact && count == 0) {
+		points[count].x = (mgt->stall_contact < 0) ? -0.20 : 0.15;
+		points[count].y = 0;
+		count += 1;
+	    }
 
 	    mtp_init_packet(&cmp,
 			    MA_Opcode, MTP_CONTACT_REPORT,

robots/primotion/wheelManager.cc

@@ -149,7 +149,9 @@ aErr endCallback::call()
     status = this->ec_behavior->
 	getNamedValue("completion-status")->getIntVal();
 
-    this->ec_wheel_manager.motionFinished(status, this->ec_callback);
+    this->ec_wheel_manager.motionFinished(this->ec_behavior,
+					  status,
+					  this->ec_callback);
 
     this->ec_callback = NULL;
     
@@ -321,11 +323,23 @@ void wheelManager::motionStarted(void)
     this->wm_dashboard->startMove(this->wm_garcia);
 }
 
-void wheelManager::motionFinished(int status, wmCallback *callback)
+void wheelManager::motionFinished(acpObject *behavior,
+				  int status,
+				  wmCallback *callback)
 {
     if (debug) {
 	fprintf(stderr, "debug: motion finished -- %d\n", status);
     }
+
+    if (status == aGARCIA_ERRFLAG_STALL) {
+	float distance = behavior->getNamedValue("distance")->getFloatVal();
+
+	this->wm_dashboard->getTelemetry()->stall_contact =
+	    (distance < 0) ? -1 : 1;
+    }
+    else {
+	this->wm_dashboard->getTelemetry()->stall_contact = 0;
+    }
     
     if (status != aGARCIA_ERRFLAG_WONTEXECUTE) {
 	if ((status != aGARCIA_ERRFLAG_NORMAL) &&

robots/primotion/wheelManager.hh

@@ -161,7 +161,7 @@ public:
      * values.)
      * @param callback The callback to trigger.
      */
-    void motionFinished(int status, wmCallback *callback);
+    void motionFinished(acpObject *behavior, int status, wmCallback *callback);
 
     /**
      * Check the object to make sure it is internally consistent.