Merge branch 'pps_project' of https://gitlab.ethz.ch/D-FaLL/D-FaLL-System into pps_project

pps_ws/src/d_fall_pps/CMakeLists.txt

@@ -57,7 +57,8 @@ find_package(catkin REQUIRED COMPONENTS
 add_message_files(
   FILES
   ViconData.msg
-  ControlParameters.msg
+  MotorCommands.msg
+  Setpoint.msg
 )
 
 ## Generate services in the 'srv' folder
@@ -66,6 +67,10 @@ add_message_files(
 #   Service1.srv
 #   Service2.srv
 # )
+add_service_files(
+  FILES
+  Controller.srv
+)
 
 ## Generate actions in the 'action' folder
 # add_action_files(
@@ -148,6 +153,7 @@ include_directories(
 # add_executable(${PROJECT_NAME}_node src/d_fall_pps_node.cpp)
 add_executable(ViconDataPublisher src/ViconDataPublisher.cpp)
 add_executable(PPSClient src/PPSClient.cpp)
+add_executable(SafeControllerService src/SafeControllerService.cpp)
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
@@ -171,6 +177,8 @@ target_link_libraries(ViconDataPublisher ${VICON_LIBRARY})
 
 target_link_libraries(PPSClient ${catkin_LIBRARIES})
 
+target_link_libraries(SafeControllerService ${catkin_LIBRARIES})
+
 #############
 ## Install ##
 #############

pps_ws/src/d_fall_pps/msg/ControlParameters.msg

-#copied from Dusan, eventually remove onboardControllerType
-float32 roll
-float32 pitch
-float32 yaw
-uint16 thrust
-uint16 motorCmd1
-uint16 motorCmd2
-uint16 motorCmd3
-uint16 motorCmd4
-uint8 onboardControllerType
-

pps_ws/src/d_fall_pps/msg/MotorCommands.msg

+float32 cmd1
+float32 cmd2
+float32 cmd3
+float32 cmd4
+

pps_ws/src/d_fall_pps/msg/Setpoint.msg

+float64 x
+float64 y
+float64 z
+float64 yaw

pps_ws/src/d_fall_pps/src/SafeControllerService.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 "ros/ros.h"
+#include "d_fall_pps/Controller.h"
+#include "d_fall_pps/ViconData.h"
+#include "d_fall_pps/Setpoint.h"
+#include "d_fall_pps/MotorCommands.h"
+
+bool calculateControlOutput(d_fall_pps::Controller::Request &request, d_fall_pps::Controller::Response &response) {
+    ROS_INFO("calculate control output");
+}
+
+int main(int argc, char* argv[]) {
+    ros::init(argc, argv, "SafeControllerService");
+
+    ros::NodeHandle nodeHandle("~");
+
+    ros::ServiceServer service = nodeHandle.advertiseService("Controller", calculateControlOutput);
+    ROS_INFO("SafeControllerService ready");
+    ros::spin();
+
+    return 0;
+}

pps_ws/src/d_fall_pps/srv/Controller.srv

+ViconData crazyflieLocation
+Setpoint setpoint
+---
+MotorCommands controlOutput