Merge pull request #6 from BCLab-UNM/virtual_fence

Initial commit of the virtual fence

Author: gmfricke

src/behaviours/CMakeLists.txt

@@ -29,6 +29,7 @@ add_executable(
   src/ROSAdapter.cpp
   src/PID.cpp
   src/DriveController.cpp
+  src/RangeController.cpp
   src/LogicController.cpp
 )
 

src/behaviours/src/LogicController.cpp

@@ -1,6 +1,7 @@
 #include "LogicController.h"
 
 LogicController::LogicController() {
+
   logicState = LOGIC_STATE_INTERRUPT;
   processState = PROCCESS_STATE_SEARCHING;
 
@@ -32,10 +33,11 @@ Result LogicController::DoWork() {
   //most important. A priority of less than 0 is an ignored controller use -1 for standards sake.
   //if any controller needs and interrupt the logic state is changed to interrupt
   for(PrioritizedController cntrlr : prioritizedControllers) {
-    if(cntrlr.controller->ShouldInterrupt() && cntrlr.priority >= 0) {
-      logicState = LOGIC_STATE_INTERRUPT;
-      //do not break all shouldInterupts may need calling in order to properly pre-proccess data.
-    }
+    if(cntrlr.controller->ShouldInterrupt() && cntrlr.priority >= 0) 
+      {
+	logicState = LOGIC_STATE_INTERRUPT;
+	//do not break all shouldInterupts may need calling in order to properly pre-proccess data.
+      }
   }
 
   //logic state switch
@@ -186,8 +188,9 @@ void LogicController::ProcessData() {
   if (processState == PROCCESS_STATE_SEARCHING) {
     prioritizedControllers = {
       PrioritizedController{0, (Controller*)(&searchController)},
-      PrioritizedController{1, (Controller*)(&obstacleController)},
-      PrioritizedController{2, (Controller*)(&pickUpController)},
+      PrioritizedController{10, (Controller*)(&obstacleController)},
+      PrioritizedController{15, (Controller*)(&pickUpController)},
+      PrioritizedController{5, (Controller*)(&range_controller)},
       PrioritizedController{-1, (Controller*)(&dropOffController)}
     };
   }
@@ -196,8 +199,9 @@ void LogicController::ProcessData() {
   else if (processState  == PROCCESS_STATE_TARGET_PICKEDUP) {
     prioritizedControllers = {
       PrioritizedController{-1, (Controller*)(&searchController)},
-      PrioritizedController{2, (Controller*)(&obstacleController)},
+      PrioritizedController{15, (Controller*)(&obstacleController)},
       PrioritizedController{-1, (Controller*)(&pickUpController)},
+      PrioritizedController{10, (Controller*)(&range_controller)},
       PrioritizedController{1, (Controller*)(&dropOffController)}
     };
   }
@@ -242,16 +246,18 @@ void LogicController::controllerInterconnect() {
   }
 }
 
-
+// Recieves position in the world inertial frame (should rename to SetOdomPositionData)
 void LogicController::SetPositionData(Point currentLocation) {
   searchController.SetCurrentLocation(currentLocation);
   dropOffController.SetCurrentLocation(currentLocation);
   obstacleController.SetCurrentLocation(currentLocation);
   driveController.SetCurrentLocation(currentLocation);
 }
 
-void LogicController::SetMapPositionData(Point currentLocationMap) {
-
+// Recieves position in the world frame with global data (GPS)
+void LogicController::SetMapPositionData(Point currentLocation) {
+  range_controller.setCurrentLocation(currentLocation);
+  
 }
 
 void LogicController::SetVelocityData(float linearVelocity, float angularVelocity) {
@@ -273,11 +279,23 @@ void LogicController::SetSonarData(float left, float center, float right) {
   obstacleController.SetSonarData(left,center,right);
 }
 
+// Called once by RosAdapter in guarded init
 void LogicController::SetCenterLocationOdom(Point centerLocationOdom) {
   searchController.SetCenterLocation(centerLocationOdom);
   dropOffController.SetCenterLocation(centerLocationOdom);
 }
 
+void LogicController::setVirtualFenceOn( RangeShape* range )
+{
+  range_controller.setRangeShape(range);
+  range_controller.setEnabled(true);
+}
+
+void LogicController::setVirtualFenceOff()
+{
+  range_controller.setEnabled(false);
+}
+
 void LogicController::SetCenterLocationMap(Point centerLocationMap) {
 
 }

src/behaviours/src/LogicController.h

@@ -7,6 +7,7 @@
 #include "SearchController.h"
 #include "ObstacleController.h"
 #include "DriveController.h"
+#include "RangeController.h"
 
 #include <vector>
 #include <queue>
@@ -47,6 +48,12 @@ class LogicController : virtual Controller
 
   void SetCurrentTimeInMilliSecs( long int time );
 
+  // Tell the logic controller whether rovers should automatically
+  // resstrict their foraging range. If so provide the shape of the
+  // allowed range.
+  void setVirtualFenceOn( RangeShape* range );
+  void setVirtualFenceOff( );
+
 protected:
   void ProcessData();
 
@@ -73,6 +80,7 @@ class LogicController : virtual Controller
   SearchController searchController;
   ObstacleController obstacleController;
   DriveController driveController;
+  RangeController range_controller; 
 
   std::vector<PrioritizedController> prioritizedControllers;
   priority_queue<PrioritizedController> control_queue;

src/behaviours/src/ROSAdapter.cpp

@@ -20,6 +20,7 @@
 #include <geometry_msgs/Twist.h>
 #include <nav_msgs/Odometry.h>
 #include <apriltags_ros/AprilTagDetectionArray.h>
+#include <std_msgs/Float32MultiArray.h>
 
 // Include Controllers
 #include "LogicController.h"
@@ -33,9 +34,30 @@
 #include <ros/ros.h>
 #include <signal.h>
 
+#include <exception> // For exception handling
 
 using namespace std;
 
+// Define Exceptions
+// Define an exception to be thrown if the user tries to create
+// a RangeShape using invalid dimensions
+class ROSAdapterRangeShapeInvalidTypeException : public std::exception {
+ public:
+  ROSAdapterRangeShapeInvalidTypeException(std::string msg) {
+    this->msg = msg;
+  }
+
+  virtual const char* what() const throw()
+  {
+    std::string message = "Invalid RangeShape type provided: " + msg;
+    return message.c_str();
+  }
+
+ private:
+  std::string msg;
+};
+
+
 // Random number generator
 random_numbers::RandomNumberGenerator* rng;
 
@@ -106,6 +128,7 @@ ros::Subscriber modeSubscriber;
 ros::Subscriber targetSubscriber;
 ros::Subscriber odometrySubscriber;
 ros::Subscriber mapSubscriber;
+ros::Subscriber virtualFenceSubscriber;
 
 
 // Timers
@@ -133,6 +156,7 @@ void modeHandler(const std_msgs::UInt8::ConstPtr& message);
 void targetHandler(const apriltags_ros::AprilTagDetectionArray::ConstPtr& tagInfo);
 void odometryHandler(const nav_msgs::Odometry::ConstPtr& message);
 void mapHandler(const nav_msgs::Odometry::ConstPtr& message);
+void virtualFenceHandler(const std_msgs::Float32MultiArray& message);
 void behaviourStateMachine(const ros::TimerEvent&);
 void publishStatusTimerEventHandler(const ros::TimerEvent& event);
 void publishHeartBeatTimerEventHandler(const ros::TimerEvent& event);
@@ -167,7 +191,8 @@ int main(int argc, char **argv) {
   targetSubscriber = mNH.subscribe((publishedName + "/targets"), 10, targetHandler);
   odometrySubscriber = mNH.subscribe((publishedName + "/odom/filtered"), 10, odometryHandler);
   mapSubscriber = mNH.subscribe((publishedName + "/odom/ekf"), 10, mapHandler);
-  message_filters::Subscriber<sensor_msgs::Range> sonarLeftSubscriber(mNH, (publishedName + "/sonarLeft"), 10);
+  virtualFenceSubscriber = mNH.subscribe(("/virtualFence"), 10, virtualFenceHandler);
+    message_filters::Subscriber<sensor_msgs::Range> sonarLeftSubscriber(mNH, (publishedName + "/sonarLeft"), 10);
   message_filters::Subscriber<sensor_msgs::Range> sonarCenterSubscriber(mNH, (publishedName + "/sonarCenter"), 10);
   message_filters::Subscriber<sensor_msgs::Range> sonarRightSubscriber(mNH, (publishedName + "/sonarRight"), 10);
 
@@ -294,7 +319,7 @@ void behaviourStateMachine(const ros::TimerEvent&) {
     }
 
     //publishHandeling here
-    //logicController.getPublishData() suggested;
+    //logicController.getPublishData(); suggested
 
 
     //adds a blank space between sets of debugging data to easly tell one tick from the next
@@ -385,6 +410,53 @@ void odometryHandler(const nav_msgs::Odometry::ConstPtr& message) {
   logicController.SetVelocityData(linearVelocity, angularVelocity);
 }
 
+// Allows a virtual fence to be defined and enabled or disabled through ROS
+void virtualFenceHandler(const std_msgs::Float32MultiArray& message) 
+{
+  // Read data from the message array
+  // The first element is an integer indicating the shape type
+  // 0 = Disable the virtual fence
+  // 1 = circle
+  // 2 = rectangle
+  int shape_type = static_cast<int>(message.data[0]); // Shape type
+  
+  if (shape_type == 0)
+    {
+      logicController.setVirtualFenceOff();
+    }
+  else
+    {
+      // Elements 2 and 3 are the x and y coordinates of the range center
+      Point center;
+      center.x = message.data[1]; // Range center x
+      center.y = message.data[2]; // Range center y
+      
+      // If the shape type is "circle" then element 4 is the radius, if rectangle then width
+      switch ( shape_type )
+	{
+	case 1: // Circle
+	  {
+	    if ( message.data.size() != 4 ) throw ROSAdapterRangeShapeInvalidTypeException("Wrong number of parameters for circle shape type in ROSAdapter.cpp:virtualFenceHandler()");
+	    float radius = message.data[3]; 
+	    logicController.setVirtualFenceOn( new RangeCircle(center, radius) );
+	    break;
+	  }
+	case 2: // Rectangle 
+	  {
+	    if ( message.data.size() != 5 ) throw ROSAdapterRangeShapeInvalidTypeException("Wrong number of parameters for rectangle shape type in ROSAdapter.cpp:virtualFenceHandler()");
+	    float width = message.data[3]; 
+	    float height = message.data[4]; 
+	    logicController.setVirtualFenceOn( new RangeRectangle(center, width, height) );
+	    break;
+	  }
+	default:
+	  { // Unknown shape type specified
+	    throw ROSAdapterRangeShapeInvalidTypeException("Unknown Shape type in ROSAdapter.cpp:virtualFenceHandler()");
+	  }
+	}
+    }
+}
+
 void mapHandler(const nav_msgs::Odometry::ConstPtr& message) {
   //Get (x,y) location directly from pose
   currentLocationMap.x = message->pose.pose.position.x;
@@ -400,11 +472,11 @@ void mapHandler(const nav_msgs::Odometry::ConstPtr& message) {
   linearVelocity = message->twist.twist.linear.x;
   angularVelocity = message->twist.twist.angular.z;
 
-  Point currentLoc;
-  currentLoc.x = currentLocation.x;
-  currentLoc.y = currentLocation.y;
-  currentLoc.theta = currentLocation.theta;
-  logicController.SetPositionData(currentLoc);
+  Point curr_loc;
+  curr_loc.x = currentLocationMap.x;
+  curr_loc.y = currentLocationMap.y;
+  curr_loc.theta = currentLocationMap.theta;
+  logicController.SetMapPositionData(curr_loc);
   logicController.SetMapVelocityData(linearVelocity, angularVelocity);
 }