Going back to Obstacle Avoidance within behaviors

demiurge-project · Aug 31, 2018 · f905b36 · f905b36
1 parent e41d2a4
commit f905b36
Show file tree

Hide file tree

Showing 17 changed files with 54 additions and 39 deletions.
diff --git a/src/core/AutoMoDeBehaviorTree.cpp b/src/core/AutoMoDeBehaviorTree.cpp
@@ -62,7 +62,7 @@ namespace argos {
 		 * However, not all conditions have been tested if the pervious RUNNING branch is not the first (left) branch
 		 */
 		if ((m_eTreeStatus == Node::SUCCESS) || (m_eTreeStatus == Node::FAILURE)) {
-			//LOG << "STOP" << std::endl;
+			LOG << "STOP" << std::endl;
 			m_pcRobotDAO->SetWheelsVelocity(0.0, 0.0);
 		}
 	}

diff --git a/src/core/bt/Action.cpp b/src/core/bt/Action.cpp
@@ -48,7 +48,7 @@ namespace argos {
 			eCurrentState = Node::SUCCESS;
 		} else {
 			//LOG << "---> Running" << std::endl;
-			//LOG << "[" << m_pcBehaviour->LogRobotIdientifier() << "] " << m_strLabel << m_strBranchId  << " (" << m_pcBehaviour->GetLabel() << ")" << std::endl;
+			//LOG << "[" << m_pcBehaviour->LogRobotIdientifier() << "] " << m_strLabel << m_strBranchId  << " (" << m_pcBehaviour->GetLabel() << ") " << m_pcBehaviour->GetSuccessProbability() << std::endl;
 			m_pcBehaviour->ControlStep();
 			eCurrentState = Node::RUNNING;
 		}

diff --git a/src/modules/AutoMoDeBehaviour.cpp b/src/modules/AutoMoDeBehaviour.cpp
@@ -164,4 +164,11 @@ namespace argos {
 		return m_pcRobotDAO->GetRandomNumberGenerator()->Bernoulli(f_probability);
 	}
 
+	/****************************************/
+	/****************************************/
+
+	Real AutoMoDeBehaviour::GetSuccessProbability() const {
+		return m_fSuccessProbabilityParameter;
+	}
+
 }
diff --git a/src/modules/AutoMoDeBehaviour.h b/src/modules/AutoMoDeBehaviour.h
@@ -64,6 +64,8 @@ namespace argos {
 			 */
       EpuckDAO* m_pcRobotDAO;
 
+			Real m_fSuccessProbabilityParameter;
+
 		public:
 
 		 virtual ~AutoMoDeBehaviour();
@@ -191,9 +193,11 @@ namespace argos {
 
 			bool EvaluateBernoulliProbability(const Real& f_probability) const;
 
+			Real GetSuccessProbability() const;
+
 			/* Debug functions */
 
-		 UInt32 LogRobotIdientifier() {
+		  UInt32 LogRobotIdientifier() {
 				return m_pcRobotDAO->GetRobotIdentifier();
 			};
 	};

diff --git a/src/modules/AutoMoDeBehaviourAntiPhototaxis.cpp b/src/modules/AutoMoDeBehaviourAntiPhototaxis.cpp
@@ -55,14 +55,17 @@ namespace argos {
 
 		sLightVector = CVector2(cLightReading.Value, cLightReading.Angle);
 
-	// LOG << cLightReading.Value << " " << cLightReading.Angle << std::endl;
-	// LOG << sLightVector << std::endl;
-
-		if (sLightVector.Length() < 0.1) {
-			sLightVector = CVector2(1.0, CRadians::ZERO);
+		// Obstacle avoidance
+		CVector2 sDirectionVector(0,CRadians::ZERO);
+		CVector2 sProxVector(0,CRadians::ZERO);
+		sProxVector = CVector2(m_pcRobotDAO->GetProximityReading().Value, m_pcRobotDAO->GetProximityReading().Angle);
+		sDirectionVector = -sLightVector - 5*sProxVector;
+
+		if (sDirectionVector.Length() < 0.1) {
+			sDirectionVector = CVector2(1.0, CRadians::ZERO);
 		}
 
-		m_pcRobotDAO->SetWheelsVelocity(ComputeWheelsVelocityFromVector(-sLightVector));
+		m_pcRobotDAO->SetWheelsVelocity(ComputeWheelsVelocityFromVector(sDirectionVector));
 
 		m_bLocked = false;
 	}
@@ -108,6 +111,6 @@ namespace argos {
 	/****************************************/
 
 	bool AutoMoDeBehaviourAntiPhototaxis::Failed() {
-		return (ObstacleInFront() || !LightPerceived());
+		return false; //(ObstacleInFront() || !LightPerceived());
 	}
 }
diff --git a/src/modules/AutoMoDeBehaviourAntiPhototaxis.h b/src/modules/AutoMoDeBehaviourAntiPhototaxis.h
@@ -31,7 +31,6 @@ namespace argos {
 			virtual bool Failed();
 
 		private:
-			Real m_fSuccessProbabilityParameter;
 
 	};
 }

diff --git a/src/modules/AutoMoDeBehaviourAttraction.cpp b/src/modules/AutoMoDeBehaviourAttraction.cpp
@@ -56,16 +56,17 @@ namespace argos {
 			sRabVector = CVector2(cRabReading.Range, cRabReading.Bearing);
 		}
 
-		// LOG << "[" << m_pcRobotDAO->GetRobotIdentifier() << "] " << cRabReading.Range << " " << cRabReading.Bearing << std::endl;
-		// LOG << "[" << m_pcRobotDAO->GetRobotIdentifier() << "] " << sRabVector << std::endl;
-
-		if (sRabVector.Length() < 0.1) {
-			sRabVector = CVector2(1.0, CRadians::ZERO);
+		// Obstacle avoidance
+		CVector2 sDirectionVector(0,CRadians::ZERO);
+		CVector2 sProxVector(0,CRadians::ZERO);
+		sProxVector = CVector2(m_pcRobotDAO->GetProximityReading().Value, m_pcRobotDAO->GetProximityReading().Angle);
+		sDirectionVector = sRabVector - 6*sProxVector;
+
+		if (sDirectionVector.Length() < 0.1) {
+			sDirectionVector = CVector2(1.0, CRadians::ZERO);
 		}
 
-		LOG << "[" << m_pcRobotDAO->GetRobotIdentifier() << "] " << sRabVector << std::endl;
-
-		m_pcRobotDAO->SetWheelsVelocity(ComputeWheelsVelocityFromVector(sRabVector));
+		m_pcRobotDAO->SetWheelsVelocity(ComputeWheelsVelocityFromVector(sDirectionVector));
 
 		m_bLocked = false;
 	}
@@ -121,6 +122,6 @@ namespace argos {
 	/****************************************/
 
 	bool AutoMoDeBehaviourAttraction::Failed() {
-		return (ObstacleInFront() || (m_pcRobotDAO->GetNumberNeighbors() == 0));
+		return false; // (ObstacleInFront() || (m_pcRobotDAO->GetNumberNeighbors() == 0));
 	}
 }
diff --git a/src/modules/AutoMoDeBehaviourAttraction.h b/src/modules/AutoMoDeBehaviourAttraction.h
@@ -32,7 +32,6 @@ namespace argos {
 
 		private:
 			UInt8 m_unAttractionParameter;
-			Real m_fSuccessProbabilityParameter;
 	};
 }
 

diff --git a/src/modules/AutoMoDeBehaviourCurve.h b/src/modules/AutoMoDeBehaviourCurve.h
@@ -32,7 +32,6 @@ namespace argos {
 			virtual bool Failed();
 
 		private:
-			Real m_fSuccessProbabilityParameter;
 			Real m_fRadiusParameter;
 
 			enum CurveState {

diff --git a/src/modules/AutoMoDeBehaviourExploration.h b/src/modules/AutoMoDeBehaviourExploration.h
@@ -48,7 +48,6 @@ namespace argos {
 
 			Real m_fProximityThreshold;
 			CRange<UInt32> m_cRandomStepsRange;
-			Real m_fSuccessProbabilityParameter;
 	};
 }
 

diff --git a/src/modules/AutoMoDeBehaviourPhototaxis.cpp b/src/modules/AutoMoDeBehaviourPhototaxis.cpp
@@ -54,14 +54,17 @@ namespace argos {
 
 		sLightVector = CVector2(cLightReading.Value, cLightReading.Angle);
 
-		// LOG << cLightReading.Value << " " << cLightReading.Angle << std::endl;
-		// LOG << sLightVector << std::endl;
-
-		if (sLightVector.Length() < 0.1) {
-			sLightVector = CVector2(1.0, CRadians::ZERO);
+		// Obstacle avoidance
+		CVector2 sDirectionVector(0,CRadians::ZERO);
+		CVector2 sProxVector(0,CRadians::ZERO);
+		sProxVector = CVector2(m_pcRobotDAO->GetProximityReading().Value, m_pcRobotDAO->GetProximityReading().Angle);
+		sDirectionVector = sLightVector - 5*sProxVector;;
+
+		if (sDirectionVector.Length() < 0.1) {
+			sDirectionVector = CVector2(1.0, CRadians::ZERO);
 		}
 
-		m_pcRobotDAO->SetWheelsVelocity(ComputeWheelsVelocityFromVector(sLightVector));
+		m_pcRobotDAO->SetWheelsVelocity(ComputeWheelsVelocityFromVector(sDirectionVector));
 
 		m_bLocked = false;
 	}
@@ -106,6 +109,6 @@ namespace argos {
 	/****************************************/
 
 	bool AutoMoDeBehaviourPhototaxis::Failed() {
-		return (ObstacleInFront() || !LightPerceived());
+		return false; //(ObstacleInFront() || !LightPerceived());
 	}
 }
diff --git a/src/modules/AutoMoDeBehaviourPhototaxis.h b/src/modules/AutoMoDeBehaviourPhototaxis.h
@@ -31,7 +31,6 @@ namespace argos {
 			virtual bool Failed();
 
 		private:
-			Real m_fSuccessProbabilityParameter;
 
 	};
 }

diff --git a/src/modules/AutoMoDeBehaviourRepulsion.cpp b/src/modules/AutoMoDeBehaviourRepulsion.cpp
@@ -57,11 +57,17 @@ namespace argos {
 			sRabVector = CVector2(cRabReading.Range, cRabReading.Bearing);
 		}
 
-		if (sRabVector.Length() < 0.1) {
-			sRabVector = CVector2(1.0, CRadians::ZERO);
+		CVector2 sDirectionVector(0,CRadians::ZERO);
+		CVector2 sProxVector(0,CRadians::ZERO);
+		sProxVector = CVector2(m_pcRobotDAO->GetProximityReading().Value, m_pcRobotDAO->GetProximityReading().Angle);
+		sDirectionVector = -sRabVector - 5*sProxVector;;
+
+
+		if (sDirectionVector.Length() < 0.1) {
+			sDirectionVector = CVector2(1.0, CRadians::ZERO);
 		}
 
-		m_pcRobotDAO->SetWheelsVelocity(ComputeWheelsVelocityFromVector(-sRabVector));
+		m_pcRobotDAO->SetWheelsVelocity(ComputeWheelsVelocityFromVector(sDirectionVector));
 
 		m_bLocked = false;
 	}
@@ -117,6 +123,6 @@ namespace argos {
 	/****************************************/
 
 	bool AutoMoDeBehaviourRepulsion::Failed() {
-		return ObstacleInFront();
+		return false; //ObstacleInFront();
 	}
 }
diff --git a/src/modules/AutoMoDeBehaviourRepulsion.h b/src/modules/AutoMoDeBehaviourRepulsion.h
@@ -32,7 +32,6 @@ namespace argos {
 
 		private:
 			UInt8 m_unRepulsionParameter;
-			Real m_fSuccessProbabilityParameter;
 
 	};
 }

diff --git a/src/modules/AutoMoDeBehaviourRotation.h b/src/modules/AutoMoDeBehaviourRotation.h
@@ -32,7 +32,6 @@ namespace argos {
 			virtual bool Failed();
 
 		private:
-			Real m_fSuccessProbabilityParameter;
 
 			enum RotationState {
 				IDLE, LEFT, RIGHT

diff --git a/src/modules/AutoMoDeBehaviourStop.h b/src/modules/AutoMoDeBehaviourStop.h
@@ -32,7 +32,6 @@ namespace argos {
 			virtual bool Failed();
 
 		private:
-			Real m_fSuccessProbabilityParameter;
 	};
 }
 

diff --git a/src/modules/AutoMoDeBehaviourStraight.h b/src/modules/AutoMoDeBehaviourStraight.h
@@ -32,7 +32,6 @@ namespace argos {
 			virtual bool Failed();
 
 		private:
-			Real m_fSuccessProbabilityParameter;
 	};
 }
-Original file line number
+Diff line change
@@ Expand Up / @@ -31,7 +31,6 @@ namespace argos { @@
     			virtual bool Failed();
     		private:
-    			Real m_fSuccessProbabilityParameter;
     	};
     }
@@ Expand Down @@