Virtual fence ui

src/behaviours/src/ROSAdapter.cpp

@@ -20,8 +20,8 @@
 #include <geometry_msgs/Twist.h>
 #include <nav_msgs/Odometry.h>
 #include <apriltags_ros/AprilTagDetectionArray.h>
-#include <std_msgs/Float32MultiArray.h>
 #include "swarmie_msgs/Waypoint.h"
+#include "swarmie_msgs/VirtualFence.h"
 
 // Include Controllers
 #include "LogicController.h"
@@ -164,12 +164,12 @@ tf::TransformListener *tfListener;
 void sigintEventHandler(int signal);
 
 //Callback handlers
-void joyCmdHandler(const sensor_msgs::Joy::ConstPtr& message);				//for joystick control
+void joyCmdHandler(const sensor_msgs::Joy::ConstPtr& message);			//for joystick control
 void modeHandler(const std_msgs::UInt8::ConstPtr& message);				//for detecting which mode the robot needs to be in
 void targetHandler(const apriltags_ros::AprilTagDetectionArray::ConstPtr& tagInfo);	//receives and stores April Tag Data using the TAG class
 void odometryHandler(const nav_msgs::Odometry::ConstPtr& message);			//receives and stores ODOM information
 void mapHandler(const nav_msgs::Odometry::ConstPtr& message);				//receives and stores GPS information
-void virtualFenceHandler(const std_msgs::Float32MultiArray& message);			//Used to set an invisible boundary for robots to keep them from traveling outside specific bounds
+void virtualFenceHandler(const swarmie_msgs::VirtualFence& message);		        //Used to set an invisible boundary for robots to keep them from traveling outside specific bounds
 void manualWaypointHandler(const swarmie_msgs::Waypoint& message);			//Receives a waypoint (from GUI) and sets the coordinates
 void behaviourStateMachine(const ros::TimerEvent&);					//Upper most state machine, calls logic controller to perform all actions
 void publishStatusTimerEventHandler(const ros::TimerEvent& event);			//Publishes "ONLINE" when rover is successfully connected
@@ -512,41 +512,40 @@ void odometryHandler(const nav_msgs::Odometry::ConstPtr& message) {
 }
 
 // Allows a virtual fence to be defined and enabled or disabled through ROS
-void virtualFenceHandler(const std_msgs::Float32MultiArray& message) 
+void virtualFenceHandler(const swarmie_msgs::VirtualFence& 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
+  int cmd = static_cast<int>(message.cmd); // Shape type
 
-  if (shape_type == 0)
+  if (cmd == 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
+    center.x = message.center_x; // Range center x
+    center.y = message.center_y; // Range center y
 
     // If the shape type is "circle" then element 4 is the radius, if rectangle then width
-    switch ( shape_type )
+    switch ( cmd )
     {
     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) );
+      //if ( message.data.size() != 4 ) throw ROSAdapterRangeShapeInvalidTypeException("Wrong number of parameters for circle shape type in ROSAdapter.cpp:virtualFenceHandler()");
+      //float radius = message.data; 
+      //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]; 
+      float width = message.width; 
+      float height = message.height; 
       logicController.setVirtualFenceOn( new RangeRectangle(center, width, height) );
       break;
     }

src/rqt_rover_gui/src/MapData.cpp

@@ -7,6 +7,26 @@ MapData::MapData()
     display_global_offset = false;
 }
 
+// Define virtual fences for display to the user.
+void MapData::setVirtualFenceCircle(float center_x, float center_y, float radius)
+{
+    update_mutex.lock();
+    virtual_fence_center = pair<float,float>(center_x,center_y);
+    virtual_fence_radius = radius;
+    virtual_fence_cmd = CIRCLE;
+    update_mutex.unlock();
+}
+
+void MapData::setVirtualFenceRectangle(float corner_x, float corner_y, float height, float width)
+{
+    update_mutex.lock();
+    virtual_fence_corner = pair<float,float>(corner_x, corner_y);
+    virtual_fence_height = height;
+    virtual_fence_width = width;
+    virtual_fence_cmd = RECTANGLE;
+    update_mutex.unlock();
+}
+
 void MapData::addToGPSRoverPath(string rover, float x, float y)
 {
   // Negate the y direction to orient the map so up is north.
@@ -230,6 +250,21 @@ void MapData::clear(string rover)
     update_mutex.unlock();
 }
 
+std::tuple<float,float,float,float> MapData::getVirtualFenceRectangle()
+{
+    return std::tuple<float,float,float,float>(virtual_fence_corner.first, virtual_fence_corner.second, virtual_fence_height, virtual_fence_width);
+}
+
+std::tuple<float,float,float> MapData::getVirtualFenceCircle()
+{
+    return std::tuple<float,float,float>(virtual_fence_center.first, virtual_fence_center.second, virtual_fence_radius);
+}
+
+VirtualFenceCmd MapData::getVirtualFenceCmd()
+{
+    return virtual_fence_cmd;
+}
+
 std::vector< std::pair<float,float> >* MapData::getEKFPath(std::string rover_name)
 {
     if(display_global_offset)

src/rqt_rover_gui/src/MapData.h

@@ -9,7 +9,8 @@
 #include <string>
 #include <QMutex>
 
-#include "MapData.h"
+// Virtual fence command definitions that can be sent to the rover
+enum VirtualFenceCmd {UNDEFINED, CIRCLE, RECTANGLE};
 
 // This class is the "model" for std::map frame in the model-view UI pattern,
 // where std::mapFrame is the view.
@@ -25,6 +26,16 @@ class MapData
     void addTargetLocation(std::string rover, float x, float y);
     void addCollectionPoint(std::string rover, float x, float y);
 
+    // Define virtual fences (also called search range). The virtual fence constrains
+    // Robot movement. They currently apply to all robots, though setting a different virtual
+    // fence for each robot could be useful.
+    void setVirtualFenceRectangle(float corner_x, float corner_y, float height, float width);
+    void setVirtualFenceCircle(float center_x, float center_y, float radius);
+
+    VirtualFenceCmd getVirtualFenceCmd();
+    std::tuple<float,float,float,float> getVirtualFenceRectangle();
+    std::tuple<float,float,float> getVirtualFenceCircle();
+
     // Add a waypoint for the specified rover name
     // Returns the id of the waypoint. IDs are use to issue waypoint commands to the rover.
     int addToWaypointPath(std::string rover, float x, float y);
@@ -106,6 +117,13 @@ class MapData
     std::map<std::string, float> min_ekf_seen_x;
     std::map<std::string, float> min_ekf_seen_y;
 
+    std::pair<float,float> virtual_fence_corner;
+    std::pair<float,float> virtual_fence_center;
+    float virtual_fence_height = 0;
+    float virtual_fence_width = 0;
+    float virtual_fence_radius = 0;
+    VirtualFenceCmd virtual_fence_cmd = UNDEFINED;
+
     bool display_global_offset;
 
     QMutex update_mutex; // To prevent race conditions when the data is being displayed by MapFrame

src/rqt_rover_gui/src/MapFrame.cpp

@@ -8,7 +8,6 @@
 #include <QGridLayout>
 #include <QLabel>
 #include <QMouseEvent>
-#include <MapData.h>
 #include "MapFrame.h"
 
 namespace rqt_rover_gui
@@ -52,6 +51,9 @@ MapFrame::MapFrame(QWidget *parent, Qt::WindowFlags flags) : QFrame(parent)
   // Trigger mouseMoveEvent even when button not pressed
   setMouseTracking(true);
 
+  // Accept keyboard events when a mouse click occurs in this frame
+  // Mouse press events are latched so that they are recieved here even when the key was pressed before the map click
+  setFocusPolicy(Qt::FocusPolicy::ClickFocus);
 }
 
 // This can't go in the constructor or there will be an infinite regression.
@@ -219,8 +221,8 @@ void MapFrame::paintEvent(QPaintEvent* event) {
   map_width = this->width()-1;// Minus 1 or will go off the edge
   map_height = this->height()-1;//
 
-  int map_center_x = map_origin_x+((map_width-map_origin_x)/2);
-  int map_center_y = map_origin_y+((map_height-map_origin_y)/2);
+  map_center_x = map_origin_x+((map_width-map_origin_x)/2);
+  map_center_y = map_origin_y+((map_height-map_origin_y)/2);
 
   // The map axes do not need to be redrawn for each rover so this code is
   // sandwiched between the two rover display list loops
@@ -543,6 +545,52 @@ void MapFrame::paintEvent(QPaintEvent* event) {
 
     hardware_rover_color_index = (hardware_rover_color_index + 1) % 8;
 
+    // Draw the virtual fence
+    if (map_data->getVirtualFenceCmd() == CIRCLE)
+    {
+
+        painter.setPen(Qt::yellow);
+        tuple<float,float,float> virtual_fence = map_data->getVirtualFenceCircle();
+        float center_x = map_origin_x+((get<0>(virtual_fence)-min_seen_x)/max_seen_width)*(map_width-map_origin_x);
+        float center_y = map_origin_y+((get<1>(virtual_fence)-min_seen_y)/max_seen_height)*(map_height-map_origin_y);
+
+        // Radius x and y because the map may not be scaled isomorphically
+        float radius_x = map_origin_x+((get<2>(virtual_fence)-min_seen_x)/max_seen_width)*(map_width-map_origin_x);
+        float radius_y = map_origin_y+((get<2>(virtual_fence)-min_seen_y)/max_seen_height)*(map_height-map_origin_y);
+
+        //emit sendInfoLogMessage("Circular fence detected: Fence center: (" + QString::number(center_x) + ", " + QString::number(center_y) + "); Radius_x: " + QString::number(radius_x) + "Radius_y: " + QString::number(radius_y));
+        painter.drawEllipse(center_x, center_y, radius_x, radius_y);
+    }
+    else if (map_data->getVirtualFenceCmd() == RECTANGLE)
+    {
+        painter.setPen(Qt::yellow);
+        tuple<float,float,float,float> virtual_fence = map_data->getVirtualFenceRectangle();
+        float corner_x = map_origin_x+((get<0>(virtual_fence)-min_seen_x)/max_seen_width)*(map_width-map_origin_x);
+        float corner_y = map_origin_y+((get<1>(virtual_fence)-min_seen_y)/max_seen_height)*(map_height-map_origin_y);
+        float width = map_origin_x+((get<2>(virtual_fence)-min_seen_x)/max_seen_width)*(map_width-map_origin_x) - corner_x;
+        float height = map_origin_y+((get<3>(virtual_fence)-min_seen_y)/max_seen_height)*(map_height-map_origin_y) - corner_y;
+
+        //emit sendInfoLogMessage("Rectangle fence detected: Fence corner: (" + QString::number(get<0>(virtual_fence)) + ", " + QString::number(get<1>(virtual_fence)) + "); Width: " + QString::number(width) + "Height: " + QString::number(height));
+
+        QPainterPath rectangle_fence;
+        rectangle_fence.addRect(corner_x, corner_y, width, height);
+        painter.drawPath(rectangle_fence);
+
+        if (user_is_drawing_fence)
+        {
+            float rectangle_center_x = corner_x+width/2;
+            float rectangle_center_y = corner_y+height/2;
+            float meter_conversion_factor = max_seen_width/map_width; // Convert pixels into meters
+            float rectangle_center_x_in_meters = ((corner_x+width/2)-map_center_x) * meter_conversion_factor;
+            float rectangle_center_y_in_meters = ((corner_y+height/2)-map_center_y) * meter_conversion_factor;
+            float width_in_meters = abs(width * meter_conversion_factor); // abs because distances must be positive values
+            float height_in_meters = abs(height * meter_conversion_factor);
+            painter.drawEllipse(QPointF(rectangle_center_x, rectangle_center_y), 2.5, 2.5);
+            QString rect_geometry = "Center: (" + QString::number(rectangle_center_x_in_meters) + ", " + QString::number(rectangle_center_y_in_meters) + ")\nWidth: " + QString::number(width_in_meters)  + " m\nHeight: " + QString::number(height_in_meters) + " m";
+            painter.drawText(QRect(rectangle_center_x, rectangle_center_y, 300, 200), rect_geometry);
+        }
+    }
+
     painter.setPen(Qt::white);
   } // End rover display list set iteration
 
@@ -633,10 +681,57 @@ void MapFrame::mouseReleaseEvent(QMouseEvent *event)
 {
   previous_translate_x = translate_x;
   previous_translate_y = translate_y;
+
+  if (user_is_drawing_fence)
+  {
+    // Stop drawing the fence
+    user_is_drawing_fence = false;
+
+    // Extract the geometry of the virtual fence from map data
+    tuple<float,float,float,float> virtual_fence = map_data->getVirtualFenceRectangle();
+    float corner_x = map_origin_x+((get<0>(virtual_fence)-min_seen_x)/max_seen_width)*(map_width-map_origin_x);
+    float corner_y = map_origin_y+((get<1>(virtual_fence)-min_seen_y)/max_seen_height)*(map_height-map_origin_y);
+    float width = map_origin_x+((get<2>(virtual_fence)-min_seen_x)/max_seen_width)*(map_width-map_origin_x) - corner_x;
+    float height = map_origin_y+((get<3>(virtual_fence)-min_seen_y)/max_seen_height)*(map_height-map_origin_y) - corner_y;
+    float rectangle_center_x = corner_x+width/2;
+    float rectangle_center_y = corner_y+width/2;
+
+    // Convert the geometry into meters using robot data plotted in the map to relate pixel space to actual meters
+    float meter_conversion_factor = max_seen_width/map_width; // Convert pixels into meters
+    float rectangle_center_x_in_meters = ((corner_x+width/2)-map_center_x) * meter_conversion_factor;
+    float rectangle_center_y_in_meters = ((corner_y+height/2)-map_center_y) * meter_conversion_factor;
+    float width_in_meters = abs(width * meter_conversion_factor); // abs because distances must be positive values
+    float height_in_meters = abs(height * meter_conversion_factor);
+
+    // Gett the command associated with the virtual fence. This can be the shape or undefined. Undefined will delete the virtual fence.
+    VirtualFenceCmd cmd = map_data->getVirtualFenceCmd();
+
+    // Send the command to Rover GUI. Rover GUI will package it up for ROS.
+    sendVirtualFenceCmd( cmd, rectangle_center_x_in_meters, rectangle_center_y_in_meters, width_in_meters, height_in_meters );
+  }
 }
 
 void MapFrame::mousePressEvent(QMouseEvent *event)
 {
+  // Handle user defining the virtual fence by left clicking and dragging with shift held down
+  // Holding shift while dragging indicates the user wants to create a circular virtual fence
+    // If the user is not already dragging the mouse to draw the fence then set the center
+    if (event->buttons() == Qt::LeftButton && !user_is_drawing_fence && shift_key_down)
+    {
+
+
+        float mouse_map_x = ((event->pos().x() - map_origin_x*1.0f)/(map_width-map_origin_x))*max_seen_width + min_seen_x;
+        float mouse_map_y = ((event->pos().y() - map_origin_y*1.0f)/(map_height-map_origin_y))*max_seen_height + min_seen_y;
+
+        fence_corner_x = mouse_map_x;
+        fence_corner_y = mouse_map_y;
+
+        emit sendInfoLogMessage("MapFrame: Mouse left button press with shift key down. Fence corner: (" + QString::number(fence_corner_x) + ", " + QString::number(fence_corner_y) + ")");
+        user_is_drawing_fence = true;
+    }
+
+    emit sendInfoLogMessage("MapFrame: Mouse left button press.");
+
   std::set<std::string>::iterator it = display_list.find(rover_currently_selected);
 
   // Failure condition: a valid rover is not selected.
@@ -717,6 +812,29 @@ void MapFrame::mouseMoveEvent(QMouseEvent *event)
   // Get the mouse pointer position to print on the map
   mouse_pointer_position = event->pos();
 
+  if ( event->type() == QEvent::MouseMove && event->buttons() == Qt::LeftButton && shift_key_down && user_is_drawing_fence )
+  {
+      float mouse_map_x = ((event->pos().x() - map_origin_x*1.0f)/(map_width-map_origin_x))*max_seen_width + min_seen_x;
+      float mouse_map_y = ((event->pos().y() - map_origin_y*1.0f)/(map_height-map_origin_y))*max_seen_height + min_seen_y;
+
+      //float delta_x = mouse_map_x - fence_center_x;
+      //float delta_y = mouse_map_y - fence_center_y;
+      //float fence_radius = sqrt(delta_x*delta_x + delta_y*delta_y);
+
+      map_data->setVirtualFenceRectangle(fence_corner_x, fence_corner_y, mouse_map_x, mouse_map_y);
+      emit sendInfoLogMessage(QString("MapFrame: mouse move drawing fence.") + " Corner: ("
+                              + QString::number(fence_corner_x)
+                              + ","
+                              + QString::number(fence_corner_y)
+                              + "); Width: "
+                              + QString::number(mouse_map_x)
+                              + "; Height: "
+                              + QString::number(mouse_map_y)
+                              );
+
+      return;
+  }
+
   // Do not adjust panning in auto-transform mode.. the changes will not be reflected
     // and will be applied only when the user clicks on manual panning mode which will
     // cause undesired results.
@@ -748,6 +866,8 @@ void MapFrame::mouseMoveEvent(QMouseEvent *event)
         // emit sendInfoLogMessage("MapFrame: mouse move: xp: " + QString::number(previous_clicked_position.x()) + " yp: " + QString::number(previous_clicked_position.y()));
 
         // Store the current mouse position for use in the map
+
+
       }
    }
 }
@@ -944,6 +1064,27 @@ void MapFrame::receiveCurrentRoverName( QString rover_name )
   }
 }
 
+void MapFrame::keyPressEvent(QKeyEvent* event)
+{
+    if(event->key() == Qt::Key_Shift)
+    {
+        emit sendInfoLogMessage("MapFrame: shift key pressed.");
+        shift_key_down = true;
+    }
+
+
+}
+
+void MapFrame::keyReleaseEvent(QKeyEvent* event)
+{
+    if(event->key() == Qt::Key_Shift)
+    {
+        emit sendInfoLogMessage("MapFrame: shift key released.");
+        shift_key_down = false;
+        user_is_drawing_fence = false;
+    }
+}
+
 void MapFrame::enableWaypoints(string rover_name)
 {
   map_data->setManualMode(rover_name);