[TeamTalk 232]: [768] trunk/moast-bth: Switched from vehicle-mode to autonomous-mobility-mode moves in teamtalksimulator

[tk@edam.speech.cs.cmu.edu]

An HTML attachment was scrubbed...
URL: http://mailman.srv.cs.cmu.edu/pipermail/teamtalk-developers/attachments/20070924/104bc4d5/attachment.html
-------------- next part --------------
Modified: trunk/moast-bth/TeamTalkSimulator/robot.cc
===================================================================
--- trunk/moast-bth/TeamTalkSimulator/robot.cc	2007-09-23 02:59:43 UTC (rev 767)
+++ trunk/moast-bth/TeamTalkSimulator/robot.cc	2007-09-24 04:59:03 UTC (rev 768)
@@ -539,7 +539,11 @@
       debug << "interpreting as turn relative: " << goTo->getAngle() << endl;
       float r = getYaw();
       AmMobJACmdSpin spin;
-      spin.absAngle = canonical_angle(r - goTo->getAngle());
+      //we are right-handed, moast is also right-handed, but since 
+      //z points down in moast it appears left-handeed on the xy plane
+      //so we take the opposite of the difference 
+      //from the current yaw to the goal yaw
+      spin.absAngle = canonical_angle(-(goTo->getAngle()-r)); 
       spin.tolerance = PI/40;
       spin.direction = MOAST_ROTATE_SHORTEST;
       spin.serial_number = amStatP->echo_serial_number + 1;
@@ -558,15 +562,17 @@
 	destination = 
 	  loc + Point<float>::Polar(vec.length(), getYaw()+vec.angle());
       }
-      VehMobPLCmdMoveWaypoint move = VehMobPLCmdMoveWaypoint();
-      move.p.x = destination.x;
-      move.p.y = destination.y;
-      move.serial_number = statP->echo_serial_number+1;
-      move.isGlobal = true;
-      move.neighborhood = 1;
-      move.startTime = 0;
-      vehMobPLCmdBuf->write(&move);
-      setNewWorkingMsg(goTo, VEHWORK, move.serial_number);
+      AmMobJACmdMoveWaypoint move = AmMobJACmdMoveWaypoint();
+      move.waypoint[0].p.x = destination.x;
+      move.waypoint[0].p.y = destination.y;
+      move.waypoint[0].p.z = 0;
+      move.serial_number = amStatP->echo_serial_number+1;
+      move.waypoint[0].neighborhood = 0.2;
+      move.waypoint[0].speed = 1.0;
+      move.waypoint_length = 1;
+      move.computeHorizonCosts =  false;
+      amMobJACmdBuf->write(&move);
+      setNewWorkingMsg(goTo, AMWORK, move.serial_number);
     }
     return;
   } else if(const MsgCmdHalt* halt = dynamic_cast<const MsgCmdHalt*>(msgp)) {
@@ -575,14 +581,17 @@
     return;
   } else if(const MsgCmdHome* home = dynamic_cast<const MsgCmdHome*>(msgp)) {
     debug << "got: " << home << endl;
-    VehMobPLCmdMoveWaypoint move = VehMobPLCmdMoveWaypoint();
-    move.p.x = 0;
-    move.p.y = 0;
-    move.serial_number = statP->echo_serial_number+1;
-    move.isGlobal = true;
-    move.neighborhood = 1;
-    move.startTime = 0;
-    vehMobPLCmdBuf->write(&move);
+    AmMobJACmdMoveWaypoint move = AmMobJACmdMoveWaypoint();
+    move.waypoint[0].p.x = 0;
+    move.waypoint[0].p.y = 0;
+    move.waypoint[0].p.z = 0;
+    move.serial_number = amStatP->echo_serial_number+1;
+    move.waypoint[0].neighborhood = 0.2;
+    move.waypoint[0].speed = 1.0;
+    move.waypoint_length = 1;
+    move.computeHorizonCosts =  false;
+    amMobJACmdBuf->write(&move);
+    setNewWorkingMsg(goTo, AMWORK, move.serial_number);
     return;
   } else if(const MsgCmdPause* pause = dynamic_cast<const MsgCmdPause*>(msgp)) {
     debug << "got: " << pause << endl;

Modified: trunk/moast-bth/moast/bin/regression/four_right_turns.exp
===================================================================
--- trunk/moast-bth/moast/bin/regression/four_right_turns.exp	2007-09-23 02:59:43 UTC (rev 767)
+++ trunk/moast-bth/moast/bin/regression/four_right_turns.exp	2007-09-24 04:59:03 UTC (rev 768)
@@ -1,7 +1,7 @@
 #!/usr/bin/expect -f
 
 set rt_time 20
-set rt_tol 6
+set rt_tol 4.5
 set adm_prompt "> \rAdm> "
 set mob_prompt "> \rMob> "
 set timeout -1