Merge pull request #122 from BCLab-UNM/no-manual-waypoints-while-autonomous

Don't accept manual waypoint input for rover's in autonomous mode

Author: gmfricke

src/rqt_rover_gui/src/MapData.cpp

@@ -153,6 +153,7 @@ void MapData::clear()
     target_locations.clear();
     collection_points.clear();
     waypoint_path.clear();
+    rover_mode.clear();
 
     update_mutex.unlock();
 }
@@ -175,6 +176,7 @@ void MapData::clear(string rover)
     gps_rover_path.erase(rover);
     target_locations.erase(rover);
     collection_points.erase(rover);
+    rover_mode.erase(rover);
 
     update_mutex.unlock();
 }
@@ -344,6 +346,25 @@ float MapData::getMinEncoderY(string rover_name)
     return min_encoder_seen_y[rover_name];
 }
 
+bool MapData::inManualMode(string rover_name)
+{
+   return rover_mode[rover_name] == 0;
+}
+
+void MapData::setAutonomousMode(string rover_name)
+{
+   update_mutex.lock();
+   rover_mode[rover_name] = 1;
+   update_mutex.unlock();
+}
+
+void MapData::setManualMode(string rover_name)
+{
+   update_mutex.lock();
+   rover_mode[rover_name] = 0;
+   update_mutex.unlock();
+}
+
 void MapData::lock()
 {
     update_mutex.lock();

src/rqt_rover_gui/src/MapData.h

@@ -63,6 +63,10 @@ class MapData
     float getMinEncoderX(std::string rover_name);
     float getMinEncoderY(std::string rover_name);
 
+    bool inManualMode(std::string rover_name);
+    void setAutonomousMode(std::string rover_name);
+    void setManualMode(std::string rover_name);
+
     ~MapData();
 
 private:
@@ -100,6 +104,7 @@ class MapData
     QMutex update_mutex; // To prevent race conditions when the data is being displayed by MapFrame
 
     std::string currently_selected_rover;
+    std::map<std::string, int> rover_mode;
 
     int waypoint_id_counter = 0;
 };

src/rqt_rover_gui/src/MapFrame.cpp

@@ -624,6 +624,10 @@ void MapFrame::mouseReleaseEvent(QMouseEvent *event)
 
 void MapFrame::mousePressEvent(QMouseEvent *event)
 {
+  if(! map_data->inManualMode(rover_currently_selected))
+  {
+    return;
+  }
 
   float waypoint_click_tolerance = 0.25*(scale/10);
 
@@ -896,6 +900,16 @@ void MapFrame::receiveCurrentRoverName( QString rover_name )
   }
 }
 
+void MapFrame::enableWaypoints(string rover_name)
+{
+  map_data->setManualMode(rover_name);
+}
+
+void MapFrame::disableWaypoints(string rover_name)
+{
+  map_data->setAutonomousMode(rover_name);
+}
+
 MapFrame::~MapFrame()
 {
   // Safely erase map data - locks to make sure a frame isnt being drawn

src/rqt_rover_gui/src/MapFrame.h

@@ -82,7 +82,10 @@ namespace rqt_rover_gui
       // Calculate scale and transform to keep all data in the map frame
       // Excludes manual trasform
       void setAutoTransform();
- 
+
+      void enableWaypoints(std::string rover);
+      void disableWaypoints(std::string rover);
+
       // Show a copy of the map in its own resizable window
       void popout();
 
@@ -181,7 +184,12 @@ namespace rqt_rover_gui
       float max_seen_height = -std::numeric_limits<float>::max();
 
       std::string rover_currently_selected; // This is the rover selected in the main GUI.
-      
+
+      enum mode {
+        AUTONOMOUS,
+        MANUAL
+      };
+      std::map<std::string, bool> rover_mode;
   };
 
 }

src/rqt_rover_gui/src/rover_gui_plugin.cpp

@@ -1274,6 +1274,9 @@ void RoverGUIPlugin::autonomousRadioButtonEventHandler(bool marked)
 
     //Hide joystick frame
     ui.joystick_frame->setHidden(true);
+
+    // disable waypoint input in map frame
+    ui.map_frame->disableWaypoints(selected_rover_name);
 }
 
 void RoverGUIPlugin::joystickRadioButtonEventHandler(bool marked)
@@ -1317,6 +1320,9 @@ void RoverGUIPlugin::joystickRadioButtonEventHandler(bool marked)
 
     //Show joystick frame
     ui.joystick_frame->setHidden(false);
+
+    // enable wayoint input in the map frame
+    ui.map_frame->enableWaypoints(selected_rover_name);
 }
 
 void RoverGUIPlugin::allAutonomousButtonEventHandler()