Merge branch 'pre_fs2019_updates' into 'master'

Merge of pre-Spring 2019 semester updates into the master

See merge request D-FaLL/PandS-System/D-FaLL-System!12

dfall_ws/src/dfall_pkg/CMakeLists.txt

@@ -328,6 +328,8 @@ add_executable(PickerControllerService   src/nodes/PickerControllerService.cpp
                                          src/classes/GetParamtersAndNamespaces.cpp)
 add_executable(TemplateControllerService src/nodes/TemplateControllerService.cpp
                                          src/classes/GetParamtersAndNamespaces.cpp)
+add_executable(TestMotorsControllerService src/nodes/TestMotorsControllerService.cpp
+                                         src/classes/GetParamtersAndNamespaces.cpp)
 add_executable(CentralManagerService     src/nodes/CentralManagerService.cpp src/CrazyflieIO.cpp)
 add_executable(ParameterService          src/nodes/ParameterService.cpp)
 
@@ -403,6 +405,7 @@ add_dependencies(RemoteControllerService   dfall_pkg_generate_messages_cpp ${cat
 add_dependencies(TuningControllerService   dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
 add_dependencies(PickerControllerService   dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
 add_dependencies(TemplateControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(TestMotorsControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
 add_dependencies(CentralManagerService     dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
 add_dependencies(ParameterService          dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
 
@@ -455,6 +458,7 @@ target_link_libraries(RemoteControllerService   ${catkin_LIBRARIES})
 target_link_libraries(TuningControllerService   ${catkin_LIBRARIES})
 target_link_libraries(PickerControllerService   ${catkin_LIBRARIES})
 target_link_libraries(TemplateControllerService ${catkin_LIBRARIES})
+target_link_libraries(TestMotorsControllerService ${catkin_LIBRARIES})
 target_link_libraries(CentralManagerService     ${catkin_LIBRARIES})
 target_link_libraries(ParameterService          ${catkin_LIBRARIES})
 

dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/mainwindow.ui

@@ -215,6 +215,8 @@
     <addaction name="action_showHideController_picker"/>
     <addaction name="action_showHideController_tuning"/>
     <addaction name="action_showHideController_template"/>
+    <addaction name="separator"/>
+    <addaction name="action_testMotors"/>
    </widget>
    <addaction name="menuFile"/>
    <addaction name="menuLoad_YAML"/>
@@ -313,6 +315,11 @@
     <string>Template</string>
    </property>
   </action>
+  <action name="action_testMotors">
+   <property name="text">
+    <string>TestMotors</string>
+   </property>
+  </action>
  </widget>
  <layoutdefault spacing="6" margin="11"/>
  <customwidgets>

dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/Constants_for_Qt_compile.h

@@ -116,6 +116,8 @@
 #define PICKER_CONTROLLER    7
 #define TEMPLATE_CONTROLLER  8
 
+#define TESTMOTORS_CONTROLLER  21
+
 // The constants that "command" changes in the
 // operation state of this agent
 #define CMD_USE_DEFAULT_CONTROLLER      1
@@ -132,6 +134,8 @@
 #define CMD_CRAZYFLY_LAND            12
 #define CMD_CRAZYFLY_MOTORS_OFF      13
 
+#define CMD_USE_TESTMOTORS_CONTROLLER 21
+
 // Flying states
 #define STATE_MOTORS_OFF     1
 #define STATE_TAKE_OFF       2
@@ -208,4 +212,4 @@
 //    ----------------------------------------------------------------------------------
 
 // For standard buttons in the GUI
-#define REQUEST_DEFAULT_SETPOINT_BUTTON_ID    100
\ No newline at end of file
+#define REQUEST_DEFAULT_SETPOINT_BUTTON_ID    100

dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/enablecontrollerloadyamlbar.h

@@ -94,6 +94,8 @@ public:
     void showHideController_tuning_changed();
     void showHideController_template_changed();
 
+    void testMotors_triggered();
+
 
 public slots:
     void setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll);

dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/mainwindow.h

@@ -133,6 +133,8 @@ private slots:
     void on_action_showHideController_tuning_changed();
     void on_action_showHideController_template_changed();
 
+    void on_action_testMotors_triggered();
+
 };
 
 #endif // MAINWINDOW_H

dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/enablecontrollerloadyamlbar.cpp

@@ -116,6 +116,20 @@ void EnableControllerLoadYamlBar::showHideController_template_changed()
 
 
 
+// TEST MOTORS
+
+void EnableControllerLoadYamlBar::testMotors_triggered()
+{
+#ifdef CATKIN_MAKE
+    dfall_pkg::IntWithHeader msg;
+    fillIntMessageHeader(msg);
+    msg.data = CMD_USE_TESTMOTORS_CONTROLLER;
+    this->commandPublisher.publish(msg);
+    ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Enable Test Motors Controller");
+#endif
+}
+
+
 
 
 // ENABLE CONTROLLER BUTTONS ON-CLICK CALLBACK

dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/mainwindow.cpp

@@ -253,6 +253,12 @@ void MainWindow::on_action_showHideController_template_changed()
 }
 
 
+void MainWindow::on_action_testMotors_triggered()
+{
+    // Notify the UI elements of this change
+    ui->customWidget_enableControllerLoadYamlBar->testMotors_triggered();
+}
+
 
 //    ----------------------------------------------------------------------------------
 //    III  DDDD       &&&      TTTTT  Y   Y  PPPP   EEEEE

dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/mainguiwindow.cpp

@@ -410,16 +410,27 @@ void MainGUIWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to
 
             QString filename(":/images/drone_fixed_");
 
-            if(std::regex_search(s, m, e))
-            {
-                std::string found_string = m[1].str();
-                filename.append(QString::fromStdString(found_string));
-                filename.append(".svg");
-            }
-            else
-            {
-                filename.append("unk.svg");
-            }
+			if(std::regex_search(s, m, e))
+			{
+				// Get the string that was found
+				// > it should be a zero-padded 2-digit number
+				std::string found_string = m[1].str();
+				// Convert to an integer
+				int found_string_as_int = QString::fromStdString(found_string).toInt();
+				if ((1 <= found_string_as_int) && (found_string_as_int <= 9))
+				{
+					filename.append(QString::fromStdString(found_string));
+					filename.append(".svg");
+				}
+				else
+				{
+					filename.append("unk.svg");
+				}
+			}
+			else
+			{
+				filename.append("unk.svg");
+			}
 
             crazyFly* tmp_p_crazyfly = new crazyFly(&(p_msg->crazyflies[i]), filename);
             tmp_p_crazyfly->setScaleCFs(ui->scaleSpinBox->value());

dfall_ws/src/dfall_pkg/include/nodes/Constants.h

@@ -129,6 +129,8 @@
 #define PICKER_CONTROLLER    7
 #define TEMPLATE_CONTROLLER  8
 
+#define TESTMOTORS_CONTROLLER  21
+
 // The constants that "command" changes in the
 // operation state of this agent
 #define CMD_USE_DEFAULT_CONTROLLER   1
@@ -145,6 +147,8 @@
 #define CMD_CRAZYFLY_LAND            12
 #define CMD_CRAZYFLY_MOTORS_OFF      13
 
+#define CMD_USE_TESTMOTORS_CONTROLLER 21
+
 // Flying states
 #define STATE_MOTORS_OFF     1
 #define STATE_TAKE_OFF       2

dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerConstants.h

@@ -39,6 +39,14 @@
 #define DEFAULT_CONTROLLER_REQUEST_LANDING     2
 
 
+
+// TO NOTIFY THAT MANOEUVRES ARE COMPLETED
+
+#define DEFAULT_CONTROLLER_TAKEOFF_COMPLETE     1
+#define DEFAULT_CONTROLLER_LANDING_COMPLETE     2
+
+
+
 // TO IDENITFY THE STATE OF THE DEFAULT CONTROLLER
 
 #define DEFAULT_CONTROLLER_STATE_NORMAL        1

dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h

@@ -70,6 +70,7 @@
 #include "dfall_pkg/IntIntService.h"
 #include "dfall_pkg/LoadYamlFromFilename.h"
 #include "dfall_pkg/GetSetpointService.h"
+#include "dfall_pkg/CMQuery.h"
 
 // Include the shared definitions
 #include "nodes/Constants.h"
@@ -369,12 +370,27 @@ ros::Publisher m_debugPublisher;
 // changes to the setpoint
 ros::Publisher m_setpointChangedPublisher;
 
+// ROS Publisher to inform the flying agent client
+// when a requested manoeuvre is complete
+ros::Publisher m_manoeuvreCompletePublisher;
 
 // ROS Publisher for sending motors-off command
 // to the flying agent client
 ros::Publisher m_motorsOffToFlyingAgentClientPublisher;
 
+// Describes the area of the crazyflie and other parameters
+//CrazyflieContext m_context;
 
+// Flag for whether a context is available or not
+bool m_isAvailableContext = false;
+
+// Variable for each coordinate
+float m_symmetric_area_bounds_x = 0.5;
+float m_symmetric_area_bounds_y = 0.5;
+float m_symmetric_area_bounds_z = 0.5;
+
+// Service Client from which the "CrazyflieContext" is loaded
+ros::ServiceClient m_centralManager;
 
 
 
@@ -458,12 +474,21 @@ bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpoin
 // PUBLISH THE CURRENT SETPOINT AND STATE
 void publishCurrentSetpointAndState();
 
+// CLIP TO MIN AND MAX
+float clipToBounds(float val, float val_min, float val_max);
+
 // CUSTOM COMMAND RECEIVED CALLBACK
 void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived);
 
 // PUBLISH MOTORS-OFF MESSAGE TO FLYING AGENT CLIENT
 void publish_motors_off_to_flying_agent_client();
 
+// FUNCTIONS FOR THE CONTEXT OF THIS AGENT
+// > Callback that the context database changed
+void crazyflieContextDatabaseChangedCallback(const std_msgs::Int32& msg);
+// > For loading the context of this agent
+void loadCrazyflieContext();
+
 // LOADING OF YAML PARAMETERS
 void timerCallback_initial_load_yaml(const ros::TimerEvent&);
 void isReadyDefaultControllerYamlCallback(const IntWithHeader & msg);

dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h

@@ -204,6 +204,8 @@ ros::ServiceClient m_pickerController;
 // The Template controller specified in the FlyingAgentClientConfig.yaml
 ros::ServiceClient m_templateController;
 
+// The Test Mtoros controller specified in the FlyingAgentClientConfig.yaml
+ros::ServiceClient m_testMotorsController;
 
 
 
@@ -294,6 +296,8 @@ void requestChangeFlyingStateToLand();
 void takeOffTimerCallback(const ros::TimerEvent&);
 // > Callback that the landing phase is complete
 void landTimerCallback(const ros::TimerEvent&);
+// > Callback that the Default Controller complete a manoeuvre
+void defaultControllerManoeuvreCompleteCallback(const IntWithHeader & msg);
 
 
 

dfall_ws/src/dfall_pkg/include/nodes/TestMotorsControllerService.h

+//    Copyright (C) 2019, ETH Zurich, D-ITET, Paul Beuchat
+//
+//    This file is part of D-FaLL-System.
+//    
+//    D-FaLL-System 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.
+//    
+//    D-FaLL-System 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 D-FaLL-System.  If not, see <http://www.gnu.org/licenses/>.
+//    
+//
+//    ----------------------------------------------------------------------------------
+//    DDDD        FFFFF        L     L           SSSS  Y   Y   SSSS  TTTTT  EEEEE  M   M
+//    D   D       F      aaa   L     L          S       Y Y   S        T    E      MM MM
+//    D   D  ---  FFFF  a   a  L     L     ---   SSS     Y     SSS     T    EEE    M M M
+//    D   D       F     a  aa  L     L              S    Y        S    T    E      M   M
+//    DDDD        F      aa a  LLLL  LLLL       SSSS     Y    SSSS     T    EEEEE  M   M
+//
+//
+//    DESCRIPTION:
+//    A controller to test the motors
+//
+//    ----------------------------------------------------------------------------------
+
+
+
+
+
+//    ----------------------------------------------------------------------------------
+//    III  N   N   CCCC  L      U   U  DDDD   EEEEE   SSSS
+//     I   NN  N  C      L      U   U  D   D  E      S
+//     I   N N N  C      L      U   U  D   D  EEE     SSS
+//     I   N  NN  C      L      U   U  D   D  E          S
+//    III  N   N   CCCC  LLLLL   UUU   DDDD   EEEEE  SSSS
+//    ----------------------------------------------------------------------------------
+
+// These various headers need to be included so that this controller service can be
+// connected with the D-FaLL system.
+
+//some useful libraries
+#include <math.h>
+#include <stdlib.h>
+#include "ros/ros.h"
+#include <ros/package.h>
+
+// Include the standard message types
+#include "std_msgs/Int32.h"
+#include "std_msgs/Float32.h"
+#include <std_msgs/String.h>
+
+// Include the DFALL message types
+#include "dfall_pkg/IntWithHeader.h"
+//#include "dfall_pkg/StringWithHeader.h"
+#include "dfall_pkg/SetpointWithHeader.h"
+#include "dfall_pkg/CustomButtonWithHeader.h"
+#include "dfall_pkg/ViconData.h"
+#include "dfall_pkg/Setpoint.h"
+#include "dfall_pkg/ControlCommand.h"
+#include "dfall_pkg/Controller.h"
+#include "dfall_pkg/DebugMsg.h"
+
+// Include the DFALL service types
+#include "dfall_pkg/LoadYamlFromFilename.h"
+#include "dfall_pkg/GetSetpointService.h"
+
+// Include the shared definitions
+#include "nodes/Constants.h"
+
+// Include other classes
+#include "classes/GetParamtersAndNamespaces.h"
+
+// Need for having a ROS "bag" to store data for post-analysis
+//#include <rosbag/bag.h>
+
+
+
+
+
+// Namespacing the package
+using namespace dfall_pkg;
+
+
+
+
+
+//    ----------------------------------------------------------------------------------
+//    DDDD   EEEEE  FFFFF  III  N   N  EEEEE   SSSS
+//    D   D  E      F       I   NN  N  E      S
+//    D   D  EEE    FFF     I   N N N  EEE     SSS
+//    D   D  E      F       I   N  NN  E          S
+//    DDDD   EEEEE  F      III  N   N  EEEEE  SSSS
+//    ----------------------------------------------------------------------------------
+
+// These constants are defined to make the code more readable and adaptable.
+
+// NOTE: many constants are already defined in the
+//       "Constant.h" header file
+
+
+
+
+
+
+//    ----------------------------------------------------------------------------------
+//    V   V    A    RRRR   III    A    BBBB   L      EEEEE   SSSS
+//    V   V   A A   R   R   I    A A   B   B  L      E      S
+//    V   V  A   A  RRRR    I   A   A  BBBB   L      EEE     SSS
+//     V V   AAAAA  R  R    I   AAAAA  B   B  L      E          S
+//      V    A   A  R   R  III  A   A  BBBB   LLLLL  EEEEE  SSSS
+//    ----------------------------------------------------------------------------------
+
+
+// The ID of the agent that this node is monitoring
+int m_agentID;
+
+// The ID of the agent that can coordinate this node
+int m_coordID;
+
+// NAMESPACES FOR THE PARAMETER SERVICES
+// > For the paramter service of this agent
+std::string m_namespace_to_own_agent_parameter_service;
+// > For the parameter service of the coordinator
+std::string m_namespace_to_coordinator_parameter_service;
+
+
+
+// VARAIBLES FOR VALUES LOADED FROM THE YAML FILE
+// > the mass of the crazyflie, in [grams]
+float yaml_cf_mass_in_grams = 25.0;
+
+// > the frequency at which the controller is running
+float yaml_control_frequency = 200.0;
+
+// > the coefficients of the 16-bit command to thrust conversion
+//std::vector<float> yaml_motorPoly(3);
+std::vector<float> yaml_motorPoly = {5.484560e-4, 1.032633e-6, 2.130295e-11};
+
+
+// The min and max for saturating 16 bit thrust commands
+float yaml_command_sixteenbit_min = 1000;
+float yaml_command_sixteenbit_max = 60000;
+
+// > the default setpoint, the ordering is (x,y,z,yaw),
+//   with units [meters,meters,meters,radians]
+std::vector<float> yaml_default_setpoint = {0.0,0.0,0.4,0.0};
+
+// Boolean indiciating whether the "Debug Message" of this agent should be published or not
+bool yaml_shouldPublishDebugMessage = false;
+
+// Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
+bool yaml_shouldDisplayDebugInfo = false;
+
+// The LQR Controller parameters for "LQR_RATE_MODE"
+std::vector<float> yaml_gainMatrixThrust_NineStateVector  =  { 0.00, 0.00, 0.98, 0.00, 0.00, 0.25, 0.00, 0.00, 0.00};
+std::vector<float> yaml_gainMatrixRollRate                =  { 0.00,-6.20, 0.00, 0.00,-3.00, 0.00, 5.20, 0.00, 0.00};
+std::vector<float> yaml_gainMatrixPitchRate               =  { 6.20, 0.00, 0.00, 3.00, 0.00, 0.00, 0.00, 5.20, 0.00};
+std::vector<float> yaml_gainMatrixYawRate                 =  { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.30};
+
+
+
+// The weight of the Crazyflie in Newtons, i.e., mg
+float m_cf_weight_in_newtons = 0.0;
+
+// The location error of the Crazyflie at the "previous" time step
+float m_previous_stateErrorInertial[9];
+
+// The setpoint to be tracked, the ordering is (x,y,z,yaw),
+// with units [meters,meters,meters,radians]
+std::vector<float>  m_setpoint{0.0,0.0,0.4,0.0};
+
+
+
+
+// ROS Publisher for debugging variables
+ros::Publisher m_debugPublisher;
+
+// ROS Publisher for inform the network about
+// changes to the setpoin
+ros::Publisher m_setpointChangedPublisher;
+
+
+
+
+
+//    ----------------------------------------------------------------------------------
+//    FFFFF  U   U  N   N   CCCC  TTTTT  III   OOO   N   N
+//    F      U   U  NN  N  C        T     I   O   O  NN  N
+//    FFF    U   U  N N N  C        T     I   O   O  N N N
+//    F      U   U  N  NN  C        T     I   O   O  N  NN
+//    F       UUU   N   N   CCCC    T    III   OOO   N   N
+//
+//    PPPP   RRRR    OOO   TTTTT   OOO   TTTTT  Y   Y  PPPP   EEEEE   SSSS
+//    P   P  R   R  O   O    T    O   O    T     Y Y   P   P  E      S
+//    PPPP   RRRR   O   O    T    O   O    T      Y    PPPP   EEE     SSS
+//    P      R  R   O   O    T    O   O    T      Y    P      E          S
+//    P      R   R   OOO     T     OOO     T      Y    P      EEEEE  SSSS
+//    ----------------------------------------------------------------------------------
+
+// These function prototypes are not strictly required for this code to
+// complile, but adding the function prototypes here means the the functions
+// can be written below in any order. If the function prototypes are not
+// included then the function need to written below in an order that ensure
+// each function is implemented before it is called from another function,
+// hence why the "main" function is at the bottom.
+
+// CONTROLLER COMPUTATIONS
+bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
+
+// TRANSFORMATION OF THE (x,y) INERTIAL FRAME ERROR
+// INTO AN (x,y) BODY FRAME ERROR
+void convertIntoBodyFrame(float stateInertial[9], float (&stateBody)[9], float yaw_measured);
+
+// CONVERSION FROM THRUST IN NEWTONS TO 16-BIT COMMAND
+float computeMotorPolyBackward(float thrust);
+
+// REQUEST SETPOINT CHANGE CALLBACK
+void requestSetpointChangeCallback(const SetpointWithHeader& newSetpoint);
+
+// CHANGE SETPOINT FUNCTION
+void setNewSetpoint(float x, float y, float z, float yaw);
+
+// GET CURRENT SETPOINT SERVICE CALLBACK
+bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpointService::Response &response);
+
+// CUSTOM COMMAND RECEIVED CALLBACK
+void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived);
+
+// FOR LOADING THE YAML PARAMETERS
+void isReadyTemplateControllerYamlCallback(const IntWithHeader & msg);
+void fetchTemplateControllerYamlParameters(ros::NodeHandle& nodeHandle);
\ No newline at end of file

dfall_ws/src/dfall_pkg/launch/agent.launch

@@ -119,6 +119,15 @@
 			>
 		</node>
 
+		<!-- TEST MOTORS CONTROLLER -->
+		<node
+			pkg    = "dfall_pkg"
+			name   = "TestMotorsControllerService"
+			output = "screen"
+			type   = "TestMotorsControllerService"
+			>
+		</node>
+
 		<!-- PARAMETER SERVICE -->
 		<node
 			pkg    = "dfall_pkg"

dfall_ws/src/dfall_pkg/param/DefaultController.yaml

@@ -42,7 +42,7 @@ takoff_goto_setpoint_max_time: 4.0
 takoff_integrator_on_box_horizontal: 0.25
 takoff_integrator_on_box_vertical:   0.15
 # The time for: take off integrator-on
-takoff_integrator_on_time: 2.0
+takoff_integrator_on_time: 4.0
 
 
 # Height change for the landing move-down
@@ -99,16 +99,16 @@ gainMatrixThrust_2StateVector     :  [ 0.98, 0.25]
 gainMatrixRollRate_3StateVector   :  [-6.20,-3.00, 5.20]
 gainMatrixPitchRate_3StateVector  :  [ 6.20, 3.00, 5.20]
 # The LQR Controller parameters for mode 2 (Angle-nested)
-gainMatrixRollAngle_2StateVector  :  [-0.20,-0.20]
-gainMatrixPitchAngle_2StateVector :  [ 0.20, 0.20]
+gainMatrixRollAngle_2StateVector  :  [-0.80,-0.50]
+gainMatrixPitchAngle_2StateVector :  [ 0.80, 0.50]
 gainRollRate_fromAngle   :  4.00
 gainPitchRate_fromAngle  :  4.00
 # The LQR Controller parameters for yaw
 gainYawRate_fromAngle    :  2.30
 # Integrator gains
-integratorGain_forThrust  :  0.00
-integratorGain_forTauXY   :  0.00
-integratorGain_forTauYaw  :  0.00
+integratorGain_forThrust  :  0.10
+integratorGain_forTauXY   :  0.06
+integratorGain_forTauYaw  :  0.05
 
 
 

dfall_ws/src/dfall_pkg/param/FlyingAgentClientConfig.yaml

@@ -6,6 +6,8 @@ tuningController:   "TuningControllerService/TuningController"
 pickerController:   "PickerControllerService/PickerController"
 templateController: "TemplateControllerService/TemplateController"
 
+testMotorsController: "TestMotorsControllerService/TestMotorsController"
+
 # The name of the service advertised by the Default
 # Controller for requests manoeuvres to be performed
 defaultController_requestManoeuvre: "DefaultControllerService/RequestManoeuvre"

dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp

@@ -538,12 +538,16 @@ void computeResponse_for_takeoff_integrator_on(Controller::Response &response)
 	if (m_time_in_seconds > yaml_takoff_integrator_on_time)
 	{
 		// Inform the user
-		ROS_INFO("[DEFAULT CONTROLLER] Switch to state: normal");
+		ROS_INFO("[DEFAULT CONTROLLER] Publish message that take-off is complete, and  switch to state: normal");
 		// Reset the time variable
 		m_time_in_seconds = 0.0;
 		// Update the state accordingly
 		m_current_state = DEFAULT_CONTROLLER_STATE_NORMAL;
 		m_current_state_changed = true;
+		// Publish a message that the take-off is complete
+		IntWithHeader msg;
+		msg.data = DEFAULT_CONTROLLER_TAKEOFF_COMPLETE;
+		m_manoeuvreCompletePublisher.publish(msg);
 	}
 }
 
@@ -664,12 +668,16 @@ void computeResponse_for_landing_spin_motors(Controller::Response &response)
 		if ( m_time_in_seconds > (0.7*yaml_landing_spin_motors_time) )
 		{
 			// Inform the user
-			ROS_INFO("[DEFAULT CONTROLLER] Switch to state: standby");
+			ROS_INFO("[DEFAULT CONTROLLER] Publish message that landing is complete, and switch to state: standby");
 			// Reset the time variable
 			m_time_in_seconds = 0.0;