Changes to CircleController. Publishing proper data for FollowController

pps_ws/src/d_fall_pps/src/CircleControllerService.cpp

@@ -156,6 +156,15 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
 	
     currentTime += request.ownCrazyflie.acquiringTime;
 
+    //publish setpoint for FollowController of another student group
+    Setpoint pubSetpoint;
+    pubSetpoint.x = request.ownCrazyflie.x;
+    pubSetpoint.y = request.ownCrazyflie.y;
+    pubSetpoint.z = request.ownCrazyflie.z;
+    pubSetpoint.yaw = request.ownCrazyflie.yaw;
+    followPublisher.publish(pubSetpoint);
+
+    //actual Circle Controller stuff follows
 	Setpoint circlePoint;
     calculateCircle(circlePoint);
 
@@ -207,13 +216,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
 
     previousLocation = request.ownCrazyflie;
 
-    Setpoint pubSetpoint;
-    pubSetpoint.x = previousLocation.x;
-    pubSetpoint.y = previousLocation.y;
-    pubSetpoint.z = previousLocation.z;
-    pubSetpoint.yaw = previousLocation.yaw;
 
-    followPublisher.publish(pubSetpoint);
     
 	return true;
 }
@@ -227,6 +230,7 @@ int main(int argc, char* argv[]) {
     ros::NodeHandle nodeHandle("~");
     loadParameters(nodeHandle);
 
+    //publisher for Follow Controller of another student group
     followPublisher = nodeHandle.advertise<Setpoint>("FollowTopic", 1);
 
     ros::ServiceServer service = nodeHandle.advertiseService("CircleController", calculateControlOutput);

pps_ws/src/d_fall_pps/src/FollowCrazyflieService.cpp

@@ -194,6 +194,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
 	return true;
 }
 
+//the setpoint is copied from the other crazyflie. This crazyflie should then make the same figure in its own area
 void followCallback(const Setpoint& newSetpoint) {
     setpoint[0] = newSetpoint.x-0.1;
     setpoint[1] = newSetpoint.y-0.1;