Merge pull request #153 from BCLab-UNM/comments-manual-waypoints

Comments for ManualWaypointController

Author: gmfricke

src/behaviours/src/LogicController.cpp

@@ -226,6 +226,7 @@ void LogicController::ProcessData()
     };
   }
   else if (processState == PROCESS_STATE_MANUAL) {
+    // under manual control only the manual waypoint controller is active
     prioritizedControllers = {
       PrioritizedController{-1, (Controller*)(&searchController)},
       PrioritizedController{-1, (Controller*)(&obstacleController)},

src/behaviours/src/LogicController.h

@@ -46,11 +46,36 @@ class LogicController : virtual Controller
   void SetMapVelocityData(float linearVelocity, float angularVelocity);
   void SetCenterLocationOdom(Point centerLocationOdom);
   void SetCenterLocationMap(Point centerLocationMap);
+
+  
+  // Passthrough for providing new waypoints to the
+  // ManualWaypointController.
   void AddManualWaypoint(Point wpt, int waypoint_id);
+
+  
+  // Passthrough for removing waypoints from the
+  // ManualWaypointController.
   void RemoveManualWaypoint(int waypoint_id);
+
+  
+  // Passthrough for getting the list of manual waypoints that have
+  // been visited. 
   std::vector<int> GetClearedWaypoints();
 
+  
+  // Put the logic controller into manual mode. Changes process state
+  // to PROCESS_STATE_MANUAL and logic state to LOGIC_STATE_INTERRUPT.
+  
+  // If the logic controller is already in manual mode this has no
+  // effect.
   void SetModeManual();
+
+  
+  // Put the logic controller into autonomous mode. Resets the logic
+  // controller and clears all manual waypoints.
+  //
+  // If the logic controller is already in autonomous mode, then this
+  // has no effect.
   void SetModeAuto();
 
   void SetCurrentTimeInMilliSecs( long int time );
@@ -78,7 +103,7 @@ class LogicController : virtual Controller
     PROCCESS_STATE_TARGET_PICKEDUP,
     PROCCESS_STATE_DROP_OFF,
     _LAST,
-    PROCESS_STATE_MANUAL
+    PROCESS_STATE_MANUAL // robot is under manual control
   };
 
   LogicState logicState;

src/behaviours/src/ManualWaypointController.h

@@ -11,14 +11,50 @@ class ManualWaypointController : virtual Controller
    ManualWaypointController();
    ~ManualWaypointController();
 
+   
+   // Clears the list of waypoints to visit.
+    
+   // NOTE: this will not stop the robot from driving to a waypoint if
+   // it was already driving there before this was called. 
    void Reset() override;
+
+   
+   // Returns the next waypoint in the list. The result is set up to
+   // be used by DriveController for waypoint navigation.
+   //
    Result DoWork() override;
+
+   
+   // True if there are waypoints in the list. False otherwise.
    bool HasWork() override;
+   
+   // Interrupts only if the number of waypoints has changed and is
+   // non-zero.
    bool ShouldInterrupt() override;
 
+   
+   // Tell the controller the current location of the robot.
    void SetCurrentLocation(Point currentLocation);
+
+   // Add the provided waypoint to the list of manual waypoints.
+   
+   // NOTE: Waypoints should have unique ids, it is incumbent on the
+   // caller to ensure this. Providing the same IDs for multiple
+   // waypoints may cause undefined behavior in the GUI. Further more
+   // if the ID is removed using RemoveManualWaypoint() then all
+   // waypoints with that ID may be removed.
    void AddManualWaypoint(Point wpt, int id);
+
+   
+   // Remove the waypoint with the given ID from the list of waypoints
+   // to visit. If no maypoint exists with the given ID, the no action
+   // is taken.
    void RemoveManualWaypoint(int id);
+
+   // Get a vector containing all waypoint IDs that have been visited
+   // since this function was last called.
+   
+   // This should be called regularly to prevent memory leaks.
    std::vector<int> ReachedWaypoints();
 
 protected:
@@ -35,4 +71,4 @@ class ManualWaypointController : virtual Controller
    const float waypoint_tolerance = 0.15;
 };
 
-#endif // MANUALWAYPOINTCONTROLLER_H
+#endif // MANUALWAYPOINTCONTROLLER_H

src/behaviours/src/ROSAdapter.cpp

@@ -129,6 +129,8 @@ ros::Publisher wristAnglePublish;
 ros::Publisher infoLogPublisher;
 ros::Publisher driveControlPublish;
 ros::Publisher heartbeatPublisher;
+// Publishes swarmie_msgs::Waypoint messages on "/<robot>/waypooints"
+// to indicate when waypoints have been reached.
 ros::Publisher waypointFeedbackPublisher;
 
 // Subscribers
@@ -138,6 +140,8 @@ ros::Subscriber targetSubscriber;
 ros::Subscriber odometrySubscriber;
 ros::Subscriber mapSubscriber;
 ros::Subscriber virtualFenceSubscriber;
+// manualWaypointSubscriber listens on "/<robot>/waypoints/cmd" for
+// swarmie_msgs::Waypoint messages.
 ros::Subscriber manualWaypointSubscriber;
 
 // Timers
@@ -382,6 +386,8 @@ void behaviourStateMachine(const ros::TimerEvent&)
     // publish current state for the operator to see
     stateMachineMsg.data = "WAITING";
 
+    // poll the logicController to get the waypoints that have been
+    // reached.
     std::vector<int> cleared_waypoints = logicController.GetClearedWaypoints();
 
     for(std::vector<int>::iterator it = cleared_waypoints.begin();
@@ -395,9 +401,12 @@ void behaviourStateMachine(const ros::TimerEvent&)
     result = logicController.DoWork();
     if(result.type != behavior || result.b != wait)
     {
+      // if the logic controller requested that the robot drive, then
+      // drive. Otherwise there are no manual waypoints and the robot
+      // should sit idle. (ie. only drive according to joystick
+      // input).
       sendDriveCommand(result.pd.left,result.pd.right);
     }
-    //cout << endl;
   }
 
   // publish state machine string for user, only if it has changed, though