Outdoor testing at wpi, day 1.

computer_vision/data/calibration/cameraCalibration.csv

@@ -1,6 +1,6 @@
-0.0139626
+0.00523599
+-0.00953028
 -0.00953028
--0.0020718
 1965
 2069
 1041
@@ -10,6 +10,6 @@
 1404
 872
 120
-58
+53
 148
-120
+102

robot_control/include/robot_control/chestnut_ridge_rois.h

@@ -1,8 +1,8 @@
 #ifndef CHESTNUT_RIDGE_ROIS_H
 #define CHESTNUT_RIDGE_ROIS_H
 
-ROI.e = 114.8 - satMapStartE;
-ROI.s = 34.7 - satMapStartS;
+ROI.eMap = 114.8;
+ROI.sMap = 34.7;
 ROI.sampleProb = 1.0;
 ROI.sampleSig = 10.0;
 ROI.radialAxis = 7.5;
@@ -19,8 +19,8 @@ ROI.redProb = 1.0;
 ROI.orangeProb = 1.0;
 ROI.yellowProb = 1.0;
 regionsOfInterest.push_back(ROI);
-ROI.e = 141.0 - satMapStartE;
-ROI.s = 48.0 - satMapStartS;
+ROI.eMap = 141.0;
+ROI.sMap = 48.0;
 ROI.sampleProb = 0.2;
 ROI.sampleSig = 0.5;
 ROI.radialAxis = 7.5;
@@ -37,8 +37,8 @@ ROI.redProb = 1.0;
 ROI.orangeProb = 1.0;
 ROI.yellowProb = 1.0;
 regionsOfInterest.push_back(ROI);
-ROI.e = 114.0 - satMapStartE;
-ROI.s = 60.0 - satMapStartS;
+ROI.eMap = 114.0;
+ROI.sMap = 60.0;
 ROI.sampleProb = 0.2;
 ROI.sampleSig = 0.5;
 ROI.radialAxis = 7.5;
@@ -55,8 +55,8 @@ ROI.redProb = 1.0;
 ROI.orangeProb = 1.0;
 ROI.yellowProb = 1.0;
 regionsOfInterest.push_back(ROI);
-ROI.e = 140.0 - satMapStartE;
-ROI.s = 98.0 - satMapStartS;
+ROI.eMap = 140.0;
+ROI.sMap = 98.0;
 ROI.sampleProb = 0.2;
 ROI.sampleSig = 0.5;
 ROI.radialAxis = 7.5;
@@ -73,8 +73,8 @@ ROI.redProb = 1.0;
 ROI.orangeProb = 1.0;
 ROI.yellowProb = 1.0;
 regionsOfInterest.push_back(ROI);
-ROI.e = 37.5 - satMapStartE;
-ROI.s = 28.2 - satMapStartS;
+ROI.eMap = 37.5;
+ROI.sMap = 28.2;
 ROI.sampleProb = 0.5;
 ROI.sampleSig = 1.0;
 ROI.radialAxis = 7.5;

robot_control/include/robot_control/map_manager.h

@@ -81,6 +81,7 @@ class MapManager
 	void calculateGlobalMapSize();
 	void initializeGlobalMap();
 	void cutOutGlobalSubMap();
+	void computeROIEastSouth();
 	// Members
 	ros::NodeHandle nh;
 	ros::ServiceServer roiServ;

robot_control/include/robot_control/test_field_map.h

@@ -4,12 +4,12 @@
 const int driveabilityNumRows = 144;
 const int driveabilityNumCols = 139;
 const float driveabilityMapRes = 1.0; // m
-const float satMapStartE1 = 95.0; // m
-const float satMapStartS1 = 75.0; // m
+const float satMapStartE1 = 92.0; // m
+const float satMapStartS1 = 57.0; // m
 const float satMapStartE2 = 95.0; // m
 const float satMapStartS2 = 75.0; // m
-const float satMapStartE3 = 95.0; // m
-const float satMapStartS3 = 75.0; // m
+const float satMapStartE3 = 92.0; // m
+const float satMapStartS3 = 57.0; // m
 float driveabilityMap[driveabilityNumRows][driveabilityNumCols] =
 {
 {100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100},

robot_control/include/robot_control/test_field_rois.h

@@ -1,13 +1,13 @@
 #ifndef TEST_FIELD_ROIS_H
 #define TEST_FIELD_ROIS_H
 
-ROI.e = 105.0 - satMapStartE; // precached (0)
-ROI.s = 35.0 - satMapStartS;
+ROI.eMap = 105.0; // precached (0)
+ROI.sMap = 35.0;
 ROI.sampleProb = 1.0;
 ROI.sampleSig = 10.0;
-ROI.radialAxis = 9.5;
-ROI.tangentialAxis = 9.5;
-ROI.allocatedTime = 600.0;
+ROI.radialAxis = 7.5;
+ROI.tangentialAxis = 7.5;
+ROI.allocatedTime = 210.0;
 ROI.highRisk = 0;
 ROI.hardLockout = false;
 ROI.roiGroup = 0;
@@ -19,12 +19,12 @@ ROI.redProb = 0.0;
 ROI.orangeProb = 0.0;
 ROI.yellowProb = 0.0;
 regionsOfInterest.push_back(ROI);
-ROI.e = 95.0 - satMapStartE; // 1
-ROI.s = 90.0 - satMapStartS;
+ROI.eMap = 95.0; // 1
+ROI.sMap = 90.0;
 ROI.sampleProb = 0.5;
 ROI.sampleSig = 1.0;
-ROI.radialAxis = 9.5;
-ROI.tangentialAxis = 9.5;
+ROI.radialAxis = 5.5;
+ROI.tangentialAxis = 5.5;
 ROI.allocatedTime = 210.0;
 ROI.highRisk = 0;
 ROI.hardLockout = false;

robot_control/include/robot_control/wpi_rois.h

@@ -1,8 +1,8 @@
 #ifndef WPI_ROIS_H
 #define WPI_ROIS_H
 
-ROI.e = 96.9 - satMapStartE; // precached (0)
-ROI.s = 26.4 - satMapStartS;
+ROI.eMap = 96.9; // precached (0)
+ROI.sMap = 26.4;
 ROI.sampleProb = 1.0;
 ROI.sampleSig = 10.0;
 ROI.radialAxis = 9.5;
@@ -19,8 +19,8 @@ ROI.redProb = 0.0;
 ROI.orangeProb = 0.0;
 ROI.yellowProb = 0.0;
 regionsOfInterest.push_back(ROI);
-ROI.e = 18.5 - satMapStartE; // 1
-ROI.s = 25.0 - satMapStartS;
+ROI.eMap = 18.5; // 1
+ROI.sMap = 25.0;
 ROI.sampleProb = 0.5;
 ROI.sampleSig = 1.0;
 ROI.radialAxis = 9.5;
@@ -37,8 +37,8 @@ ROI.redProb = 0.07;
 ROI.orangeProb = 0.07;
 ROI.yellowProb = 0.07;
 regionsOfInterest.push_back(ROI);
-ROI.e = 16.9 - satMapStartE; // 2
-ROI.s = 49.2 - satMapStartS;
+ROI.eMap = 16.9; // 2
+ROI.sMap = 49.2;
 ROI.sampleProb = 0.5;
 ROI.sampleSig = 1.0;
 ROI.radialAxis = 7.5;
@@ -55,8 +55,8 @@ ROI.redProb = 0.07;
 ROI.orangeProb = 0.07;
 ROI.yellowProb = 0.07;
 regionsOfInterest.push_back(ROI);
-ROI.e = 39.8 - satMapStartE; // 3
-ROI.s = 16.4 - satMapStartS;
+ROI.eMap = 39.8; // 3
+ROI.sMap = 16.4;
 ROI.sampleProb = 0.0;
 ROI.sampleSig = 1.0;
 ROI.radialAxis = 7.5;
@@ -73,8 +73,8 @@ ROI.redProb = 0.07;
 ROI.orangeProb = 0.07;
 ROI.yellowProb = 0.07;
 regionsOfInterest.push_back(ROI);
-ROI.e = 75.9 - satMapStartE; // 4
-ROI.s = 20.9 - satMapStartS;
+ROI.eMap = 75.9; // 4
+ROI.sMap = 20.9;
 ROI.sampleProb = 0.0;
 ROI.sampleSig = 1.0;
 ROI.radialAxis = 5.5;
@@ -91,8 +91,8 @@ ROI.redProb = 0.07;
 ROI.orangeProb = 0.07;
 ROI.yellowProb = 0.07;
 regionsOfInterest.push_back(ROI);
-ROI.e = 95.3 - satMapStartE; // 5
-ROI.s = 53.8 - satMapStartS;
+ROI.eMap = 95.3; // 5
+ROI.sMap = 53.8;
 ROI.sampleProb = 0.3;
 ROI.sampleSig = 1.0;
 ROI.radialAxis = 7.5;
@@ -109,8 +109,8 @@ ROI.redProb = 0.07;
 ROI.orangeProb = 0.07;
 ROI.yellowProb = 0.0;
 regionsOfInterest.push_back(ROI);
-ROI.e = 96.6 - satMapStartE; // 6
-ROI.s = 95.6 - satMapStartS;
+ROI.eMap = 96.6; // 6
+ROI.sMap = 95.6;
 ROI.sampleProb = 0.5;
 ROI.sampleSig = 1.0;
 ROI.radialAxis = 5.5;
@@ -127,8 +127,8 @@ ROI.redProb = 0.07;
 ROI.orangeProb = 0.07;
 ROI.yellowProb = 0.07;
 regionsOfInterest.push_back(ROI);
-ROI.e = 138.5 - satMapStartE; // 7
-ROI.s = 51.6 - satMapStartS;
+ROI.eMap = 138.5; // 7
+ROI.sMap = 51.6;
 ROI.sampleProb = 0.0;
 ROI.sampleSig = 1.0;
 ROI.radialAxis = 5.5;
@@ -145,8 +145,8 @@ ROI.redProb = 0.07;
 ROI.orangeProb = 0.07;
 ROI.yellowProb = 0.07;
 regionsOfInterest.push_back(ROI);
-ROI.e = 160.9 - satMapStartE; // 8
-ROI.s = 83.2 - satMapStartS;
+ROI.eMap = 160.9; // 8
+ROI.sMap = 83.2;
 ROI.sampleProb = 0.3;
 ROI.sampleSig = 1.0;
 ROI.radialAxis = 5.5;
@@ -163,8 +163,8 @@ ROI.redProb = 0.07;
 ROI.orangeProb = 0.07;
 ROI.yellowProb = 0.07;
 regionsOfInterest.push_back(ROI);
-ROI.e = 154.8 - satMapStartE; // 9
-ROI.s = 106.8 - satMapStartS;
+ROI.eMap = 154.8; // 9
+ROI.sMap = 106.8;
 ROI.sampleProb = 0.5;
 ROI.sampleSig = 1.0;
 ROI.radialAxis = 7.5;
@@ -181,8 +181,8 @@ ROI.redProb = 0.07;
 ROI.orangeProb = 0.07;
 ROI.yellowProb = 0.07;
 regionsOfInterest.push_back(ROI);
-ROI.e = 191.2 - satMapStartE; // 10
-ROI.s = 115.7 - satMapStartS;
+ROI.eMap = 191.2; // 10
+ROI.sMap = 115.7;
 ROI.sampleProb = 0.5;
 ROI.sampleSig = 1.0;
 ROI.radialAxis = 7.5;
@@ -199,8 +199,8 @@ ROI.redProb = 0.07;
 ROI.orangeProb = 0.07;
 ROI.yellowProb = 0.07;
 regionsOfInterest.push_back(ROI);
-ROI.e = 197.1 - satMapStartE; // 11
-ROI.s = 142.3 - satMapStartS;
+ROI.eMap = 197.1; // 11
+ROI.sMap = 142.3;
 ROI.sampleProb = 0.5;
 ROI.sampleSig = 1.0;
 ROI.radialAxis = 5.5;
@@ -217,8 +217,8 @@ ROI.redProb = 0.07;
 ROI.orangeProb = 0.07;
 ROI.yellowProb = 0.07;
 regionsOfInterest.push_back(ROI);
-ROI.e = 229.6 - satMapStartE; // 12
-ROI.s = 105.9 - satMapStartS;
+ROI.eMap = 229.6; // 12
+ROI.sMap = 105.9;
 ROI.sampleProb = 0.5;
 ROI.sampleSig = 1.0;
 ROI.radialAxis = 9.5;
@@ -235,8 +235,8 @@ ROI.redProb = 0.07;
 ROI.orangeProb = 0.07;
 ROI.yellowProb = 0.07;
 regionsOfInterest.push_back(ROI);
-ROI.e = 258.2 - satMapStartE; // 13
-ROI.s = 71.9 - satMapStartS;
+ROI.eMap = 258.2; // 13
+ROI.sMap = 71.9;
 ROI.sampleProb = 0.5;
 ROI.sampleSig = 1.0;
 ROI.radialAxis = 7.5;
@@ -253,8 +253,8 @@ ROI.redProb = 0.07;
 ROI.orangeProb = 0.07;
 ROI.yellowProb = 0.07;
 regionsOfInterest.push_back(ROI);
-ROI.e = 265.4 - satMapStartE; // 14
-ROI.s = 52.2 - satMapStartS;
+ROI.eMap = 265.4; // 14
+ROI.sMap = 52.2;
 ROI.sampleProb = 0.5;
 ROI.sampleSig = 1.0;
 ROI.radialAxis = 7.5;
@@ -271,8 +271,8 @@ ROI.redProb = 0.07;
 ROI.orangeProb = 0.07;
 ROI.yellowProb = 0.07;
 regionsOfInterest.push_back(ROI);
-ROI.e = 288.3 - satMapStartE; // 15
-ROI.s = 39.0 - satMapStartS;
+ROI.eMap = 288.3; // 15
+ROI.sMap = 39.0;
 ROI.sampleProb = 0.5;
 ROI.sampleSig = 1.0;
 ROI.radialAxis = 9.5;

robot_control/msg/ROI.msg

@@ -1,3 +1,5 @@
+float32 eMap
+float32 sMap
 float32 e
 float32 s
 float32 x

robot_control/src/deposit_approach.cpp

@@ -90,6 +90,7 @@ bool DepositApproach::runProc()
                 biasRemovalTimedOut = false;
                 performBiasRemoval = false;
                 sendDriveGlobal(false, false, 0.0);
+                sendWait(lidarUpdateWaitTime, false);
                 step = _align;
             }
             else step = _biasRemoval;

robot_control/src/drive_halt.cpp

@@ -15,7 +15,7 @@ void DriveHalt::init()
     posError_ = 0.0;
     vPrev_ = robotStatus.velocity;
     prevTime_ = ros::Time::now().toSec();
-    if(fabs(robotStatus.pitchAngle)>minTiltForHold_) state_ = _waitingForStop;
+    if(fabs(robotStatus.pitchAngle)>minTiltForHold_) {state_ = _waitingForStop; ROS_WARN("tilt at halt = %f",robotStatus.pitchAngle);}
     else state_ = _noHold;
 }
 
@@ -24,6 +24,7 @@ int DriveHalt::run()
     switch(state_)
     {
     case _noHold:
+    	ROS_INFO("_noHold");
         robotOutputs.flMotorSpeed = 0;
         robotOutputs.mlMotorSpeed = 0;
         robotOutputs.blMotorSpeed = 0;
@@ -35,6 +36,7 @@ int DriveHalt::run()
         state_ = _noHold;
         break;
     case _waitingForStop:
+    	ROS_INFO("_waitingForStop");
         robotOutputs.flMotorSpeed = 0;
         robotOutputs.mlMotorSpeed = 0;
         robotOutputs.blMotorSpeed = 0;
@@ -47,8 +49,11 @@ int DriveHalt::run()
         else stopCounts_ = 0;
         if(stopCounts_>=stopCountsThreshold_) state_ = _holding;
         else state_ = _waitingForStop;
+        prevTime_ = ros::Time::now().toSec();
+        vPrev_ = robotStatus.velocity;
         break;
     case _holding:
+    	ROS_INFO("_holding");
         if(fabs(posError_>=maxErrorThreshold_))
         {
             posError_ = 0.0;
@@ -80,5 +85,6 @@ int DriveHalt::run()
         state_ = _holding;
         break;
     }
+    ROS_INFO("posError_ = %f",posError_);
 	return 1;
 }

robot_control/src/map_manager.cpp

@@ -65,6 +65,8 @@ MapManager::MapManager()
 #include <robot_control/chestnut_ridge_rois.h>
 #endif // CHESTNUT_RIDGE
 
+    computeROIEastSouth();
+
 	// ***********************************
     /*globalMapPub = nh.advertise<grid_map_msgs::GridMap>("control/mapmanager/globalmap",1);
     globalMap.setFrameId("map");
@@ -521,6 +523,7 @@ bool MapManager::setStartingPlatformCallback(messages::SetStartingPlatform::Requ
         setStartingPlatform();
         calculateGlobalMapSize();
         initializeGlobalMap();
+        computeROIEastSouth();
         writeSatMapIntoGlobalMap();
         writeKeyframesIntoGlobalMap();
     }
@@ -1050,3 +1053,12 @@ void MapManager::cutOutGlobalSubMap()
 {
 
 }
+
+void MapManager::computeROIEastSouth()
+{
+	for(int i=0; i<regionsOfInterest.size(); i++)
+	{
+		regionsOfInterest.at(i).e = regionsOfInterest.at(i).eMap - satMapStartE;
+		regionsOfInterest.at(i).s = regionsOfInterest.at(i).sMap - satMapStartS;
+	}
+}