Updates from pool test

Author: Marlin

src/sub_mission/src/commands.cpp

@@ -10,8 +10,8 @@ float align(vision::Perception &per, ros::ServiceClient &client, int attempts)
     {
         client.call(per);
         printResponse(per);
-
-        average += atmega::state().axis[YAW] + per.response.hangle; 
+        if (per.response.prob > 0.5)
+            average += atmega::state().axis[YAW] + per.response.hangle; 
     }
 
     average /= attempts;
@@ -20,6 +20,7 @@ float align(vision::Perception &per, ros::ServiceClient &client, int attempts)
 
 void move(const State &dest)
 {
+    atmega::write(dest);
     bool quit = false;
     while (!quit)
     {

src/sub_mission/src/mission.cpp

@@ -26,6 +26,7 @@ int main(int argc, char** argv)
         else 
             start = true;
     }
+    atmega::write("p 0.2\n");
 
     // Run mission functions.
     gate(perception, client);

src/sub_mission/src/prelim.cpp

@@ -18,15 +18,17 @@ int main(int argc, char** argv)
     bool start = false;
     while (!start && !SIM)
     {
+        ROS_INFO("Waiting to start PRELIM.");
         if (!atmega::alive())
             ros::Duration(0.5).sleep();
         else 
             start = true;
     }
+    atmega::write("p 0.2\n");
 
     // Run mission functions.
-    move(State(8, -2, 0, 0, 0, 0));
-    move(State(8, 2, 0, 0, 0, 0));
-    move(State(4, 0, 0, 0, 0, 0));
+    move(State(3, -1, 0, 0, 0, 0));
+    move(State(3, 1, 0, 0, 0, 0));
+    move(State(1, 0, 0, 0, 0, 0));
     move(State(-1, 0, 0, 0, 0, 0));
 }

src/sub_vision/src/acquisition.cpp

@@ -68,7 +68,7 @@ int acquireImages(CameraPtr pCam, INodeMap & nodeMap, INodeMap & nodeMapTLDevice
         exposureMode->SetIntValue(exposureMode->GetEntryByName("Timed")->GetValue());
 
         Spinnaker::GenApi::CFloatPtr exposureTime = pCam->GetNodeMap().GetNode("ExposureTime");
-        exposureTime->SetValue(1300);
+        exposureTime->SetValue(1305);
 
         // Retrieve integer value from entry node
         int64_t acquisitionModeContinuous = ptrAcquisitionModeContinuous->GetValue();

src/sub_vision/src/mock_camera.cpp

@@ -20,6 +20,7 @@ int main(int argc, char** argv)
     while (node.ok()) 
     {
         ROS_INFO("Published image.");
+        ros::Duration(0.5).sleep();
         pub.publish(msg);
         ros::spinOnce();
     }