added a followController - push to test

pps_ws/src/d_fall_pps/CMakeLists.txt

@@ -217,6 +217,7 @@ add_executable(SafeControllerService src/SafeControllerService.cpp)
 add_executable(CustomControllerService src/CustomControllerService.cpp)
 add_executable(CentralManagerService src/CentralManagerService.cpp src/CrazyflieIO.cpp)
 add_executable(CircleControllerService src/CircleControllerService.cpp)
+add_executable(FollowCrazyflieService src/FollowCrazyflieService.cpp)
 
 
 
@@ -250,7 +251,7 @@ add_dependencies(SafeControllerService d_fall_pps_generate_messages_cpp ${catkin
 add_dependencies(CustomControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
 add_dependencies(CentralManagerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
 add_dependencies(CircleControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
-
+add_dependencies(FollowCrazyflieService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
 
 # GUI-- dependencies
 add_dependencies(my_GUI d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
@@ -285,6 +286,7 @@ target_link_libraries(CentralManagerService ${catkin_LIBRARIES})
 
 target_link_libraries(CircleControllerService ${catkin_LIBRARIES})
 
+target_link_libraries(FollowCrazyflieService ${catkin_LIBRARIES})
 
 # GUI -- link libraries
 target_link_libraries(my_GUI Qt5::Widgets) # GUI -- let my_GUI have acesss to Qt stuff

pps_ws/src/d_fall_pps/launch/StudentFollow.launch

+<launch>
+
+	<node pkg="d_fall_pps" name="CrazyRadio" output="screen" type="CrazyRadio.py">
+		<rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
+	</node>
+
+	<node pkg="d_fall_pps" name="PPSClient" output="screen" type="PPSClient">
+		<rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
+		<param name="studentID" value="$(optenv ROS_NAMESPACE)" />
+	</node>
+
+	<node pkg="d_fall_pps" name="SafeControllerService" output="screen" type="SafeControllerService">
+		<rosparam command="load" file="$(find d_fall_pps)/param/SafeController.yaml" />
+	</node>
+
+	<node pkg="d_fall_pps" name="FollowControllerService" output="screen" type="FollowControllerService">
+		<rosparam command="load" file="$(find d_fall_pps)/param/SafeController.yaml" />
+	</node>
+
+
+	<!-- <node pkg="rqt_plot" name="commandplotter" type="rqt_plot" args="/PPSClient/ControlCommand/roll /PPSClient/ControlCommand/pitch /PPSClient/ControlCommand/yaw" /> -->
+
+</launch>

pps_ws/src/d_fall_pps/src/CircleControllerService.cpp

@@ -55,6 +55,9 @@ float currentTime;
 const float OMEGA = 0.5*2*PI;
 const float RADIUS = 0.35;
 
+//point publisher for FollowCrazyflieService
+ros::Publisher followPublisher;
+
 
 void loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length) {
     if(!nodeHandle.getParam(name, val)){
@@ -204,6 +207,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
 
     previousLocation = request.ownCrazyflie;
 
+    followPublisher.publish(previousLocation);
     
 	return true;
 }
@@ -217,6 +221,8 @@ int main(int argc, char* argv[]) {
     ros::NodeHandle nodeHandle("~");
     loadParameters(nodeHandle);
 
+    followPublisher = nodeHandle.advertise<Setpoint>("FollowTopic", 1);
+
     ros::ServiceServer service = nodeHandle.advertiseService("CircleController", calculateControlOutput);
     ROS_INFO("CircleControllerService ready");
     

pps_ws/src/d_fall_pps/src/FollowCrazyflieService.cpp

+//    Alternate controller that is expected to work.
+//    Copyright (C) 2017  Cyrill Burgener, Marco Mueller, Philipp Friedli
+//
+//    This program is free software: you can redistribute it and/or modify
+//    it under the terms of the GNU General Public License as published by
+//    the Free Software Foundation, either version 3 of the License, or
+//    (at your option) any later version.
+//
+//    This program is distributed in the hope that it will be useful,
+//    but WITHOUT ANY WARRANTY; without even the implied warranty of
+//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//    GNU General Public License for more details.
+//
+//    You should have received a copy of the GNU General Public License
+//    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <math.h>
+#include <stdlib.h>
+#include "ros/ros.h"
+#include <std_msgs/String.h>
+#include <ros/package.h>
+#include "std_msgs/Float32.h"
+
+#include "d_fall_pps/CrazyflieData.h"
+#include "d_fall_pps/Setpoint.h"
+#include "d_fall_pps/ControlCommand.h"
+#include "d_fall_pps/Controller.h"
+
+#define PI 3.1415926535
+#define RATE_CONTROLLER 0
+
+using namespace d_fall_pps;
+
+std::vector<float>  ffThrust(4);
+std::vector<float>  feedforwardMotor(4);
+std::vector<float>  motorPoly(3);
+
+std::vector<float>  gainMatrixRoll(9);
+std::vector<float>  gainMatrixPitch(9);
+std::vector<float>  gainMatrixYaw(9);
+std::vector<float>  gainMatrixThrust(9);
+
+//K_infinite of feedback
+std::vector<float> filterGain(6);
+//only for velocity calculation
+std::vector<float> estimatorMatrix(2);
+float prevEstimate[9];
+
+std::vector<float>  setpoint(4);
+float saturationThrust;
+
+CrazyflieData previousLocation;
+
+void loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length) {
+    if(!nodeHandle.getParam(name, val)){
+        ROS_ERROR_STREAM("missing parameter '" << name << "'");
+    }
+    if(val.size() != length) {
+        ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
+    }
+}
+
+void loadParameters(ros::NodeHandle& nodeHandle) {
+    loadParameterFloatVector(nodeHandle, "feedforwardMotor", feedforwardMotor, 4);
+    loadParameterFloatVector(nodeHandle, "motorPoly", motorPoly, 3);
+
+    for(int i = 0; i < 4; ++i) {
+        ffThrust[i] = motorPoly[2] * feedforwardMotor[i] * feedforwardMotor[i] + motorPoly[1] * feedforwardMotor[i] + motorPoly[0];
+    }
+    saturationThrust = motorPoly[2] * 12000 * 12000 + motorPoly[1] * 12000 + motorPoly[0];
+
+    loadParameterFloatVector(nodeHandle, "gainMatrixRoll", gainMatrixRoll, 9);
+    loadParameterFloatVector(nodeHandle, "gainMatrixPitch", gainMatrixPitch, 9);
+    loadParameterFloatVector(nodeHandle, "gainMatrixYaw", gainMatrixYaw, 9);
+    loadParameterFloatVector(nodeHandle, "gainMatrixThrust", gainMatrixThrust, 9);
+
+    loadParameterFloatVector(nodeHandle, "filterGain", filterGain, 6);
+    loadParameterFloatVector(nodeHandle, "estimatorMatrix", estimatorMatrix, 2);
+
+    loadParameterFloatVector(nodeHandle, "defaultSetpoint", setpoint, 4);
+}
+
+float computeMotorPolyBackward(float thrust) {
+    return (-motorPoly[1] + sqrt(motorPoly[1] * motorPoly[1] - 4 * motorPoly[2] * (motorPoly[0] - thrust))) / (2 * motorPoly[2]);
+}
+
+
+//Kalman
+void estimateState(Controller::Request &request, float (&est)[9]) {
+    // attitude
+    est[6] = request.ownCrazyflie.roll;
+    est[7] = request.ownCrazyflie.pitch;
+    est[8] = request.ownCrazyflie.yaw;
+
+    //velocity & filtering
+    float ahat_x[6]; //estimator matrix times state (x, y, z, vx, vy, vz)
+    ahat_x[0] = 0; ahat_x[1]=0; ahat_x[2]=0;
+    ahat_x[3] = estimatorMatrix[0] * prevEstimate[0] + estimatorMatrix[1] * prevEstimate[3];
+    ahat_x[4] = estimatorMatrix[0] * prevEstimate[1] + estimatorMatrix[1] * prevEstimate[4];
+    ahat_x[5] = estimatorMatrix[0] * prevEstimate[2] + estimatorMatrix[1] * prevEstimate[5];
+
+    
+    float k_x[6]; //filterGain times state
+    k_x[0] = request.ownCrazyflie.x * filterGain[0];
+    k_x[1] = request.ownCrazyflie.y * filterGain[1];
+    k_x[2] = request.ownCrazyflie.z * filterGain[2];
+    k_x[3] = request.ownCrazyflie.x * filterGain[3];
+    k_x[4] = request.ownCrazyflie.y * filterGain[4];
+    k_x[5] = request.ownCrazyflie.z * filterGain[5];
+   
+    est[0] = ahat_x[0] + k_x[0];
+    est[1] = ahat_x[1] + k_x[1];
+	est[2] = ahat_x[2] + k_x[2];
+    est[3] = ahat_x[3] + k_x[3];
+    est[4] = ahat_x[4] + k_x[4];
+    est[5] = ahat_x[5] + k_x[5];
+
+    memcpy(prevEstimate, est, 9 * sizeof(float));
+    
+}
+
+
+void convertIntoBodyFrame(float est[9], float (&state)[9], float yaw_measured) {
+	float sinYaw = sin(yaw_measured);
+    float cosYaw = cos(yaw_measured);
+
+    state[0] = est[0] * cosYaw + est[1] * sinYaw;
+    state[1] = -est[0] * sinYaw + est[1] * cosYaw;
+    state[2] = est[2];
+
+    state[3] = est[3] * cosYaw + est[4] * sinYaw;
+    state[4] = -est[3] * sinYaw + est[4] * cosYaw;
+    state[5] = est[5];
+
+    state[6] = est[6];
+    state[7] = est[7];
+    state[8] = est[8];
+}
+
+bool calculateControlOutput(Controller::Request &request, Controller::Response &response) {
+    CrazyflieData vicon = request.ownCrazyflie;
+	
+	float yaw_measured = request.ownCrazyflie.yaw;
+
+    //move coordinate system to make setpoint origin
+    request.ownCrazyflie.x -= setpoint[0];
+    request.ownCrazyflie.y -= setpoint[1];
+    request.ownCrazyflie.z -= setpoint[2];
+    float yaw = request.ownCrazyflie.yaw - setpoint[3];
+
+    while(yaw > PI) {yaw -= 2 * PI;}
+    while(yaw < -PI) {yaw += 2 * PI;}
+    request.ownCrazyflie.yaw = yaw;
+
+    float est[9]; //px, py, pz, vx, vy, vz, roll, pitch, yaw
+    estimateState(request, est);
+	
+    float state[9]; //px, py, pz, vx, vy, vz, roll, pitch, yaw
+    convertIntoBodyFrame(est, state, yaw_measured);
+
+    //calculate feedback
+    float outRoll = 0;
+    float outPitch = 0;
+    float outYaw = 0;
+    float thrustIntermediate = 0;
+    for(int i = 0; i < 9; ++i) {
+    	outRoll -= gainMatrixRoll[i] * state[i];
+    	outPitch -= gainMatrixPitch[i] * state[i];
+    	outYaw -= gainMatrixYaw[i] * state[i];
+    	thrustIntermediate -= gainMatrixThrust[i] * state[i];
+    }
+
+    //INFORMATION: this ugly fix was needed for the older firmware
+    //outYaw *= 0.5;
+
+    response.controlOutput.roll = outRoll;
+    response.controlOutput.pitch = outPitch;
+    response.controlOutput.yaw = outYaw;
+
+    if(thrustIntermediate > saturationThrust)
+        thrustIntermediate = saturationThrust;
+    else if(thrustIntermediate < -saturationThrust)
+        thrustIntermediate = -saturationThrust;
+
+    response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustIntermediate + ffThrust[0]);
+    response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustIntermediate + ffThrust[1]);
+    response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustIntermediate + ffThrust[2]);
+    response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustIntermediate + ffThrust[3]);
+
+    response.controlOutput.onboardControllerType = RATE_CONTROLLER;
+
+    previousLocation = request.ownCrazyflie;
+   
+	return true;
+}
+
+void followCallback(const Setpoint& newSetpoint) {
+    setpoint[0] = newSetpoint.x-0.1;
+    setpoint[1] = newSetpoint.y-0.1;
+    setpoint[2] = newSetpoint.z;
+    setpoint[3] = newSetpoint.yaw;
+}
+
+
+int main(int argc, char* argv[]) {
+    ros::init(argc, argv, "FollowControllerService");
+
+    ros::NodeHandle nodeHandle("~");
+    loadParameters(nodeHandle);
+
+    ros::Subscriber followSubscriber = nodeHandle.subscribe("/3/CircleControllerService/FollowTopic", 1, followCallback);
+
+    ros::ServiceServer service = nodeHandle.advertiseService("FollowController", calculateControlOutput);
+    ROS_INFO("FollowCrazyflieService ready");
+
+    ros::spin();
+
+    return 0;
+}