From da2c6edb066173fb8b1df483396063a556943749 Mon Sep 17 00:00:00 2001
From: Paul Beuchat <beuchatp@control.ee.ethz.ch>
Date: Sun, 7 Jul 2019 01:07:07 +0200
Subject: [PATCH] Added Mocap Emulator and complete the Quadcopter Simulator
class. Tested and working on a single laptop. Next step to test on a network
of laptops
---
dfall_ws/src/dfall_pkg/CMakeLists.txt | 9 +
.../GUI_Qt/systemConfigGUI/include/crazyFly.h | 4 +-
.../GUI_Qt/systemConfigGUI/src/crazyFly.cpp | 23 +
.../systemConfigGUI/src/mainguiwindow.cpp | 5 +-
.../include/classes/QuadrotorSimulator.h | 242 +++++++++
.../include/nodes/CentralManagerService.h | 2 +
.../src/dfall_pkg/include/nodes/Constants.h | 3 +
.../dfall_pkg/include/nodes/MocapEmulator.h | 164 ++++++
.../include/nodes/TemplateControllerService.h | 1 +
.../include/nodes/TuningControllerService.h | 19 +-
dfall_ws/src/dfall_pkg/launch/teacher.launch | 34 +-
.../dfall_pkg/param/DefaultController.yaml | 2 +-
.../dfall_pkg/param/MocapEmulatorConfig.yaml | 10 +
.../src/classes/QuadrotorSimulator.cpp | 482 ++++++++++++++++++
.../src/nodes/CentralManagerService.cpp | 20 +-
.../src/nodes/DefaultControllerService.cpp | 2 +-
.../src/dfall_pkg/src/nodes/MocapEmulator.cpp | 369 ++++++++++++++
.../src/nodes/PickerControllerService.cpp | 4 +-
.../src/nodes/RemoteControllerService.cpp | 2 +-
.../src/nodes/TemplateControllerService.cpp | 60 ++-
.../src/nodes/TuningControllerService.cpp | 66 ++-
.../dfall_pkg/srv/CMQueryCrazyflieName.srv | 5 +
22 files changed, 1449 insertions(+), 79 deletions(-)
create mode 100644 dfall_ws/src/dfall_pkg/include/classes/QuadrotorSimulator.h
create mode 100644 dfall_ws/src/dfall_pkg/include/nodes/MocapEmulator.h
create mode 100644 dfall_ws/src/dfall_pkg/param/MocapEmulatorConfig.yaml
create mode 100644 dfall_ws/src/dfall_pkg/src/classes/QuadrotorSimulator.cpp
create mode 100644 dfall_ws/src/dfall_pkg/src/nodes/MocapEmulator.cpp
create mode 100755 dfall_ws/src/dfall_pkg/srv/CMQueryCrazyflieName.srv
diff --git a/dfall_ws/src/dfall_pkg/CMakeLists.txt b/dfall_ws/src/dfall_pkg/CMakeLists.txt
index da748325..c8aa8c4b 100755
--- a/dfall_ws/src/dfall_pkg/CMakeLists.txt
+++ b/dfall_ws/src/dfall_pkg/CMakeLists.txt
@@ -273,6 +273,7 @@ add_service_files(
Controller.srv
CMRead.srv
CMQuery.srv
+ CMQueryCrazyflieName.srv
CMUpdate.srv
CMCommand.srv
LoadYamlFromFilename.srv
@@ -410,6 +411,10 @@ add_executable(TestMotorsControllerService src/nodes/TestMotorsControllerService
add_executable(CentralManagerService src/nodes/CentralManagerService.cpp src/CrazyflieIO.cpp)
add_executable(ParameterService src/nodes/ParameterService.cpp)
+add_executable(MocapEmulator src/nodes/MocapEmulator.cpp
+ src/classes/QuadrotorSimulator.cpp
+ src/classes/GetParamtersAndNamespaces.cpp)
+
if(Qt5_FOUND)
@@ -490,6 +495,8 @@ add_dependencies(TestMotorsControllerService dfall_pkg_generate_messages_cpp ${c
add_dependencies(CentralManagerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(ParameterService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(MocapEmulator dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+
if(Qt5_FOUND)
@@ -545,6 +552,8 @@ target_link_libraries(TestMotorsControllerService ${catkin_LIBRARIES})
target_link_libraries(CentralManagerService ${catkin_LIBRARIES})
target_link_libraries(ParameterService ${catkin_LIBRARIES})
+target_link_libraries(MocapEmulator ${catkin_LIBRARIES})
+
if(Qt5_FOUND)
# GUI -- link libraries
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/crazyFly.h b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/crazyFly.h
index fe7ec9b6..d26b198c 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/crazyFly.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/crazyFly.h
@@ -94,7 +94,7 @@ private:
std::string m_name;
qreal m_x;
qreal m_y;
- qreal m_z;
+ qreal m_z = 0.0;
qreal m_roll;
qreal m_pitch;
@@ -102,6 +102,8 @@ private:
bool m_occluded;
+ qreal m_baseline_scale = 1.0;
+
// info for plotting CF
qreal m_width;
qreal m_height;
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/crazyFly.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/crazyFly.cpp
index 8d40b6eb..5bea8cf6 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/crazyFly.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/crazyFly.cpp
@@ -62,6 +62,18 @@ void crazyFly::setAddedToScene(bool added)
void crazyFly::setScaleCFs(double scale)
{
+ // Update the class variable for the baseline scale
+ m_baseline_scale = scale;
+
+ // Determine scaling based on height
+ float temp_z_scale = 0.75*m_z + 0.7;
+ if (temp_z_scale < 0.1)
+ temp_z_scale = 0.1;
+ else if (temp_z_scale > 2.0)
+ temp_z_scale = 2.0;
+ float temp_cf_scale = temp_z_scale * m_baseline_scale;
+
+ // Apply the scaling
this->setScale(scale);
}
@@ -89,6 +101,17 @@ void crazyFly::updateCF(const CrazyflieData* p_crazyfly_msg)
this->setPos(m_x * FROM_METERS_TO_UNITS, -m_y * FROM_METERS_TO_UNITS); // - y because of coordinates
this->setRotation(- m_yaw * RAD2DEG); //negative beacause anti-clock wise should be positive
+
+ // Determine scaling based on height
+ float temp_z_scale = 0.75*m_z + 0.7;
+ if (temp_z_scale < 0.1)
+ temp_z_scale = 0.1;
+ else if (temp_z_scale > 2.0)
+ temp_z_scale = 2.0;
+ float temp_cf_scale = temp_z_scale * m_baseline_scale;
+
+ // Apply the scaling
+ this->setScale(temp_cf_scale);
}
}
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/mainguiwindow.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/mainguiwindow.cpp
index b17fab5a..f624a864 100755
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/mainguiwindow.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/mainguiwindow.cpp
@@ -432,8 +432,11 @@ void MainGUIWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to
filename.append("unk.svg");
}
+ // Determine scaling based on height
+ float temp_cf_scale = (ui->scaleSpinBox->value());
+
crazyFly* tmp_p_crazyfly = new crazyFly(&(p_msg->crazyflies[i]), filename);
- tmp_p_crazyfly->setScaleCFs(ui->scaleSpinBox->value());
+ tmp_p_crazyfly->setScaleCFs(temp_cf_scale);
crazyflies_vector.push_back(tmp_p_crazyfly);
}
diff --git a/dfall_ws/src/dfall_pkg/include/classes/QuadrotorSimulator.h b/dfall_ws/src/dfall_pkg/include/classes/QuadrotorSimulator.h
new file mode 100644
index 00000000..b520e0f0
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/include/classes/QuadrotorSimulator.h
@@ -0,0 +1,242 @@
+// 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:
+// The service that manages the loading of YAML parameters
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+#ifndef QUADROTORSIMULATOR_H
+#define QUADROTORSIMULATOR_H
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+#include <stdlib.h>
+#include <iostream>
+#include <string>
+#include <random>
+
+#include <ros/ros.h>
+#include <ros/package.h>
+#include <ros/network.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/ControlCommand.h"
+
+// Include the shared definitions
+#include "nodes/Constants.h"
+
+// SPECIFY THE PACKAGE NAMESPACE
+//using namespace dfall_pkg;
+using namespace std;
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+
+
+
+
+class QuadrotorSimulator
+{
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+public:
+
+ // Default Constructor
+ QuadrotorSimulator ( std::string id );
+
+ // Paramterised Constructor
+ QuadrotorSimulator ( std::string id , float mass );
+
+
+
+public:
+
+ // Identifier String
+ std::string m_id_string = "none";
+
+ // Agent ID which command this quadrotor
+ int m_commanding_agent_id = -1;
+
+ // Mass of the quadrotor [kilograms]
+ float m_mass_in_kg = 0.032;
+
+ // The current state
+ std::vector<float> m_position = {0.0,0.0,0.0};
+ std::vector<float> m_velocity = {0.0,0.0,0.0};
+ std::vector<float> m_euler_angles = {0.0,0.0,0.0};
+ std::vector<float> m_euler_velocities = {0.0,0.0,0.0};
+
+
+
+private:
+
+ // The flying state
+ bool m_isFlying = false;
+
+ // The flying state of the commanding agent
+ int m_flying_state_of_commanding_agent = STATE_MOTORS_OFF;
+
+ // Subscribe for the radio commands of the commanding agent
+ ros::Subscriber controlCommandsSubscriber;
+
+ // Subscribe for the flying state updates of the
+ // commanding agent
+ ros::Subscriber flyingStateUpdatesOfCommandingAgentSubscriber;
+
+ // Current control commands
+ float m_current_command_total_thurst = 0.0;
+ float m_current_command_body_rate_x = 0.0;
+ float m_current_command_body_rate_y = 0.0;
+ float m_current_command_body_rate_z = 0.0;
+
+ // The reset state
+ float m_reset_position_x = 0.0;
+ float m_reset_position_y = 0.0;
+ float m_reset_position_z = 0.0;
+ float m_reset_euler_angle_yaw = 0.0;
+
+ // THE COEFFICIENT OF THE 16-BIT COMMAND TO THRUST CONVERSION
+ std::vector<float> m_motorPoly = {5.484560e-4, 1.032633e-6, 2.130295e-11};
+
+ // THE MIN AND MAX FOR SATURATING THE 16-BIT THRUST COMMANDS
+ float m_command_sixteenbit_min = 1000.0;
+ float m_command_sixteenbit_max = 65000.0;
+
+ // RANDOM NUMBER GENERATORS
+ std::mt19937 m_gen_thrust;
+ std::uniform_real_distribution<float> m_dist_thrust;
+
+ std::mt19937 m_gen_body_x;
+ std::uniform_real_distribution<float> m_dist_body_x;
+
+ std::mt19937 m_gen_body_y;
+ std::uniform_real_distribution<float> m_dist_body_y;
+
+ std::mt19937 m_gen_body_z;
+ std::uniform_real_distribution<float> m_dist_body_z;
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+
+public:
+
+ // Function to simulate the quadrotor for one time step
+ void simulate_for_one_time_step(float deltaT);
+
+ // Function to reset the quadrotor
+ void reset();
+
+ // Function to update the commanding agent id
+ void update_commanding_agent_id( int new_commanding_agent_id );
+
+ // Function to get the flying state
+ bool getIsFlying();
+
+ // Function to set the reset state
+ void setResetState_xyz_yaw(float x, float y, float z, float yaw);
+
+ // Function to set the parameters for the
+ // 16-bit command to thrust conversion
+ void setParameters_for_16bitCommand_to_thrust_conversion(float polyCoeff0, float polyCoeff1, float polyCoeff2, float cmd_min, float cmd_max);
+
+ // Function to print the details of this quadrotor
+ void print_details();
+
+
+private:
+
+ // Callback for receiving control commands
+ void control_commands_callback( const dfall_pkg::ControlCommand & msg );
+
+ // Function to update the current control command
+ void update_current_control_command_rate( const dfall_pkg::ControlCommand & msg );
+
+ // Callback for receiving flying state updates
+ void flying_state_update_of_commanding_agent_callback( const std_msgs::Int32& msg );
+
+ // Function to unsubscribe from the commanding agent
+ void unsubscribe_from_commanding_agent();
+
+ // Function to subscribe to the commanding agent
+ void subscribe_to_commanding_agent_id( int commanding_agent_id );
+
+ // Function to convert 16-bit motor command to Newtons
+ float convert_16_bit_motor_command_to_newtons( int motor_command );
+
+};
+
+#endif // QUADROTORSIMULATOR_H
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/CentralManagerService.h b/dfall_ws/src/dfall_pkg/include/nodes/CentralManagerService.h
index 5079edfe..8d6f536e 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/CentralManagerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/CentralManagerService.h
@@ -56,6 +56,7 @@
// Include the DFALL service types
#include "dfall_pkg/CMRead.h"
#include "dfall_pkg/CMQuery.h"
+#include "dfall_pkg/CMQueryCrazyflieName.h"
#include "dfall_pkg/CMUpdate.h"
#include "dfall_pkg/CMCommand.h"
@@ -122,6 +123,7 @@ ros::Publisher m_databaseChangedPublisher;
bool cmRead(CMRead::Request &request, CMRead::Response &response);
int findEntryByStudID(unsigned int studID);
bool cmQuery(CMQuery::Request &request, CMQuery::Response &response);
+bool cmQueryCrazyflieName(CMQueryCrazyflieName::Request &request, CMQueryCrazyflieName::Response &response);
int findEntryByCF(string name);
bool cmUpdate(CMUpdate::Request &request, CMUpdate::Response &response);
bool cmCommand(CMCommand::Request &request, CMCommand::Response &response);
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/Constants.h b/dfall_ws/src/dfall_pkg/include/nodes/Constants.h
index 707472a5..f1114564 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/Constants.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/Constants.h
@@ -49,6 +49,9 @@
// PI
#define PI 3.141592653589
+// GRAVITY
+#define GRAVITY 9.81
+
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/MocapEmulator.h b/dfall_ws/src/dfall_pkg/include/nodes/MocapEmulator.h
new file mode 100644
index 00000000..ec94b908
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/include/nodes/MocapEmulator.h
@@ -0,0 +1,164 @@
+// 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:
+// Emulator for the Motion Capture data, and simulates a fleet of quadrotor
+// to prouce the emulated data
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+#include "ros/ros.h"
+#include <stdlib.h>
+#include <std_msgs/String.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/ViconData.h"
+#include "dfall_pkg/CrazyflieData.h"
+#include "dfall_pkg/ControlCommand.h"
+#include "dfall_pkg/CrazyflieContext.h"
+#include "dfall_pkg/AreaBounds.h"
+#include "dfall_pkg/Setpoint.h"
+
+// Include the DFALL service types
+#include "dfall_pkg/CMQuery.h"
+#include "dfall_pkg/CMQueryCrazyflieName.h"
+//#include "dfall_pkg/IntIntService.h"
+
+// Include the shared definitions
+#include "nodes/Constants.h"
+
+// Include other classes
+#include "classes/GetParamtersAndNamespaces.h"
+#include "classes/QuadrotorSimulator.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
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+
+// TIMER FOR THE MOTION CAPTURE PUBLISHING
+ros::Timer m_timer_mocap_publisher;
+
+// FREQUENCY OF THE MOTION CAPTURE EMULATOR [Hertz]
+float yaml_mocap_frequency = 200.0;
+float yaml_mocap_deltaT_in_seconds = 0.005;
+
+// THE COEFFICIENT OF THE 16-BIT COMMAND TO THRUST CONVERSION
+std::vector<float> yaml_motorPoly = {5.484560e-4, 1.032633e-6, 2.130295e-11};
+
+// THE MIN AND MAX FOR SATURATING THE 16-BIT THRUST COMMANDS
+float yaml_command_sixteenbit_min = 1000;
+float yaml_command_sixteenbit_max = 65000;
+
+// A VECTOR OF QUADROTORS
+std::vector<QuadrotorSimulator> m_quadrotor_fleet;
+
+// PUBLISHER FOR THE MOTION CAPTURE DATA
+ros::Publisher m_mocapDataPublisher;
+
+// SERVICE CLIENT FOR THE CENTRAL MANAGER
+ros::ServiceClient m_centralManagerService;
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+// CALLBACK FOR EVERYTIME MOCAP DATA SHOULD BE PUBLISHED
+void timerCallback_mocap_publisher(const ros::TimerEvent&);
+
+// 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 each quadrotor simulator
+void loadContextForEachQuadrotor();
+
+// FOR LOADING THE YAML PARAMETERS
+void fetchMocapEmulatorConfigYamlParameters();
+
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/TemplateControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/TemplateControllerService.h
index d7e69289..2e77299a 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/TemplateControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/TemplateControllerService.h
@@ -233,5 +233,6 @@ bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpoin
void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived);
// FOR LOADING THE YAML PARAMETERS
+void timerCallback_initial_load_yaml(const ros::TimerEvent&);
void isReadyTemplateControllerYamlCallback(const IntWithHeader & msg);
void fetchTemplateControllerYamlParameters(ros::NodeHandle& nodeHandle);
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/TuningControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/TuningControllerService.h
index d6eed836..6172fb86 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/TuningControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/TuningControllerService.h
@@ -526,23 +526,8 @@ void setNewSetpoint(float x, float y, float z, float yaw);
void requestGainChangeCallback(const FloatWithHeader& newGain);
-
-// LOAD PARAMETERS
-// float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
-// void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
-// int getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
-// void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length);
-// int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val);
-// bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name);
-// std::string getParameterString(ros::NodeHandle& nodeHandle, std::string name);
-
-
-
-//void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
-//void fetchYamlParameters(ros::NodeHandle& nodeHandle);
-//void processFetchedParameters();
-
-
+// FOR LOADING THE YAML PARAMETERS
+void timerCallback_initial_load_yaml(const ros::TimerEvent&);
void isReadyTuningControllerYamlCallback(const IntWithHeader & msg);
void fetchTuningControllerYamlParameters(ros::NodeHandle& nodeHandle);
diff --git a/dfall_ws/src/dfall_pkg/launch/teacher.launch b/dfall_ws/src/dfall_pkg/launch/teacher.launch
index a9235436..a204b952 100755
--- a/dfall_ws/src/dfall_pkg/launch/teacher.launch
+++ b/dfall_ws/src/dfall_pkg/launch/teacher.launch
@@ -1,5 +1,11 @@
<launch>
+ <!-- INPUT ARGUMENT FOR EMULATING THE MOTION CAPTURE -->
+ <arg name="emulateMocap" default="false" />
+
+ <!-- Example of how to specify the emulateMocap from command line -->
+ <!-- roslaunch dfall_pkg emulateMocap:=true -->
+
<!-- CENTRAL MANAGER -->
<node
pkg="dfall_pkg"
@@ -10,14 +16,26 @@
</node>
<!-- VICON DATA PUBLISHER -->
- <node
- pkg="dfall_pkg"
- name="ViconDataPublisher"
- output="screen"
- type="ViconDataPublisher"
- >
- <rosparam command="load" file="$(find dfall_pkg)/param/ViconConfig.yaml" />
- </node>
+ <group unless="$(arg emulateMocap)">
+ <node
+ pkg="dfall_pkg"
+ name="ViconDataPublisher"
+ output="screen"
+ type="ViconDataPublisher"
+ >
+ <rosparam command="load" file="$(find dfall_pkg)/param/ViconConfig.yaml" />
+ </node>
+ </group>
+ <group if="$(arg emulateMocap)">
+ <node
+ pkg="dfall_pkg"
+ name="ViconDataPublisher"
+ output="screen"
+ type="MocapEmulator"
+ >
+ <rosparam command="load" file="$(find dfall_pkg)/param/MocapEmulatorConfig.yaml" />
+ </node>
+ </group>
<!-- TEACHER GUI -->
<node
diff --git a/dfall_ws/src/dfall_pkg/param/DefaultController.yaml b/dfall_ws/src/dfall_pkg/param/DefaultController.yaml
index 52c150ef..2ae87165 100644
--- a/dfall_ws/src/dfall_pkg/param/DefaultController.yaml
+++ b/dfall_ws/src/dfall_pkg/param/DefaultController.yaml
@@ -24,7 +24,7 @@ threshold_roll_pitch_for_turn_off_degrees: 70
# The thrust for take off spin motors
takeoff_spin_motors_thrust: 8000
# The time for: take off spin motors
-takoff_spin_motors_time: 0.8
+takoff_spin_motors_time: 0.001
# Height change for the take off move-up
takeoff_move_up_start_height: 0.1
diff --git a/dfall_ws/src/dfall_pkg/param/MocapEmulatorConfig.yaml b/dfall_ws/src/dfall_pkg/param/MocapEmulatorConfig.yaml
new file mode 100644
index 00000000..898441b3
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/param/MocapEmulatorConfig.yaml
@@ -0,0 +1,10 @@
+# FREQUENCY OF THE MOTION CAPTURE EMULATOR [Hertz]
+mocap_frequency : 200.0
+
+# QUADRATIC MOTOR REGRESSION EQUATION (a0, a1, a2)
+# > [Newtons] = a2 [cmd]^2 + a1 [cmd] + a0
+motorPoly : [5.484560e-4, 1.032633e-6, 2.130295e-11]
+
+# THE MIN AND MAX FOR SATURATING THE 16-BIT THRUST COMMANDS
+command_sixteenbit_min : 1000
+command_sixteenbit_max : 65000
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/src/classes/QuadrotorSimulator.cpp b/dfall_ws/src/dfall_pkg/src/classes/QuadrotorSimulator.cpp
new file mode 100644
index 00000000..92e45dc9
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/src/classes/QuadrotorSimulator.cpp
@@ -0,0 +1,482 @@
+// 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 class for simulating a single Quadrotor
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// INCLUDE THE HEADER
+#include "classes/QuadrotorSimulator.h"
+
+
+
+
+
+
+
+QuadrotorSimulator::QuadrotorSimulator ( std::string id ) :
+ m_gen_thrust( (std::random_device())() ),
+ m_dist_thrust(-0.02,0.02),
+ m_gen_body_x( (std::random_device())() ),
+ m_dist_body_x(-0.1,0.1),
+ m_gen_body_y( (std::random_device())() ),
+ m_dist_body_y(-0.1,0.1),
+ m_gen_body_z( (std::random_device())() ),
+ m_dist_body_z(-0.1,0.1)
+{
+ // Set the input argument to the ID string
+ this->m_id_string = id;
+ // Set the default mass
+ this->m_mass_in_kg = 0.032;
+}
+
+QuadrotorSimulator::QuadrotorSimulator ( std::string id , float mass ) :
+ m_gen_thrust( (std::random_device())() ),
+ m_dist_thrust(-0.1*mass*GRAVITY,0.1*mass*GRAVITY),
+ m_gen_body_x( (std::random_device())() ),
+ m_dist_body_x(-0.1,0.1),
+ m_gen_body_y( (std::random_device())() ),
+ m_dist_body_y(-0.1,0.1),
+ m_gen_body_z( (std::random_device())() ),
+ m_dist_body_z(-0.1,0.1)
+{
+ // Set the input argument to the ID string
+ this->m_id_string = id;
+ // Set the input argument to the mass
+ this->m_mass_in_kg = mass;
+}
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+//
+// III M M PPPP L EEEEE M M EEEEE N N TTTTT A TTTTT III OOO N N
+// I MM MM P P L E MM MM E NN N T A A T I O O NN N
+// I M M M PPPP L EEE M M M EEE N N N T A A T I O O N N N
+// I M M P L E M M E N NN T AAAAA T I O O N NN
+// III M M P LLLLL EEEEE M M EEEEE N N T A A T III OOO N N
+// ----------------------------------------------------------------------------------
+
+
+// Function to simulate the quadrotor for one time step
+void QuadrotorSimulator::simulate_for_one_time_step(float deltaT)
+{
+ // Only simulate if flying
+ if (this->m_isFlying)
+ {
+ // --------------------------------------- //
+ // PRE-COMPUTATIONS
+
+ // Compute the square of "deltaT"
+ float deltaT2 = deltaT * deltaT;
+
+ // Compute the reciprocal of the mass
+ float mass_recip = 1.0 / this->m_mass_in_kg;
+
+
+ // --------------------------------------- //
+ // ADD RANDOM PERTURBATION TO THE COMMANDS
+ float command_body_rate_x = m_dist_body_x(m_gen_body_x) + this->m_current_command_body_rate_x;
+ float command_body_rate_y = m_dist_body_y(m_gen_body_y) + this->m_current_command_body_rate_y;
+ float command_body_rate_z = m_dist_body_z(m_gen_body_z) + this->m_current_command_body_rate_z;
+
+ float command_total_thrust = m_dist_thrust(m_gen_thrust) + this->m_current_command_total_thurst;
+
+
+
+ // --------------------------------------- //
+ // YAW
+ // Copy the current yaw into a local variable
+ float current_yaw = this->m_euler_angles[2];
+ // Simulate forward yaw
+ float new_yaw = current_yaw + command_body_rate_z * deltaT;
+ // Compute the yaw to be used for linearising
+ float yaw_for_linearisation = 0.5 * (current_yaw+new_yaw);
+ float sin_yaw = sin(yaw_for_linearisation);
+ float cos_yaw = cos(yaw_for_linearisation);
+
+
+
+ // --------------------------------------- //
+ // PITCH
+ // Copy the current pitch into a local variable
+ float current_pitch = this->m_euler_angles[1];
+ // Simulate forward pitch
+ float new_pitch = current_pitch + command_body_rate_y * deltaT;
+ // Compute the pitch to be used for linearising
+ float pitch_for_linearisation = 0.5 * (current_pitch+new_pitch);
+ float sin_pitch = sin(pitch_for_linearisation);
+ float cos_pitch = cos(pitch_for_linearisation);
+
+
+
+ // --------------------------------------- //
+ // ROLL
+ // Copy the current roll into a local variable
+ float current_roll = this->m_euler_angles[0];
+ // Simulate forward roll
+ float new_roll = current_roll + command_body_rate_x * deltaT;
+ // Compute the roll to be used for linearising
+ float roll_for_linearisation = 0.5 * (current_roll+new_roll);
+ float sin_roll = sin(roll_for_linearisation);
+ float cos_roll = cos(roll_for_linearisation);
+
+
+
+ // --------------------------------------- //
+ // VERTICAL HEIGHT
+
+ // Compute vertical component of total thrust,
+ // taking into account gravity
+ float thrust_vertical = command_total_thrust*cos_roll*cos_pitch - this->m_mass_in_kg*GRAVITY;
+
+ // Simulate forward the vertical height
+ float new_z_dot = this->m_velocity[2] + mass_recip*deltaT*thrust_vertical;
+ float new_z = this->m_position[2] + deltaT*this->m_velocity[2] + 0.5*mass_recip*deltaT2*thrust_vertical;
+
+
+
+ // --------------------------------------- //
+ // HORIZONTAL PLANE
+
+ // Rotate the total thrust into INTERTIAL x and y components
+ float thrust_x = ( sin_yaw*sin_roll + cos_yaw*sin_pitch*cos_roll) * command_total_thrust;
+ float thrust_y = (-cos_yaw*sin_roll + sin_yaw*sin_pitch*cos_roll) * command_total_thrust;
+
+ // Simulate forward the INERTIAL x direction
+ float new_x_dot = this->m_velocity[0] + mass_recip*deltaT*thrust_x;
+ float new_x = this->m_position[0] + deltaT*this->m_velocity[0] + 0.5*mass_recip*deltaT2*thrust_x;
+
+ // Simulate forward the INERTIAL y direction
+ float new_y_dot = this->m_velocity[1] + mass_recip*deltaT*thrust_y;
+ float new_y = this->m_position[1] + deltaT*this->m_velocity[1] + 0.5*mass_recip*deltaT2*thrust_y;
+
+
+
+ // --------------------------------------- //
+ // UPDATE STATE WITH THE NEW VALUES
+ // > For the positions
+ this->m_position[0] = new_x;
+ this->m_position[1] = new_y;
+ this->m_position[2] = new_z;
+ // > For the linear velocities
+ this->m_velocity[0] = new_x_dot;
+ this->m_velocity[1] = new_y_dot;
+ this->m_velocity[2] = new_z_dot;
+ // > For the Euler angles
+ this->m_euler_angles[0] = new_roll;
+ this->m_euler_angles[1] = new_pitch;
+ this->m_euler_angles[2] = new_yaw;
+
+
+
+ // --------------------------------------- //
+ // CHECK FOR HIGH PITCH OR ROLL ANGLE
+ if ( (new_roll>(80*DEG2RAD)) || (new_pitch>(80*DEG2RAD)) )
+ {
+ // Reset the state
+ this->reset();
+ }
+
+ } // END OF: "if (this->m_isFlying)"
+}
+
+
+// Function to reset the quadrotor
+void QuadrotorSimulator::reset()
+{
+ // Update the flying state to be not flying
+ this->m_isFlying = false;
+
+ // Set the state back to the default initial state
+ // > For the position
+ this->m_position[0] = this->m_reset_position_x;
+ this->m_position[1] = this->m_reset_position_y;
+ this->m_position[2] = this->m_reset_position_z;
+ // > For the linear velocity
+ this->m_velocity[0] = 0.0;
+ this->m_velocity[1] = 0.0;
+ this->m_velocity[2] = 0.0;
+ // > For the euler angles
+ this->m_euler_angles[0] = 0.0;
+ this->m_euler_angles[1] = 0.0;
+ this->m_euler_angles[2] = this->m_reset_euler_angle_yaw;
+ // > For the euler anglular velocities
+ this->m_euler_velocities[0] = 0.0;
+ this->m_euler_velocities[1] = 0.0;
+ this->m_euler_velocities[2] = 0.0;
+}
+
+// Function to update the commanding agent id
+void QuadrotorSimulator::update_commanding_agent_id( int new_commanding_agent_id )
+{
+ // Inform the user
+ ROS_INFO_STREAM("[QUADROTOR SIMULATOR] Quadrotor \"" << this->m_id_string << "\" received request to connect to commanding agent ID " << new_commanding_agent_id );
+
+ if( new_commanding_agent_id <= 0 )
+ {
+ // Set the id to the non-connected value
+ this->m_commanding_agent_id = -1;
+ // Unsubscribe from the commands
+ this->unsubscribe_from_commanding_agent();
+ // Reset the state
+ this->reset();
+ }
+ else
+ {
+ // Only need to do something if the ID changes
+ if ( new_commanding_agent_id != this->m_commanding_agent_id )
+ {
+ // Set the agent id to the new value
+ this->m_commanding_agent_id = new_commanding_agent_id;
+ // Unsubscribe from the commands
+ this->unsubscribe_from_commanding_agent();
+ // Reset the state
+ this->reset();
+ // Subscribe to commands for the new ID
+ this->subscribe_to_commanding_agent_id( new_commanding_agent_id );
+ }
+ }
+
+}
+
+// Function to get the flying state
+bool QuadrotorSimulator::getIsFlying()
+{
+ return this->m_isFlying;
+}
+
+// Function to set the reset state
+void QuadrotorSimulator::setResetState_xyz_yaw(float x, float y, float z, float yaw)
+{
+ this->m_reset_position_x = x;
+ this->m_reset_position_y = y;
+ this->m_reset_position_z = z;
+ this->m_reset_euler_angle_yaw = yaw;
+}
+
+// Function to set the parameters for the
+// 16-bit command to thrust conversion
+void QuadrotorSimulator::setParameters_for_16bitCommand_to_thrust_conversion(float polyCoeff0, float polyCoeff1, float polyCoeff2, float cmd_min, float cmd_max)
+{
+ // Set the conversion coefficients
+ this->m_motorPoly[0] = polyCoeff0;
+ this->m_motorPoly[0] = polyCoeff1;
+ this->m_motorPoly[0] = polyCoeff2;
+ // Set the min and max
+ this->m_command_sixteenbit_min = cmd_min;
+ this->m_command_sixteenbit_max = cmd_max;
+
+}
+
+// Function to print the details of this quadrotor
+void QuadrotorSimulator::print_details()
+{
+ ROS_INFO_STREAM("Quadrotor with ID: \"" << this->m_id_string << "\", mass = " << this->m_mass_in_kg << " [kg], reset (x,y,z,yaw) = ( " << this->m_reset_position_x << ", " << this->m_reset_position_y << ", " << this->m_reset_position_z << ", " << this->m_reset_euler_angle_yaw << ")");
+}
+
+
+
+
+
+
+
+// PRIVATE FUNCTIONS
+
+
+// Callback for receiving control commands
+void QuadrotorSimulator::control_commands_callback( const dfall_pkg::ControlCommand & msg )
+{
+ // Get the type of the command
+ int command_type = msg.onboardControllerType;
+
+ // Switch based on the command type
+ // Adapt to the new state
+ switch(command_type)
+ {
+ case CF_COMMAND_TYPE_MOTORS:
+ {
+ // Put the command into the class variables
+ this->update_current_control_command_rate(msg);
+ break;
+ }
+ case CF_COMMAND_TYPE_ANGLE:
+ {
+ if (this->m_isFlying)
+ {
+ // Set that the quadrotor is NOT flying
+ this->m_isFlying = false;
+ // Reset the state
+ this->reset();
+ // Inform the user
+ ROS_INFO_STREAM("[QUADROTOR SIMULATOR] Quadrotor \"" << this->m_id_string << "\" stopped simulating and reset becuase command is of type MOTORS or ANGLE");
+ }
+ break;
+ }
+
+ case CF_COMMAND_TYPE_RATE:
+ {
+ // Put the command into the class variables
+ this->update_current_control_command_rate(msg);
+
+ // If the quadrotor is currently flying...
+ //if (this->m_isFlying)
+ //{
+ // // Simulate the quadrotor by one time step
+ //}
+ break;
+ }
+ }
+}
+
+
+// Function to update the current control command
+void QuadrotorSimulator::update_current_control_command_rate( const dfall_pkg::ControlCommand & msg )
+{
+ // Convert each motor command to [Newtons]
+ float cmd1_in_newtons = this->convert_16_bit_motor_command_to_newtons( msg.motorCmd1 );
+ float cmd2_in_newtons = this->convert_16_bit_motor_command_to_newtons( msg.motorCmd2 );
+ float cmd3_in_newtons = this->convert_16_bit_motor_command_to_newtons( msg.motorCmd3 );
+ float cmd4_in_newtons = this->convert_16_bit_motor_command_to_newtons( msg.motorCmd4 );
+
+ // Hence update the total thrust
+ this->m_current_command_total_thurst = cmd1_in_newtons + cmd2_in_newtons + cmd3_in_newtons + cmd4_in_newtons;
+
+ // Update the body rate commands
+ this->m_current_command_body_rate_x = msg.roll;
+ this->m_current_command_body_rate_y = msg.pitch;
+ this->m_current_command_body_rate_z = msg.yaw;
+}
+
+// Callback for receiving flying state updates
+void QuadrotorSimulator::flying_state_update_of_commanding_agent_callback( const std_msgs::Int32& msg )
+{
+ // Inform the user
+ //ROS_INFO_STREAM("[QUADROTOR SIMULATOR] Quadrotor \"" << this->m_id_string << "\" received flying state of " << msg.data << " from the commanding agent" );
+
+ // Update the class variable with the new state
+ this->m_flying_state_of_commanding_agent = msg.data;
+
+ // Adapt to the new state
+ switch(this->m_flying_state_of_commanding_agent)
+ {
+ case STATE_MOTORS_OFF:
+ case STATE_UNAVAILABLE:
+ {
+ // Set that the quadrotor is NOT flying
+ this->m_isFlying = false;
+ // Reset the state
+ this->reset();
+ // Inform the user
+ ROS_INFO_STREAM("[QUADROTOR SIMULATOR] Quadrotor \"" << this->m_id_string << "\" stopped simulating and reset");
+ break;
+ }
+
+ case STATE_TAKE_OFF:
+ case STATE_FLYING:
+ //case STATE_LAND:
+ {
+ if (!(this->m_isFlying))
+ {
+ // Set that the quadrotor is flying
+ this->m_isFlying = true;
+ // Inform the user
+ ROS_INFO_STREAM("[QUADROTOR SIMULATOR] Quadrotor \"" << this->m_id_string << "\" started simulating");
+ }
+ break;
+ }
+ }
+}
+
+
+// Function to unsubscribe from the commanding agent
+void QuadrotorSimulator::unsubscribe_from_commanding_agent()
+{
+ // Shutdown the subscribers
+ controlCommandsSubscriber.shutdown();
+ flyingStateUpdatesOfCommandingAgentSubscriber.shutdown();
+
+ // Inform the user
+ ROS_INFO_STREAM("[QUADROTOR SIMULATOR] Quadrotor \"" << this->m_id_string << "\" is no longer unsubscribed to a commanding agent");
+}
+
+// Function to subscribe to the commanding agent
+void QuadrotorSimulator::subscribe_to_commanding_agent_id( int commanding_agent_id )
+{
+ // Convert the agent ID to a zero padded string
+ std::ostringstream str_stream;
+ str_stream << std::setw(3) << std::setfill('0') << commanding_agent_id;
+ std::string commanding_agent_id_as_string(str_stream.str());
+
+ // Get a node handle
+ // > NOTE that subsciptions below start with a "/",
+ // hence subscribing based on an absolute path
+ ros::NodeHandle nodeHandle("~");
+
+ // Subscribe to the control commands of the
+ // commanding agent
+ controlCommandsSubscriber = nodeHandle.subscribe("/dfall/agent" + commanding_agent_id_as_string + "/CrazyRadio/ControlCommand", 1, &QuadrotorSimulator::control_commands_callback, this );
+
+ // Subscribe to the flying state updates of the
+ // commanding agent
+ flyingStateUpdatesOfCommandingAgentSubscriber = nodeHandle.subscribe("/dfall/agent" + commanding_agent_id_as_string + "/FlyingAgentClient/flyingState", 1, &QuadrotorSimulator::flying_state_update_of_commanding_agent_callback, this );
+
+ // Inform to user
+ ROS_INFO_STREAM("[QUADROTOR SIMULATOR] Quadrotor \"" << this->m_id_string << "\" now subscribed to commands from agent with ID " << commanding_agent_id_as_string);
+}
+
+
+// Function to convert 16-bit motor command to Newtons
+float QuadrotorSimulator::convert_16_bit_motor_command_to_newtons( int motor_command )
+{
+ // Convert the command to a float
+ float cmd = float( motor_command );
+
+ // Saturate it
+ if (cmd < this->m_command_sixteenbit_min)
+ cmd = 0.0;
+ else if (cmd > this->m_command_sixteenbit_max)
+ cmd = m_command_sixteenbit_max;
+
+ // Compute the thrust in [Newtons]
+ float thrust = 0.0;
+ if (cmd > 0.0)
+ thrust = this->m_motorPoly[2] * cmd*cmd + this->m_motorPoly[1] * cmd + this->m_motorPoly[0];
+
+ // Return the result
+ return thrust;
+}
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/CentralManagerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/CentralManagerService.cpp
index 375766ad..4caf6783 100755
--- a/dfall_ws/src/dfall_pkg/src/nodes/CentralManagerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/CentralManagerService.cpp
@@ -99,6 +99,21 @@ bool cmQuery(CMQuery::Request &request, CMQuery::Response &response)
}
}
+bool cmQueryCrazyflieName(CMQueryCrazyflieName::Request &request, CMQueryCrazyflieName::Response &response)
+{
+ int cfIndex = findEntryByCF(request.crazyflieName);
+ if(cfIndex != -1)
+ {
+ response.crazyflieContext = crazyflieDB.crazyflieEntries[cfIndex].crazyflieContext;
+ response.studentID = crazyflieDB.crazyflieEntries[cfIndex].studentID;
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+}
+
int findEntryByCF(string name)
{
for(int i = 0; i < crazyflieDB.crazyflieEntries.size(); i++)
@@ -197,10 +212,13 @@ int main(int argc, char* argv[])
readCrazyflieDB(crazyflieDB);
ros::ServiceServer readService = nodeHandle.advertiseService("Read", cmRead);
- ros::ServiceServer queryService = nodeHandle.advertiseService("Query", cmQuery);
+ ros::ServiceServer queryStudentIDService = nodeHandle.advertiseService("Query", cmQuery);
ros::ServiceServer updateService = nodeHandle.advertiseService("Update", cmUpdate);
ros::ServiceServer commandService = nodeHandle.advertiseService("Command", cmCommand);
+ // A Query Service based on the "Crazyflie Name"
+ ros::ServiceServer queryCrazyflieNameService = nodeHandle.advertiseService("QueryCrazyflieName", cmQueryCrazyflieName);
+
// Publisher for when the database is saved
m_databaseChangedPublisher = nodeHandle.advertise<std_msgs::Int32>("DBChanged", 1);
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
index 8d63aef8..03dcaa42 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
@@ -1957,7 +1957,7 @@ int main(int argc, char* argv[])
// instead use a timer to delay the loading
// Create a single-shot timer
- ros::Timer timer_initial_load_yaml = nodeHandle.createTimer(ros::Duration(3.0), timerCallback_initial_load_yaml, true);
+ ros::Timer timer_initial_load_yaml = nodeHandle.createTimer(ros::Duration(1.5), timerCallback_initial_load_yaml, true);
timer_initial_load_yaml.start();
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/MocapEmulator.cpp b/dfall_ws/src/dfall_pkg/src/nodes/MocapEmulator.cpp
new file mode 100644
index 00000000..0eeda89a
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/src/nodes/MocapEmulator.cpp
@@ -0,0 +1,369 @@
+// Copyright (C) 2019, ETH Zurich, D-ITET, Paul Beuchat
+// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Angel Romero, Cyrill Burgener, Marco Mueller, Philipp Friedli
+//
+// 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:
+// Emulator for the Motion Capture data, and simulates a fleet of quadrotor
+// to prouce the emulated data
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// INCLUDE THE HEADER
+#include "nodes/MocapEmulator.h"
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+//
+// III M M PPPP L EEEEE M M EEEEE N N TTTTT A TTTTT III OOO N N
+// I MM MM P P L E MM MM E NN N T A A T I O O NN N
+// I M M M PPPP L EEE M M M EEE N N N T A A T I O O N N N
+// I M M P L E M M E N NN T AAAAA T I O O N NN
+// III M M P LLLLL EEEEE M M EEEEE N N T A A T III OOO N N
+// ----------------------------------------------------------------------------------
+
+void timerCallback_mocap_publisher(const ros::TimerEvent&)
+{
+ //ROS_INFO("[MOCAP EMULATOR] temp");
+
+ // Initilise a "ViconData" struct
+ // > This is defined in the "ViconData.msg" file
+ ViconData mocapData;
+
+ // Get the number of quadrotors
+ unsigned int quadrotor_count = m_quadrotor_fleet.size();
+
+ // If there are any quadrotors:
+ if (quadrotor_count>0)
+ {
+
+ // Iterate through the vector of quadrotors
+ for(std::vector<QuadrotorSimulator>::iterator it = m_quadrotor_fleet.begin(); it != m_quadrotor_fleet.end(); ++it)
+ {
+ // Access that current element as *it
+
+ // Get the index if needed
+ //int idx = std::distance( m_quadrotor_fleet.begin() , it );
+
+ // Simulate the quadrotor for one time step
+ it->simulate_for_one_time_step( yaml_mocap_deltaT_in_seconds );
+
+ // Local variable for the data of this quadrotor
+ CrazyflieData quadrotor_data;
+
+ // Fill in the details
+ // > For the name
+ quadrotor_data.crazyflieName = it->m_id_string;
+ // > For the occulsion flag
+ quadrotor_data.occluded = false;
+ // > For position
+ quadrotor_data.x = it->m_position[0];
+ quadrotor_data.y = it->m_position[1];
+ quadrotor_data.z = it->m_position[2];
+ // > For euler angles
+ quadrotor_data.roll = it->m_euler_angles[0];
+ quadrotor_data.pitch = it->m_euler_angles[1];
+ quadrotor_data.yaw = it->m_euler_angles[2];
+ // > For the acquiring time
+ quadrotor_data.acquiringTime = 0.0;
+
+ // Push back into the Mocap Data variable
+ mocapData.crazyflies.push_back(quadrotor_data);
+ }
+ }
+
+ // Publish the Motion Capture data
+ m_mocapDataPublisher.publish(mocapData);
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// L OOO A DDDD
+// L O O A A D D
+// L O O A A D D
+// L O O AAAAA D D
+// LLLLL OOO A A DDDD
+//
+// CCCC OOO N N TTTTT EEEEE X X TTTTT
+// C O O NN N T E X X T
+// C O O N N N T EEE X T
+// C O O N NN T E X X T
+// CCCC OOO N N T EEEEE X X T
+// ----------------------------------------------------------------------------------
+
+
+void crazyflieContextDatabaseChangedCallback(const std_msgs::Int32& msg)
+{
+ // Inform the user
+ ROS_INFO("[MOCAP EMULATOR] Received message that the Context Database Changed");
+
+ // Load the context for each quadrotor simulator
+ loadContextForEachQuadrotor();
+}
+
+
+
+void loadContextForEachQuadrotor()
+{
+
+ // Iterate through the quadrotors
+ // Iterate through the vector of quadrotors
+ for(std::vector<QuadrotorSimulator>::iterator it = m_quadrotor_fleet.begin(); it != m_quadrotor_fleet.end(); ++it)
+ {
+ // Access that current element as *it
+
+ // Get the index if needed
+ //int idx = std::distance( m_quadrotor_fleet.begin() , it );
+
+ // Local variable for the service call
+ CMQueryCrazyflieName contextCall;
+
+ // Set the name of this quadrotor
+ contextCall.request.crazyflieName = it->m_id_string;
+
+ // Wait for the service to exist
+ m_centralManagerService.waitForExistence(ros::Duration(0.5));
+
+ // Local variable for the agent ID
+ int new_agent_id = -1;
+
+ // Call the service
+ if (m_centralManagerService.call(contextCall))
+ {
+ // Get the context from the response
+ CrazyflieContext new_context = contextCall.response.crazyflieContext;
+
+ // Get the student ID from the response
+ new_agent_id = contextCall.response.studentID;
+
+ // Update the reset position of the quadrotor simulator
+ // to be the center of the context
+ // > Get the area into a local variable
+ AreaBounds area = new_context.localArea;
+ // > Compute the X-Y coordinates
+ float new_reset_x = (area.xmin + area.xmax) / 2.0;
+ float new_reset_y = (area.ymin + area.ymax) / 2.0;
+ // > Update the reset state
+ it->setResetState_xyz_yaw(new_reset_x,new_reset_y,0.0,0.0);
+
+ // Print out the new context
+ //ROS_INFO_STREAM("[MOCAP EMULATOR] Quadrotor simulator with ID \"" << it->m_id_string << "\" connected to agent ID " << new_agent_id << " in database.");
+ }
+ else
+ {
+ // Let the user know that the "m_id_string" was not found
+ // for this quadrotor simulation
+ ROS_INFO_STREAM("[MOCAP EMULATOR] Quadrotor simulator with ID \"" << it->m_id_string << "\" does not appear in the database.");
+
+ // Set the "new agent id" to reflect this
+ new_agent_id = -1;
+ }
+
+ // Update the agent id of quadrotor simulator
+ it->update_commanding_agent_id( new_agent_id );
+ }
+}
+
+
+
+
+// ----------------------------------------------------------------------------------
+// L OOO A DDDD
+// L O O A A D D
+// L O O A A D D
+// L O O AAAAA D D
+// LLLLL OOO A A DDDD
+//
+// PPPP A RRRR A M M EEEEE TTTTT EEEEE RRRR SSSS
+// P P A A R R A A MM MM E T E R R S
+// PPPP A A RRRR A A M M M EEE T EEE RRRR SSS
+// P AAAAA R R AAAAA M M E T E R R S
+// P A A R R A A M M EEEEE T EEEEE R R SSSS
+// ----------------------------------------------------------------------------------
+
+void fetchMocapEmulatorConfigYamlParameters()
+{
+ // Create a "ros::NodeHandle" type local variable
+ // "nodeHandle_for_paramaters"
+ // as the current node, the "~" indcates that "self"
+ // is the node handle assigned to this variable.
+ ros::NodeHandle nodeHandle_for_paramaters("~");
+
+ // FREQUENCY OF THE MOTION CAPTURE EMULATOR [Hertz]
+ yaml_mocap_frequency = getParameterFloat(nodeHandle_for_paramaters,"mocap_frequency");
+
+ // THE COEFFICIENT OF THE 16-BIT COMMAND TO THRUST CONVERSION
+ getParameterFloatVector(nodeHandle_for_paramaters, "motorPoly", yaml_motorPoly, 3);
+
+ // THE MIN AND MAX FOR SATURATING THE 16-BIT THRUST COMMANDS
+ yaml_command_sixteenbit_min = getParameterFloat(nodeHandle_for_paramaters, "command_sixteenbit_min");
+ yaml_command_sixteenbit_max = getParameterFloat(nodeHandle_for_paramaters, "command_sixteenbit_max");
+
+
+
+ // DEBUGGING: Print out one of the parameters that was loaded
+ ROS_INFO_STREAM("[MOCAP EMULATOR] DEBUGGING: the fetched mocap_frequency = " << yaml_mocap_frequency);
+
+
+
+ // PROCESS THE PARAMTERS
+ // Convert from Hertz to second
+ yaml_mocap_deltaT_in_seconds = 1.0 / yaml_mocap_frequency;
+
+ // DEBUGGING: Print out one of the processed values
+ ROS_INFO_STREAM("[MOCAP EMULATOR] DEBUGGING: after processing yaml_mocap_deltaT_in_seconds = " << yaml_mocap_deltaT_in_seconds);
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// M M A III N N
+// MM MM A A I NN N
+// M M M A A I N N N
+// M M AAAAA I N NN
+// M M A A III N N
+// ----------------------------------------------------------------------------------
+
+int main(int argc, char* argv[])
+{
+ // Starting the ROS-node
+ ros::init(argc, argv, "ViconDataPublisher");
+
+ // Create a "ros::NodeHandle" type local variable "nodeHandle"
+ // as the current node, the "~" indcates that "self" is the
+ // node handle assigned to this variable.
+ ros::NodeHandle nodeHandle("~");
+
+ // Get the namespace of this node
+ std::string m_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[MOCAP EMULATOR] ros::this_node::getNamespace() = " << m_namespace);
+
+
+
+
+ // LOAD THE YAML PARAMETERS
+
+ // Call the class function that loads the parameters
+ // from the "MocapEmulatorConfig.yaml" file.
+ // > This is possible because that YAML file is added
+ // to the parameter service when launched via the
+ // "teacher.launch" file.
+ fetchMocapEmulatorConfigYamlParameters();
+
+
+
+ // ADD QUADROTORS TO THE FLEET
+
+ for ( int i_quad=1 ; i_quad<3 ; i_quad++ )
+ {
+ // Create a string for the ID, zero padded
+ std::ostringstream str_stream;
+ str_stream << std::setw(2) << std::setfill('0') << i_quad;
+ std::string this_quad_id_as_string(str_stream.str());
+ // Create an instance of a quadrotor
+ QuadrotorSimulator quadsim("CF"+this_quad_id_as_string,0.032);
+ // Compute the x-coordinate of the reset state
+ float this_reset_x = float(i_quad-1) * 1.0 + 0.5;
+ // Set the reset state
+ quadsim.setResetState_xyz_yaw(this_reset_x,0.0,0.0,0.0);
+ // Set the parameters for the 16-bit command to thrust conversion
+ quadsim.setParameters_for_16bitCommand_to_thrust_conversion(yaml_motorPoly[0], yaml_motorPoly[1], yaml_motorPoly[2], yaml_command_sixteenbit_min, yaml_command_sixteenbit_max);
+ // Reset the quadrotor
+ quadsim.reset();
+ // Push back into the vector of resets
+ m_quadrotor_fleet.push_back(quadsim);
+ }
+
+ // Display the details of all quadrotors added
+ ROS_INFO_STREAM("[MOCAP EMULATOR] Added " << m_quadrotor_fleet.size() << " quadrotos with the following details:" );
+
+ // Iterate through the vector of quadrotors
+ for(std::vector<QuadrotorSimulator>::iterator it = m_quadrotor_fleet.begin(); it != m_quadrotor_fleet.end(); ++it)
+ {
+ // Access that current element as *it
+
+ // Get the index if needed
+ //int idx = std::distance( m_quadrotor_fleet.begin() , it );
+
+ // Call the function to print out the details
+ it->print_details();
+ //(*it).print_details();
+ }
+
+
+
+ // INITIALISE THE MOTION CAPTURE PUBLISHER TIMER
+ m_timer_mocap_publisher = nodeHandle.createTimer(ros::Duration(yaml_mocap_deltaT_in_seconds), timerCallback_mocap_publisher, false);
+ // > And stop it immediately
+ //m_timer_mocap_timeout_check.stop();
+
+
+
+ // PUBLISHER FOR THE MOTION CAPTURE DATA
+ // Instantiate the class variable "m_mocapDataPublisher" to be a
+ // "ros::Publisher". This variable advertises under the name
+ // "ViconData" and is a message with the structure defined
+ // in the file "ViconData.msg" (located in the "msg" folder).
+ m_mocapDataPublisher = nodeHandle.advertise<ViconData>("ViconData", 1);
+
+
+ // CREATE A NODE HANDLE TO THE ROOT OF THE D-FaLL SYSTEM
+ ros::NodeHandle nodeHandle_dfall_root("/dfall");
+
+
+
+ // SUBSCRIBER FOR DATABASE CHANGES
+ // > This is the database of tuples of the form:
+ // {student ID, flying zone, crazyflie name}
+ m_centralManagerService = nodeHandle_dfall_root.serviceClient<CMQueryCrazyflieName>("CentralManagerService/QueryCrazyflieName", false);
+ ros::Subscriber databaseChangedSubscriber = nodeHandle_dfall_root.subscribe("CentralManagerService/DBChanged", 1, crazyflieContextDatabaseChangedCallback);
+
+
+ // Print out some information to the user.
+ ROS_INFO("[MOCAP EMULATOR] Ready :-)");
+
+ // Enter an endless while loop to keep the node alive.
+ ros::spin();
+
+ // Return zero if the "ross::spin" is cancelled.
+ return 0;
+}
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/PickerControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/PickerControllerService.cpp
index 4a687a4b..f1b3c85a 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/PickerControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/PickerControllerService.cpp
@@ -1112,7 +1112,7 @@ void timerCallback_initial_load_yaml(const ros::TimerEvent&)
else
{
// Inform the user
- ROS_ERROR("[DEFAULT CONTROLLER] The request load yaml file service call failed.");
+ ROS_ERROR("[PICKER CONTROLLER] The request load yaml file service call failed.");
}
}
@@ -1376,7 +1376,7 @@ int main(int argc, char* argv[]) {
// instead use a timer to delay the loading
// Create a single-shot timer
- ros::Timer timer_initial_load_yaml = nodeHandle.createTimer(ros::Duration(4.0), timerCallback_initial_load_yaml, true);
+ ros::Timer timer_initial_load_yaml = nodeHandle.createTimer(ros::Duration(2.0), timerCallback_initial_load_yaml, true);
timer_initial_load_yaml.start();
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/RemoteControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/RemoteControllerService.cpp
index b301d3fc..a2887e62 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/RemoteControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/RemoteControllerService.cpp
@@ -1580,7 +1580,7 @@ int main(int argc, char* argv[]) {
// instead use a timer to delay the loading
// Create a single-shot timer
- ros::Timer timer_initial_load_yaml = nodeHandle.createTimer(ros::Duration(2.5), timerCallback_initial_load_yaml, true);
+ ros::Timer timer_initial_load_yaml = nodeHandle.createTimer(ros::Duration(2.0), timerCallback_initial_load_yaml, true);
timer_initial_load_yaml.start();
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/TemplateControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/TemplateControllerService.cpp
index 71ecce97..99010a99 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/TemplateControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/TemplateControllerService.cpp
@@ -655,6 +655,36 @@ void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived
// ----------------------------------------------------------------------------------
+// TIMER CALLBACK FOR SENDING THE LOAD YAML REQUEST
+void timerCallback_initial_load_yaml(const ros::TimerEvent&)
+{
+ // Create a node handle to the selected parameter service
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ // Create the service client as a local variable
+ ros::ServiceClient requestLoadYamlFilenameServiceClient = nodeHandle_to_own_agent_parameter_service.serviceClient<LoadYamlFromFilename>("requestLoadYamlFilename", false);
+ // Create the service call as a local variable
+ LoadYamlFromFilename loadYamlFromFilenameCall;
+ // Specify the Yaml filename as a string
+ loadYamlFromFilenameCall.request.stringWithHeader.data = "TemplateController";
+ // Set for whom this applies to
+ loadYamlFromFilenameCall.request.stringWithHeader.shouldCheckForAgentID = false;
+ // Wait until the serivce exists
+ requestLoadYamlFilenameServiceClient.waitForExistence(ros::Duration(-1));
+ // Make the service call
+ if(requestLoadYamlFilenameServiceClient.call(loadYamlFromFilenameCall))
+ {
+ // Nothing to do in this case.
+ // The "isReadyDefaultControllerYamlCallback" function
+ // will be called once the YAML file is loaded
+ }
+ else
+ {
+ // Inform the user
+ ROS_ERROR("[TEMPLATE CONTROLLER] The request load yaml file service call failed.");
+ }
+}
+
+
// CALLBACK NOTIFYING THAT THE YAML PARAMETERS ARE READY TO BE LOADED
void isReadyTemplateControllerYamlCallback(const IntWithHeader & msg)
{
@@ -881,32 +911,16 @@ int main(int argc, char* argv[]) {
// service call containing the filename of the *.yaml file,
// and then a message will be received on the above subscribers
// when the paramters are ready.
- // > NOTE IMPORTANTLY that by using a serice client
+ // > NOTE IMPORTANTLY that by using a service client
// we stall the availability of this node until the
// paramter service is ready
+ // > NOTE FURTHER that calling on the service directly from here
+ // often means the yaml file is not actually loaded. So we
+ // instead use a timer to delay the loading
- // Create the service client as a local variable
- ros::ServiceClient requestLoadYamlFilenameServiceClient = nodeHandle_to_own_agent_parameter_service.serviceClient<LoadYamlFromFilename>("requestLoadYamlFilename", false);
- // Create the service call as a local variable
- LoadYamlFromFilename loadYamlFromFilenameCall;
- // Specify the Yaml filename as a string
- loadYamlFromFilenameCall.request.stringWithHeader.data = "TemplateController";
- // Set for whom this applies to
- loadYamlFromFilenameCall.request.stringWithHeader.shouldCheckForAgentID = false;
- // Wait until the serivce exists
- requestLoadYamlFilenameServiceClient.waitForExistence(ros::Duration(-1));
- // Make the service call
- if(requestLoadYamlFilenameServiceClient.call(loadYamlFromFilenameCall))
- {
- // Nothing to do in this case.
- // The "isReadyTemplateControllerYamlCallback" function
- // will be called once the YAML file is loaded
- }
- else
- {
- // Inform the user
- ROS_ERROR("[TEMPLATE CONTROLLER] The request load yaml file service call failed.");
- }
+ // Create a single-shot timer
+ ros::Timer timer_initial_load_yaml = nodeHandle.createTimer(ros::Duration(2.0), timerCallback_initial_load_yaml, true);
+ timer_initial_load_yaml.start();
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/TuningControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/TuningControllerService.cpp
index 072c0be3..f02aad16 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/TuningControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/TuningControllerService.cpp
@@ -1355,6 +1355,36 @@ void requestGainChangeCallback(const FloatWithHeader& newGain)
// ----------------------------------------------------------------------------------
+// TIMER CALLBACK FOR SENDING THE LOAD YAML REQUEST
+void timerCallback_initial_load_yaml(const ros::TimerEvent&)
+{
+ // Create a node handle to the selected parameter service
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ // Create the service client as a local variable
+ ros::ServiceClient requestLoadYamlFilenameServiceClient = nodeHandle_to_own_agent_parameter_service.serviceClient<LoadYamlFromFilename>("requestLoadYamlFilename", false);
+ // Create the service call as a local variable
+ LoadYamlFromFilename loadYamlFromFilenameCall;
+ // Specify the Yaml filename as a string
+ loadYamlFromFilenameCall.request.stringWithHeader.data = "TuningController";
+ // Set for whom this applies to
+ loadYamlFromFilenameCall.request.stringWithHeader.shouldCheckForAgentID = false;
+ // Wait until the serivce exists
+ requestLoadYamlFilenameServiceClient.waitForExistence(ros::Duration(-1));
+ // Make the service call
+ if(requestLoadYamlFilenameServiceClient.call(loadYamlFromFilenameCall))
+ {
+ // Nothing to do in this case.
+ // The "isReadyDefaultControllerYamlCallback" function
+ // will be called once the YAML file is loaded
+ }
+ else
+ {
+ // Inform the user
+ ROS_ERROR("[TUNING CONTROLLER] The request load yaml file service call failed.");
+ }
+}
+
+
// LOADING OF YAML PARAMETERS
// This function does NOT need to be edited
void isReadyTuningControllerYamlCallback(const IntWithHeader & msg)
@@ -1679,34 +1709,24 @@ int main(int argc, char* argv[]) {
// service call containing the filename of the *.yaml file,
// and then a message will be received on the above subscribers
// when the paramters are ready.
- // > NOTE IMPORTANTLY that by using a serice client
+ // > NOTE IMPORTANTLY that by using a service client
// we stall the availability of this node until the
// paramter service is ready
+ // > NOTE FURTHER that calling on the service directly from here
+ // often means the yaml file is not actually loaded. So we
+ // instead use a timer to delay the loading
+
+ // Create a single-shot timer
+ ros::Timer timer_initial_load_yaml = nodeHandle.createTimer(ros::Duration(2.0), timerCallback_initial_load_yaml, true);
+ timer_initial_load_yaml.start();
+
+
- // Create the service client as a local variable
- ros::ServiceClient requestLoadYamlFilenameServiceClient = nodeHandle_to_own_agent_parameter_service.serviceClient<LoadYamlFromFilename>("requestLoadYamlFilename", false);
- // Create the service call as a local variable
- LoadYamlFromFilename loadYamlFromFilenameCall;
- // Specify the Yaml filename as a string
- loadYamlFromFilenameCall.request.stringWithHeader.data = "TuningController";
- // Set for whom this applies to
- loadYamlFromFilenameCall.request.stringWithHeader.shouldCheckForAgentID = false;
- // Wait until the serivce exists
- requestLoadYamlFilenameServiceClient.waitForExistence(ros::Duration(-1));
- // Make the service call
- if(requestLoadYamlFilenameServiceClient.call(loadYamlFromFilenameCall))
- {
- // Nothing to do in this case.
- // The "isReadyTuningControllerYamlCallback" function
- // will be called once the YAML file is loaded
- }
- else
- {
- // Inform the user
- ROS_ERROR("[TUNING CONTROLLER] The request load yaml file service call failed.");
- }
+ // INITIALISE OTHER VARIABLES AS REQUIRED
+ //m_mass_total_in_grams = yaml_mass_cf_in_grams;
+ //m_weight_total_in_newtons = m_mass_total_in_grams * (9.81/1000.0);
diff --git a/dfall_ws/src/dfall_pkg/srv/CMQueryCrazyflieName.srv b/dfall_ws/src/dfall_pkg/srv/CMQueryCrazyflieName.srv
new file mode 100755
index 00000000..85d54384
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/srv/CMQueryCrazyflieName.srv
@@ -0,0 +1,5 @@
+string crazyflieName
+---
+CrazyflieContext crazyflieContext
+uint32 studentID
+
--
GitLab