Merge pull request #41 from BCLab-UNM/feature-dont-get-stuck-in-center

Feature: don't get stuck in the centre

Author: gmfricke

simulation/models/collection_disk/materials/textures/collection_disk.png

@@ -23,6 +23,7 @@ include_directories(
 
 add_executable(
   behaviours
+  src/Tag.cpp
   src/ObstacleController.cpp 
   src/PickUpController.cpp
   src/DropOffController.cpp

src/behaviours/CMakeLists.txt

@@ -288,7 +288,7 @@ void DropOffController::Reset() {
 
 }
 
-void DropOffController::SetTargetData(vector<TagPoint> tags) {
+void DropOffController::SetTargetData(vector<Tag> tags) {
   countRight = 0;
   countLeft = 0;
 
@@ -298,10 +298,10 @@ void DropOffController::SetTargetData(vector<TagPoint> tags) {
 
       // this loop is to get the number of center tags
       for (int i = 0; i < tags.size(); i++) {
-        if (tags[i].id == 256) {
+        if (tags[i].getID() == 256) {
 
           // checks if tag is on the right or left side of the image
-          if (tags[i].x + cameraOffsetCorrection > 0) {
+          if (tags[i].getPositionX() + cameraOffsetCorrection > 0) {
             countRight++;
 
           } else {

src/behaviours/src/DropOffController.cpp

@@ -3,7 +3,7 @@
 #define HEADERFILE_H
 
 #include "Controller.h"
-#include "TagPoint.h"
+#include "Tag.h"
 #include <math.h>
 
 class DropOffController : virtual Controller
@@ -22,12 +22,12 @@ class DropOffController : virtual Controller
   void SetCurrentLocation(Point current);
   void SetTargetPickedUp();
   void SetBlockBlockingUltrasound(bool blockBlock);
-  void SetTargetData(vector<TagPoint> tags);
+  void SetTargetData(vector<Tag> tags);
   bool HasTarget() {return targetHeld;}
 
   float GetSpinner() {return spinner;}
 
-  void UpdateData(vector<TagPoint> tags);
+  void UpdateData(vector<Tag> tags);
 
   void SetCurrentTimeInMilliSecs( long int time );
 

src/behaviours/src/DropOffController.h

@@ -238,24 +238,24 @@ void LogicController::controllerInterconnect() {
 
     //obstacle needs to know if the center ultrasound should be ignored
     if(pickUpController.GetIgnoreCenter()) {
-      obstacleController.SetIgnoreCenter();
+      obstacleController.setIgnoreCenterSonar();
     }
 
     //pickup controller annouces it has pickedup a target
     if(pickUpController.GetTargetHeld()) {
       dropOffController.SetTargetPickedUp();
-      obstacleController.SetTargetHeld();
+      obstacleController.setTargetHeld();
       searchController.SetSuccesfullPickup();
     }
   }
 
   //ask if drop off has released the target from the claws yet
   if (!dropOffController.HasTarget()) {
-    obstacleController.SetTargetHeldClear();
+    obstacleController.setTargetHeldClear();
   }
 
   //obstacle controller is running driveController needs to clear its waypoints
-  if(obstacleController.GetShouldClearWaypoints()) {
+  if(obstacleController.getShouldClearWaypoints()) {
     driveController.Reset();
   }
 }
@@ -264,7 +264,7 @@ void LogicController::controllerInterconnect() {
 void LogicController::SetPositionData(Point currentLocation) {
   searchController.SetCurrentLocation(currentLocation);
   dropOffController.SetCurrentLocation(currentLocation);
-  obstacleController.SetCurrentLocation(currentLocation);
+  obstacleController.setCurrentLocation(currentLocation);
   driveController.SetCurrentLocation(currentLocation);
 }
 
@@ -282,15 +282,15 @@ void LogicController::SetMapVelocityData(float linearVelocity, float angularVelo
 
 }
 
-void LogicController::SetAprilTags(vector<TagPoint> tags) {
+void LogicController::SetAprilTags(vector<Tag> tags) {
   pickUpController.SetTagData(tags);
-  obstacleController.SetTagData(tags);
+  obstacleController.setTagData(tags);
   dropOffController.SetTargetData(tags);
 }
 
 void LogicController::SetSonarData(float left, float center, float right) {
   pickUpController.SetSonarData(center);
-  obstacleController.SetSonarData(left,center,right);
+  obstacleController.setSonarData(left,center,right);
 }
 
 // Called once by RosAdapter in guarded init
@@ -319,5 +319,5 @@ void LogicController::SetCurrentTimeInMilliSecs( long int time )
   current_time = time;
   dropOffController.SetCurrentTimeInMilliSecs( time );
   pickUpController.SetCurrentTimeInMilliSecs( time );
-  obstacleController.SetCurrentTimeInMilliSecs( time );
+  obstacleController.setCurrentTimeInMilliSecs( time );
 }

src/behaviours/src/LogicController.cpp

@@ -37,7 +37,7 @@ class LogicController : virtual Controller
   bool ShouldInterrupt() override;
   bool HasWork() override;
 
-  void SetAprilTags(vector<TagPoint> tags);
+  void SetAprilTags(vector<Tag> tags);
   void SetSonarData(float left, float center, float right);
   void SetPositionData(Point currentLocation);
   void SetMapPositionData(Point currentLocationMap);

src/behaviours/src/LogicController.h

@@ -15,20 +15,32 @@ void ObstacleController::Reset() {
   delay = current_time;
 }
 
-Result ObstacleController::DoWork() {
-
-  clearWaypoints = true;
-  set_waypoint = true;
-  result.PIDMode = CONST_PID;
+// Avoid crashing into objects detected by the ultraound
+void ObstacleController::avoidObstacle() {
+  
+    //obstacle on right side
+    if (right < 0.8 || center < 0.8 || left < 0.8) {
+      result.type = precisionDriving;
 
-  if(centerSeen){
+      result.pd.cmdAngular = -K_angular;
 
+      result.pd.setPointVel = 0.0;
+      result.pd.cmdVel = 0.0;
+      result.pd.setPointYaw = 0;
+    }
+}
 
+// A collection zone was seen in front of the rover and we are not carrying a target
+// so avoid running over the collection zone and possibly pushing cubes out.
+void ObstacleController::avoidCollectionZone() {
+  
     result.type = precisionDriving;
 
     result.pd.cmdVel = 0.0;
 
-    if(countLeft < countRight) {
+    // Decide which side of the rover sees the most april tags and turn away
+    // from that side
+    if(count_left_collection_zone_tags < count_right_collection_zone_tags) {
       result.pd.cmdAngular = K_angular;
     } else {
       result.pd.cmdAngular = -K_angular;
@@ -37,21 +49,21 @@ Result ObstacleController::DoWork() {
     result.pd.setPointVel = 0.0;
     result.pd.cmdVel = 0.0;
     result.pd.setPointYaw = 0;
-
-  }
-  else {
+}
 
 
-    //obstacle on right side
-    if (right < 0.8 || center < 0.8 || left < 0.8) {
-      result.type = precisionDriving;
+Result ObstacleController::DoWork() {
 
-      result.pd.cmdAngular = -K_angular;
+  clearWaypoints = true;
+  set_waypoint = true;
+  result.PIDMode = CONST_PID;
 
-      result.pd.setPointVel = 0.0;
-      result.pd.cmdVel = 0.0;
-      result.pd.setPointYaw = 0;
-    }
+  // The obstacle is an april tag marking the collection zone
+  if(collection_zone_seen){
+    avoidCollectionZone();
+  }
+  else {
+    avoidObstacle();
   }
 
   if (can_set_waypoint) {
@@ -73,15 +85,15 @@ Result ObstacleController::DoWork() {
 }
 
 
-void ObstacleController::SetSonarData(float sonarleft, float sonarcenter, float sonarright) {
+void ObstacleController::setSonarData(float sonarleft, float sonarcenter, float sonarright) {
   left = sonarleft;
   right = sonarright;
   center = sonarcenter;
 
   ProcessData();
 }
 
-void ObstacleController::SetCurrentLocation(Point currentLocation) {
+void ObstacleController::setCurrentLocation(Point currentLocation) {
   this->currentLocation = currentLocation;
 }
 
@@ -91,7 +103,7 @@ void ObstacleController::ProcessData() {
   long int Tdifference = current_time - timeSinceTags;
   float Td = Tdifference/1e3;
   if (Td >= 0.5) {
-    centerSeen = false;
+    collection_zone_seen = false;
     phys= false;
     if (!obstacleAvoided)
     {
@@ -100,9 +112,9 @@ void ObstacleController::ProcessData() {
   }
 
   //Process sonar info
-  if(ignoreCenter){
-    if(center > reactivateCenterThreshold){
-      ignoreCenter = false;
+  if(ignore_center_sonar){
+    if(center > reactivate_center_sonar_threshold){
+      ignore_center_sonar = false;
     }
     else{
       center = 3;
@@ -124,7 +136,7 @@ void ObstacleController::ProcessData() {
   }
 
 
-  if (centerSeen || phys)
+  if (collection_zone_seen || phys)
   {
     obstacleDetected = true;
     obstacleAvoided = false;
@@ -136,29 +148,49 @@ void ObstacleController::ProcessData() {
   }
 }
 
-void ObstacleController::SetTagData(vector<TagPoint> tags){
-  float cameraOffsetCorrection = 0.020; //meters;
-  centerSeen = false;
-  countLeft = 0;
-  countRight = 0;
+// Report April tags seen by the rovers camera so it can avoid
+// the collection zone
+// Added relative pose information so we know whether the
+// top of the AprilTag is pointing towards the rover or away.
+// If the top of the tags are away from the rover then treat them as obstacles. 
+void ObstacleController::setTagData(vector<Tag> tags){
+  collection_zone_seen = false;
+  count_left_collection_zone_tags = 0;
+  count_right_collection_zone_tags = 0;
 
   // this loop is to get the number of center tags
   if (!targetHeld) {
     for (int i = 0; i < tags.size(); i++) {
-      if (tags[i].id == 256) {
-
-        // checks if tag is on the right or left side of the image
-        if (tags[i].x + cameraOffsetCorrection > 0) {
-          countRight++;
+      if (tags[i].getID() == 256) {
 
-        } else {
-          countLeft++;
-        }
-        centerSeen = true;
+	collection_zone_seen = checkForCollectionZoneTags( tags );
         timeSinceTags = current_time;
       }
     }
   }
+}
+
+bool ObstacleController::checkForCollectionZoneTags( vector<Tag> tags ) {
+
+  for ( auto & tag : tags ) { 
+
+    // Check the orientation of the tag. If we are outside the collection zone the yaw will be positive so treat the collection zone as an obstacle. If the yaw is negative the robot is inside the collection zone and the boundary should not be treated as an obstacle. This allows the robot to leave the collection zone after dropping off a target.
+    if ( tag.calcYaw() > 0 ) 
+      {
+	// checks if tag is on the right or left side of the image
+	if (tag.getPositionX() + camera_offset_correction > 0) {
+	  count_right_collection_zone_tags++;
+	  
+	} else {
+	  count_left_collection_zone_tags++;
+	}
+      }
+    
+  }
+
+
+  // Did any tags indicate that the robot is inside the collection zone?
+  return count_left_collection_zone_tags + count_right_collection_zone_tags > 0;
 
 }
 
@@ -191,16 +223,17 @@ bool ObstacleController::HasWork() {
   return !obstacleAvoided;
 }
 
-void ObstacleController::SetIgnoreCenter(){
-  ignoreCenter = true; //ignore center ultrasound
+//ignore center ultrasound
+void ObstacleController::setIgnoreCenterSonar(){
+  ignore_center_sonar = true; 
 }
 
-void ObstacleController::SetCurrentTimeInMilliSecs( long int time )
+void ObstacleController::setCurrentTimeInMilliSecs( long int time )
 {
   current_time = time;
 }
 
-void ObstacleController::SetTargetHeld() {
+void ObstacleController::setTargetHeld() {
   targetHeld = true;